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