1 // SPDX-License-Identifier: GPL-2.0 2 3 /* 4 * System Control and Management Interface(SCMI) based IIO sensor driver 5 * 6 * Copyright (C) 2021 Google LLC 7 */ 8 9 #include <linux/delay.h> 10 #include <linux/err.h> 11 #include <linux/iio/buffer.h> 12 #include <linux/iio/iio.h> 13 #include <linux/iio/kfifo_buf.h> 14 #include <linux/iio/sysfs.h> 15 #include <linux/kernel.h> 16 #include <linux/kthread.h> 17 #include <linux/module.h> 18 #include <linux/mutex.h> 19 #include <linux/scmi_protocol.h> 20 #include <linux/time.h> 21 #include <linux/types.h> 22 #include <linux/units.h> 23 24 #define SCMI_IIO_NUM_OF_AXIS 3 25 26 struct scmi_iio_priv { 27 const struct scmi_sensor_proto_ops *sensor_ops; 28 struct scmi_protocol_handle *ph; 29 const struct scmi_sensor_info *sensor_info; 30 struct iio_dev *indio_dev; 31 /* lock to protect against multiple access to the device */ 32 struct mutex lock; 33 /* adding one additional channel for timestamp */ 34 s64 iio_buf[SCMI_IIO_NUM_OF_AXIS + 1]; 35 struct notifier_block sensor_update_nb; 36 u32 *freq_avail; 37 }; 38 39 static int scmi_iio_sensor_update_cb(struct notifier_block *nb, 40 unsigned long event, void *data) 41 { 42 struct scmi_sensor_update_report *sensor_update = data; 43 struct iio_dev *scmi_iio_dev; 44 struct scmi_iio_priv *sensor; 45 s8 tstamp_scale; 46 u64 time, time_ns; 47 int i; 48 49 if (sensor_update->readings_count == 0) 50 return NOTIFY_DONE; 51 52 sensor = container_of(nb, struct scmi_iio_priv, sensor_update_nb); 53 54 for (i = 0; i < sensor_update->readings_count; i++) 55 sensor->iio_buf[i] = sensor_update->readings[i].value; 56 57 if (!sensor->sensor_info->timestamped) { 58 time_ns = ktime_to_ns(sensor_update->timestamp); 59 } else { 60 /* 61 * All the axes are supposed to have the same value for timestamp. 62 * We are just using the values from the Axis 0 here. 63 */ 64 time = sensor_update->readings[0].timestamp; 65 66 /* 67 * Timestamp returned by SCMI is in seconds and is equal to 68 * time * power-of-10 multiplier(tstamp_scale) seconds. 69 * Converting the timestamp to nanoseconds below. 70 */ 71 tstamp_scale = sensor->sensor_info->tstamp_scale + 72 const_ilog2(NSEC_PER_SEC) / const_ilog2(10); 73 if (tstamp_scale < 0) { 74 do_div(time, int_pow(10, abs(tstamp_scale))); 75 time_ns = time; 76 } else { 77 time_ns = time * int_pow(10, tstamp_scale); 78 } 79 } 80 81 scmi_iio_dev = sensor->indio_dev; 82 iio_push_to_buffers_with_timestamp(scmi_iio_dev, sensor->iio_buf, 83 time_ns); 84 return NOTIFY_OK; 85 } 86 87 static int scmi_iio_buffer_preenable(struct iio_dev *iio_dev) 88 { 89 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 90 u32 sensor_config = 0; 91 int err; 92 93 if (sensor->sensor_info->timestamped) 94 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_TSTAMP_ENABLED_MASK, 95 SCMI_SENS_CFG_TSTAMP_ENABLE); 96 97 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, 98 SCMI_SENS_CFG_SENSOR_ENABLE); 99 err = sensor->sensor_ops->config_set(sensor->ph, 100 sensor->sensor_info->id, 101 sensor_config); 102 if (err) 103 dev_err(&iio_dev->dev, "Error in enabling sensor %s err %d", 104 sensor->sensor_info->name, err); 105 106 return err; 107 } 108 109 static int scmi_iio_buffer_postdisable(struct iio_dev *iio_dev) 110 { 111 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 112 u32 sensor_config = 0; 113 int err; 114 115 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, 116 SCMI_SENS_CFG_SENSOR_DISABLE); 117 err = sensor->sensor_ops->config_set(sensor->ph, 118 sensor->sensor_info->id, 119 sensor_config); 120 if (err) { 121 dev_err(&iio_dev->dev, 122 "Error in disabling sensor %s with err %d", 123 sensor->sensor_info->name, err); 124 } 125 126 return err; 127 } 128 129 static const struct iio_buffer_setup_ops scmi_iio_buffer_ops = { 130 .preenable = scmi_iio_buffer_preenable, 131 .postdisable = scmi_iio_buffer_postdisable, 132 }; 133 134 static int scmi_iio_set_odr_val(struct iio_dev *iio_dev, int val, int val2) 135 { 136 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 137 u64 sec, mult, uHz, sf; 138 u32 sensor_config; 139 char buf[32]; 140 141 int err = sensor->sensor_ops->config_get(sensor->ph, 142 sensor->sensor_info->id, 143 &sensor_config); 144 if (err) { 145 dev_err(&iio_dev->dev, 146 "Error in getting sensor config for sensor %s err %d", 147 sensor->sensor_info->name, err); 148 return err; 149 } 150 151 uHz = val * MICROHZ_PER_HZ + val2; 152 153 /* 154 * The seconds field in the sensor interval in SCMI is 16 bits long 155 * Therefore seconds = 1/Hz <= 0xFFFF. As floating point calculations are 156 * discouraged in the kernel driver code, to calculate the scale factor (sf) 157 * (1* 1000000 * sf)/uHz <= 0xFFFF. Therefore, sf <= (uHz * 0xFFFF)/1000000 158 * To calculate the multiplier,we convert the sf into char string and 159 * count the number of characters 160 */ 161 sf = (u64)uHz * 0xFFFF; 162 do_div(sf, MICROHZ_PER_HZ); 163 mult = scnprintf(buf, sizeof(buf), "%llu", sf) - 1; 164 165 sec = int_pow(10, mult) * MICROHZ_PER_HZ; 166 do_div(sec, uHz); 167 if (sec == 0) { 168 dev_err(&iio_dev->dev, 169 "Trying to set invalid sensor update value for sensor %s", 170 sensor->sensor_info->name); 171 return -EINVAL; 172 } 173 174 sensor_config &= ~SCMI_SENS_CFG_UPDATE_SECS_MASK; 175 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_UPDATE_SECS_MASK, sec); 176 sensor_config &= ~SCMI_SENS_CFG_UPDATE_EXP_MASK; 177 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_UPDATE_EXP_MASK, -mult); 178 179 if (sensor->sensor_info->timestamped) { 180 sensor_config &= ~SCMI_SENS_CFG_TSTAMP_ENABLED_MASK; 181 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_TSTAMP_ENABLED_MASK, 182 SCMI_SENS_CFG_TSTAMP_ENABLE); 183 } 184 185 sensor_config &= ~SCMI_SENS_CFG_ROUND_MASK; 186 sensor_config |= 187 FIELD_PREP(SCMI_SENS_CFG_ROUND_MASK, SCMI_SENS_CFG_ROUND_AUTO); 188 189 err = sensor->sensor_ops->config_set(sensor->ph, 190 sensor->sensor_info->id, 191 sensor_config); 192 if (err) 193 dev_err(&iio_dev->dev, 194 "Error in setting sensor update interval for sensor %s value %u err %d", 195 sensor->sensor_info->name, sensor_config, err); 196 197 return err; 198 } 199 200 static int scmi_iio_write_raw(struct iio_dev *iio_dev, 201 struct iio_chan_spec const *chan, int val, 202 int val2, long mask) 203 { 204 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 205 int err; 206 207 switch (mask) { 208 case IIO_CHAN_INFO_SAMP_FREQ: 209 mutex_lock(&sensor->lock); 210 err = scmi_iio_set_odr_val(iio_dev, val, val2); 211 mutex_unlock(&sensor->lock); 212 return err; 213 default: 214 return -EINVAL; 215 } 216 } 217 218 static int scmi_iio_read_avail(struct iio_dev *iio_dev, 219 struct iio_chan_spec const *chan, 220 const int **vals, int *type, int *length, 221 long mask) 222 { 223 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 224 225 switch (mask) { 226 case IIO_CHAN_INFO_SAMP_FREQ: 227 *vals = sensor->freq_avail; 228 *type = IIO_VAL_INT_PLUS_MICRO; 229 *length = sensor->sensor_info->intervals.count * 2; 230 if (sensor->sensor_info->intervals.segmented) 231 return IIO_AVAIL_RANGE; 232 else 233 return IIO_AVAIL_LIST; 234 default: 235 return -EINVAL; 236 } 237 } 238 239 static void convert_ns_to_freq(u64 interval_ns, u64 *hz, u64 *uhz) 240 { 241 u64 rem, freq; 242 243 freq = NSEC_PER_SEC; 244 rem = do_div(freq, interval_ns); 245 *hz = freq; 246 *uhz = rem * 1000000UL; 247 do_div(*uhz, interval_ns); 248 } 249 250 static int scmi_iio_get_odr_val(struct iio_dev *iio_dev, int *val, int *val2) 251 { 252 u64 sensor_update_interval, sensor_interval_mult, hz, uhz; 253 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 254 u32 sensor_config; 255 int mult; 256 257 int err = sensor->sensor_ops->config_get(sensor->ph, 258 sensor->sensor_info->id, 259 &sensor_config); 260 if (err) { 261 dev_err(&iio_dev->dev, 262 "Error in getting sensor config for sensor %s err %d", 263 sensor->sensor_info->name, err); 264 return err; 265 } 266 267 sensor_update_interval = 268 SCMI_SENS_CFG_GET_UPDATE_SECS(sensor_config) * NSEC_PER_SEC; 269 270 mult = SCMI_SENS_CFG_GET_UPDATE_EXP(sensor_config); 271 if (mult < 0) { 272 sensor_interval_mult = int_pow(10, abs(mult)); 273 do_div(sensor_update_interval, sensor_interval_mult); 274 } else { 275 sensor_interval_mult = int_pow(10, mult); 276 sensor_update_interval = 277 sensor_update_interval * sensor_interval_mult; 278 } 279 280 convert_ns_to_freq(sensor_update_interval, &hz, &uhz); 281 *val = hz; 282 *val2 = uhz; 283 return 0; 284 } 285 286 static int scmi_iio_read_channel_data(struct iio_dev *iio_dev, 287 struct iio_chan_spec const *ch, int *val, int *val2) 288 { 289 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 290 u32 sensor_config; 291 struct scmi_sensor_reading readings[SCMI_IIO_NUM_OF_AXIS]; 292 int err; 293 294 sensor_config = FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, 295 SCMI_SENS_CFG_SENSOR_ENABLE); 296 err = sensor->sensor_ops->config_set( 297 sensor->ph, sensor->sensor_info->id, sensor_config); 298 if (err) { 299 dev_err(&iio_dev->dev, 300 "Error in enabling sensor %s err %d", 301 sensor->sensor_info->name, err); 302 return err; 303 } 304 305 err = sensor->sensor_ops->reading_get_timestamped( 306 sensor->ph, sensor->sensor_info->id, 307 sensor->sensor_info->num_axis, readings); 308 if (err) { 309 dev_err(&iio_dev->dev, 310 "Error in reading raw attribute for sensor %s err %d", 311 sensor->sensor_info->name, err); 312 return err; 313 } 314 315 sensor_config = FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, 316 SCMI_SENS_CFG_SENSOR_DISABLE); 317 err = sensor->sensor_ops->config_set( 318 sensor->ph, sensor->sensor_info->id, sensor_config); 319 if (err) { 320 dev_err(&iio_dev->dev, 321 "Error in disabling sensor %s err %d", 322 sensor->sensor_info->name, err); 323 return err; 324 } 325 326 *val = lower_32_bits(readings[ch->scan_index].value); 327 *val2 = upper_32_bits(readings[ch->scan_index].value); 328 329 return IIO_VAL_INT_64; 330 } 331 332 static int scmi_iio_read_raw(struct iio_dev *iio_dev, 333 struct iio_chan_spec const *ch, int *val, 334 int *val2, long mask) 335 { 336 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 337 s8 scale; 338 int ret; 339 340 switch (mask) { 341 case IIO_CHAN_INFO_SCALE: 342 scale = sensor->sensor_info->axis[ch->scan_index].scale; 343 if (scale < 0) { 344 *val = 1; 345 *val2 = int_pow(10, abs(scale)); 346 return IIO_VAL_FRACTIONAL; 347 } 348 *val = int_pow(10, scale); 349 return IIO_VAL_INT; 350 case IIO_CHAN_INFO_SAMP_FREQ: 351 ret = scmi_iio_get_odr_val(iio_dev, val, val2); 352 return ret ? ret : IIO_VAL_INT_PLUS_MICRO; 353 case IIO_CHAN_INFO_RAW: 354 ret = iio_device_claim_direct_mode(iio_dev); 355 if (ret) 356 return ret; 357 358 ret = scmi_iio_read_channel_data(iio_dev, ch, val, val2); 359 iio_device_release_direct_mode(iio_dev); 360 return ret; 361 default: 362 return -EINVAL; 363 } 364 } 365 366 static const struct iio_info scmi_iio_info = { 367 .read_raw = scmi_iio_read_raw, 368 .read_avail = scmi_iio_read_avail, 369 .write_raw = scmi_iio_write_raw, 370 }; 371 372 static ssize_t scmi_iio_get_raw_available(struct iio_dev *iio_dev, 373 uintptr_t private, 374 const struct iio_chan_spec *chan, 375 char *buf) 376 { 377 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 378 u64 resolution, rem; 379 s64 min_range, max_range; 380 s8 exponent, scale; 381 int len = 0; 382 383 /* 384 * All the axes are supposed to have the same value for range and resolution. 385 * We are just using the values from the Axis 0 here. 386 */ 387 if (sensor->sensor_info->axis[0].extended_attrs) { 388 min_range = sensor->sensor_info->axis[0].attrs.min_range; 389 max_range = sensor->sensor_info->axis[0].attrs.max_range; 390 resolution = sensor->sensor_info->axis[0].resolution; 391 exponent = sensor->sensor_info->axis[0].exponent; 392 scale = sensor->sensor_info->axis[0].scale; 393 394 /* 395 * To provide the raw value for the resolution to the userspace, 396 * need to divide the resolution exponent by the sensor scale 397 */ 398 exponent = exponent - scale; 399 if (exponent < 0) { 400 rem = do_div(resolution, 401 int_pow(10, abs(exponent)) 402 ); 403 len = sysfs_emit(buf, 404 "[%lld %llu.%llu %lld]\n", min_range, 405 resolution, rem, max_range); 406 } else { 407 resolution = resolution * int_pow(10, exponent); 408 len = sysfs_emit(buf, "[%lld %llu %lld]\n", 409 min_range, resolution, max_range); 410 } 411 } 412 return len; 413 } 414 415 static const struct iio_chan_spec_ext_info scmi_iio_ext_info[] = { 416 { 417 .name = "raw_available", 418 .read = scmi_iio_get_raw_available, 419 .shared = IIO_SHARED_BY_TYPE, 420 }, 421 {}, 422 }; 423 424 static void scmi_iio_set_timestamp_channel(struct iio_chan_spec *iio_chan, 425 int scan_index) 426 { 427 iio_chan->type = IIO_TIMESTAMP; 428 iio_chan->channel = -1; 429 iio_chan->scan_index = scan_index; 430 iio_chan->scan_type.sign = 'u'; 431 iio_chan->scan_type.realbits = 64; 432 iio_chan->scan_type.storagebits = 64; 433 } 434 435 static void scmi_iio_set_data_channel(struct iio_chan_spec *iio_chan, 436 enum iio_chan_type type, 437 enum iio_modifier mod, int scan_index) 438 { 439 iio_chan->type = type; 440 iio_chan->modified = 1; 441 iio_chan->channel2 = mod; 442 iio_chan->info_mask_separate = 443 BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_RAW); 444 iio_chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ); 445 iio_chan->info_mask_shared_by_type_available = 446 BIT(IIO_CHAN_INFO_SAMP_FREQ); 447 iio_chan->scan_index = scan_index; 448 iio_chan->scan_type.sign = 's'; 449 iio_chan->scan_type.realbits = 64; 450 iio_chan->scan_type.storagebits = 64; 451 iio_chan->scan_type.endianness = IIO_LE; 452 iio_chan->ext_info = scmi_iio_ext_info; 453 } 454 455 static int scmi_iio_get_chan_modifier(const char *name, 456 enum iio_modifier *modifier) 457 { 458 char *pch, mod; 459 460 if (!name) 461 return -EINVAL; 462 463 pch = strrchr(name, '_'); 464 if (!pch) 465 return -EINVAL; 466 467 mod = *(pch + 1); 468 switch (mod) { 469 case 'X': 470 *modifier = IIO_MOD_X; 471 return 0; 472 case 'Y': 473 *modifier = IIO_MOD_Y; 474 return 0; 475 case 'Z': 476 *modifier = IIO_MOD_Z; 477 return 0; 478 default: 479 return -EINVAL; 480 } 481 } 482 483 static int scmi_iio_get_chan_type(u8 scmi_type, enum iio_chan_type *iio_type) 484 { 485 switch (scmi_type) { 486 case METERS_SEC_SQUARED: 487 *iio_type = IIO_ACCEL; 488 return 0; 489 case RADIANS_SEC: 490 *iio_type = IIO_ANGL_VEL; 491 return 0; 492 default: 493 return -EINVAL; 494 } 495 } 496 497 static u64 scmi_iio_convert_interval_to_ns(u32 val) 498 { 499 u64 sensor_update_interval = 500 SCMI_SENS_INTVL_GET_SECS(val) * NSEC_PER_SEC; 501 u64 sensor_interval_mult; 502 int mult; 503 504 mult = SCMI_SENS_INTVL_GET_EXP(val); 505 if (mult < 0) { 506 sensor_interval_mult = int_pow(10, abs(mult)); 507 do_div(sensor_update_interval, sensor_interval_mult); 508 } else { 509 sensor_interval_mult = int_pow(10, mult); 510 sensor_update_interval = 511 sensor_update_interval * sensor_interval_mult; 512 } 513 return sensor_update_interval; 514 } 515 516 static int scmi_iio_set_sampling_freq_avail(struct iio_dev *iio_dev) 517 { 518 u64 cur_interval_ns, low_interval_ns, high_interval_ns, step_size_ns, 519 hz, uhz; 520 unsigned int cur_interval, low_interval, high_interval, step_size; 521 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 522 int i; 523 524 sensor->freq_avail = 525 devm_kzalloc(&iio_dev->dev, 526 sizeof(*sensor->freq_avail) * 527 (sensor->sensor_info->intervals.count * 2), 528 GFP_KERNEL); 529 if (!sensor->freq_avail) 530 return -ENOMEM; 531 532 if (sensor->sensor_info->intervals.segmented) { 533 low_interval = sensor->sensor_info->intervals 534 .desc[SCMI_SENS_INTVL_SEGMENT_LOW]; 535 low_interval_ns = scmi_iio_convert_interval_to_ns(low_interval); 536 convert_ns_to_freq(low_interval_ns, &hz, &uhz); 537 sensor->freq_avail[0] = hz; 538 sensor->freq_avail[1] = uhz; 539 540 step_size = sensor->sensor_info->intervals 541 .desc[SCMI_SENS_INTVL_SEGMENT_STEP]; 542 step_size_ns = scmi_iio_convert_interval_to_ns(step_size); 543 convert_ns_to_freq(step_size_ns, &hz, &uhz); 544 sensor->freq_avail[2] = hz; 545 sensor->freq_avail[3] = uhz; 546 547 high_interval = sensor->sensor_info->intervals 548 .desc[SCMI_SENS_INTVL_SEGMENT_HIGH]; 549 high_interval_ns = 550 scmi_iio_convert_interval_to_ns(high_interval); 551 convert_ns_to_freq(high_interval_ns, &hz, &uhz); 552 sensor->freq_avail[4] = hz; 553 sensor->freq_avail[5] = uhz; 554 } else { 555 for (i = 0; i < sensor->sensor_info->intervals.count; i++) { 556 cur_interval = sensor->sensor_info->intervals.desc[i]; 557 cur_interval_ns = 558 scmi_iio_convert_interval_to_ns(cur_interval); 559 convert_ns_to_freq(cur_interval_ns, &hz, &uhz); 560 sensor->freq_avail[i * 2] = hz; 561 sensor->freq_avail[i * 2 + 1] = uhz; 562 } 563 } 564 return 0; 565 } 566 567 static struct iio_dev * 568 scmi_alloc_iiodev(struct scmi_device *sdev, 569 const struct scmi_sensor_proto_ops *ops, 570 struct scmi_protocol_handle *ph, 571 const struct scmi_sensor_info *sensor_info) 572 { 573 struct iio_chan_spec *iio_channels; 574 struct scmi_iio_priv *sensor; 575 enum iio_modifier modifier; 576 enum iio_chan_type type; 577 struct iio_dev *iiodev; 578 struct device *dev = &sdev->dev; 579 const struct scmi_handle *handle = sdev->handle; 580 int i, ret; 581 582 iiodev = devm_iio_device_alloc(dev, sizeof(*sensor)); 583 if (!iiodev) 584 return ERR_PTR(-ENOMEM); 585 586 iiodev->modes = INDIO_DIRECT_MODE; 587 sensor = iio_priv(iiodev); 588 sensor->sensor_ops = ops; 589 sensor->ph = ph; 590 sensor->sensor_info = sensor_info; 591 sensor->sensor_update_nb.notifier_call = scmi_iio_sensor_update_cb; 592 sensor->indio_dev = iiodev; 593 mutex_init(&sensor->lock); 594 595 /* adding one additional channel for timestamp */ 596 iiodev->num_channels = sensor_info->num_axis + 1; 597 iiodev->name = sensor_info->name; 598 iiodev->info = &scmi_iio_info; 599 600 iio_channels = 601 devm_kzalloc(dev, 602 sizeof(*iio_channels) * (iiodev->num_channels), 603 GFP_KERNEL); 604 if (!iio_channels) 605 return ERR_PTR(-ENOMEM); 606 607 ret = scmi_iio_set_sampling_freq_avail(iiodev); 608 if (ret < 0) 609 return ERR_PTR(ret); 610 611 for (i = 0; i < sensor_info->num_axis; i++) { 612 ret = scmi_iio_get_chan_type(sensor_info->axis[i].type, &type); 613 if (ret < 0) 614 return ERR_PTR(ret); 615 616 ret = scmi_iio_get_chan_modifier(sensor_info->axis[i].name, 617 &modifier); 618 if (ret < 0) 619 return ERR_PTR(ret); 620 621 scmi_iio_set_data_channel(&iio_channels[i], type, modifier, 622 sensor_info->axis[i].id); 623 } 624 625 ret = handle->notify_ops->devm_event_notifier_register(sdev, 626 SCMI_PROTOCOL_SENSOR, SCMI_EVENT_SENSOR_UPDATE, 627 &sensor->sensor_info->id, 628 &sensor->sensor_update_nb); 629 if (ret) { 630 dev_err(&iiodev->dev, 631 "Error in registering sensor update notifier for sensor %s err %d", 632 sensor->sensor_info->name, ret); 633 return ERR_PTR(ret); 634 } 635 636 scmi_iio_set_timestamp_channel(&iio_channels[i], i); 637 iiodev->channels = iio_channels; 638 return iiodev; 639 } 640 641 static int scmi_iio_dev_probe(struct scmi_device *sdev) 642 { 643 const struct scmi_sensor_info *sensor_info; 644 struct scmi_handle *handle = sdev->handle; 645 const struct scmi_sensor_proto_ops *sensor_ops; 646 struct scmi_protocol_handle *ph; 647 struct device *dev = &sdev->dev; 648 struct iio_dev *scmi_iio_dev; 649 u16 nr_sensors; 650 int err = -ENODEV, i; 651 652 if (!handle) 653 return -ENODEV; 654 655 sensor_ops = handle->devm_protocol_get(sdev, SCMI_PROTOCOL_SENSOR, &ph); 656 if (IS_ERR(sensor_ops)) { 657 dev_err(dev, "SCMI device has no sensor interface\n"); 658 return PTR_ERR(sensor_ops); 659 } 660 661 nr_sensors = sensor_ops->count_get(ph); 662 if (!nr_sensors) { 663 dev_dbg(dev, "0 sensors found via SCMI bus\n"); 664 return -ENODEV; 665 } 666 667 for (i = 0; i < nr_sensors; i++) { 668 sensor_info = sensor_ops->info_get(ph, i); 669 if (!sensor_info) { 670 dev_err(dev, "SCMI sensor %d has missing info\n", i); 671 return -EINVAL; 672 } 673 674 /* This driver only supports 3-axis accel and gyro, skipping other sensors */ 675 if (sensor_info->num_axis != SCMI_IIO_NUM_OF_AXIS) 676 continue; 677 678 /* This driver only supports 3-axis accel and gyro, skipping other sensors */ 679 if (sensor_info->axis[0].type != METERS_SEC_SQUARED && 680 sensor_info->axis[0].type != RADIANS_SEC) 681 continue; 682 683 scmi_iio_dev = scmi_alloc_iiodev(sdev, sensor_ops, ph, 684 sensor_info); 685 if (IS_ERR(scmi_iio_dev)) { 686 dev_err(dev, 687 "failed to allocate IIO device for sensor %s: %ld\n", 688 sensor_info->name, PTR_ERR(scmi_iio_dev)); 689 return PTR_ERR(scmi_iio_dev); 690 } 691 692 err = devm_iio_kfifo_buffer_setup(&scmi_iio_dev->dev, 693 scmi_iio_dev, 694 &scmi_iio_buffer_ops); 695 if (err < 0) { 696 dev_err(dev, 697 "IIO buffer setup error at sensor %s: %d\n", 698 sensor_info->name, err); 699 return err; 700 } 701 702 err = devm_iio_device_register(dev, scmi_iio_dev); 703 if (err) { 704 dev_err(dev, 705 "IIO device registration failed at sensor %s: %d\n", 706 sensor_info->name, err); 707 return err; 708 } 709 } 710 return err; 711 } 712 713 static const struct scmi_device_id scmi_id_table[] = { 714 { SCMI_PROTOCOL_SENSOR, "iiodev" }, 715 {}, 716 }; 717 718 MODULE_DEVICE_TABLE(scmi, scmi_id_table); 719 720 static struct scmi_driver scmi_iiodev_driver = { 721 .name = "scmi-sensor-iiodev", 722 .probe = scmi_iio_dev_probe, 723 .id_table = scmi_id_table, 724 }; 725 726 module_scmi_driver(scmi_iiodev_driver); 727 728 MODULE_AUTHOR("Jyoti Bhayana <jbhayana@google.com>"); 729 MODULE_DESCRIPTION("SCMI IIO Driver"); 730 MODULE_LICENSE("GPL v2"); 731