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 = 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 if (!iio_device_claim_direct(iio_dev)) 355 return -EBUSY; 356 357 ret = scmi_iio_read_channel_data(iio_dev, ch, val, val2); 358 iio_device_release_direct(iio_dev); 359 return ret; 360 default: 361 return -EINVAL; 362 } 363 } 364 365 static const struct iio_info scmi_iio_info = { 366 .read_raw = scmi_iio_read_raw, 367 .read_avail = scmi_iio_read_avail, 368 .write_raw = scmi_iio_write_raw, 369 }; 370 371 static ssize_t scmi_iio_get_raw_available(struct iio_dev *iio_dev, 372 uintptr_t private, 373 const struct iio_chan_spec *chan, 374 char *buf) 375 { 376 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 377 u64 resolution, rem; 378 s64 min_range, max_range; 379 s8 exponent, scale; 380 int len = 0; 381 382 /* 383 * All the axes are supposed to have the same value for range and resolution. 384 * We are just using the values from the Axis 0 here. 385 */ 386 if (sensor->sensor_info->axis[0].extended_attrs) { 387 min_range = sensor->sensor_info->axis[0].attrs.min_range; 388 max_range = sensor->sensor_info->axis[0].attrs.max_range; 389 resolution = sensor->sensor_info->axis[0].resolution; 390 exponent = sensor->sensor_info->axis[0].exponent; 391 scale = sensor->sensor_info->axis[0].scale; 392 393 /* 394 * To provide the raw value for the resolution to the userspace, 395 * need to divide the resolution exponent by the sensor scale 396 */ 397 exponent = exponent - scale; 398 if (exponent < 0) { 399 rem = do_div(resolution, 400 int_pow(10, abs(exponent)) 401 ); 402 len = sysfs_emit(buf, 403 "[%lld %llu.%llu %lld]\n", min_range, 404 resolution, rem, max_range); 405 } else { 406 resolution = resolution * int_pow(10, exponent); 407 len = sysfs_emit(buf, "[%lld %llu %lld]\n", 408 min_range, resolution, max_range); 409 } 410 } 411 return len; 412 } 413 414 static const struct iio_chan_spec_ext_info scmi_iio_ext_info[] = { 415 { 416 .name = "raw_available", 417 .read = scmi_iio_get_raw_available, 418 .shared = IIO_SHARED_BY_TYPE, 419 }, 420 { } 421 }; 422 423 static void scmi_iio_set_timestamp_channel(struct iio_chan_spec *iio_chan, 424 int scan_index) 425 { 426 iio_chan->type = IIO_TIMESTAMP; 427 iio_chan->channel = -1; 428 iio_chan->scan_index = scan_index; 429 iio_chan->scan_type.sign = 'u'; 430 iio_chan->scan_type.realbits = 64; 431 iio_chan->scan_type.storagebits = 64; 432 } 433 434 static void scmi_iio_set_data_channel(struct iio_chan_spec *iio_chan, 435 enum iio_chan_type type, 436 enum iio_modifier mod, int scan_index) 437 { 438 iio_chan->type = type; 439 iio_chan->modified = 1; 440 iio_chan->channel2 = mod; 441 iio_chan->info_mask_separate = 442 BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_RAW); 443 iio_chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ); 444 iio_chan->info_mask_shared_by_type_available = 445 BIT(IIO_CHAN_INFO_SAMP_FREQ); 446 iio_chan->scan_index = scan_index; 447 iio_chan->scan_type.sign = 's'; 448 iio_chan->scan_type.realbits = 64; 449 iio_chan->scan_type.storagebits = 64; 450 iio_chan->scan_type.endianness = IIO_LE; 451 iio_chan->ext_info = scmi_iio_ext_info; 452 } 453 454 static int scmi_iio_get_chan_modifier(const char *name, 455 enum iio_modifier *modifier) 456 { 457 char *pch, mod; 458 459 if (!name) 460 return -EINVAL; 461 462 pch = strrchr(name, '_'); 463 if (!pch) 464 return -EINVAL; 465 466 mod = *(pch + 1); 467 switch (mod) { 468 case 'X': 469 *modifier = IIO_MOD_X; 470 return 0; 471 case 'Y': 472 *modifier = IIO_MOD_Y; 473 return 0; 474 case 'Z': 475 *modifier = IIO_MOD_Z; 476 return 0; 477 default: 478 return -EINVAL; 479 } 480 } 481 482 static int scmi_iio_get_chan_type(u8 scmi_type, enum iio_chan_type *iio_type) 483 { 484 switch (scmi_type) { 485 case METERS_SEC_SQUARED: 486 *iio_type = IIO_ACCEL; 487 return 0; 488 case RADIANS_SEC: 489 *iio_type = IIO_ANGL_VEL; 490 return 0; 491 default: 492 return -EINVAL; 493 } 494 } 495 496 static u64 scmi_iio_convert_interval_to_ns(u32 val) 497 { 498 u64 sensor_update_interval = 499 SCMI_SENS_INTVL_GET_SECS(val) * NSEC_PER_SEC; 500 u64 sensor_interval_mult; 501 int mult; 502 503 mult = SCMI_SENS_INTVL_GET_EXP(val); 504 if (mult < 0) { 505 sensor_interval_mult = int_pow(10, abs(mult)); 506 do_div(sensor_update_interval, sensor_interval_mult); 507 } else { 508 sensor_interval_mult = int_pow(10, mult); 509 sensor_update_interval = 510 sensor_update_interval * sensor_interval_mult; 511 } 512 return sensor_update_interval; 513 } 514 515 static int scmi_iio_set_sampling_freq_avail(struct iio_dev *iio_dev) 516 { 517 u64 cur_interval_ns, low_interval_ns, high_interval_ns, step_size_ns, 518 hz, uhz; 519 unsigned int cur_interval, low_interval, high_interval, step_size; 520 struct scmi_iio_priv *sensor = iio_priv(iio_dev); 521 int i; 522 523 sensor->freq_avail = 524 devm_kzalloc(&iio_dev->dev, 525 sizeof(*sensor->freq_avail) * 526 (sensor->sensor_info->intervals.count * 2), 527 GFP_KERNEL); 528 if (!sensor->freq_avail) 529 return -ENOMEM; 530 531 if (sensor->sensor_info->intervals.segmented) { 532 low_interval = sensor->sensor_info->intervals 533 .desc[SCMI_SENS_INTVL_SEGMENT_LOW]; 534 low_interval_ns = scmi_iio_convert_interval_to_ns(low_interval); 535 convert_ns_to_freq(low_interval_ns, &hz, &uhz); 536 sensor->freq_avail[0] = hz; 537 sensor->freq_avail[1] = uhz; 538 539 step_size = sensor->sensor_info->intervals 540 .desc[SCMI_SENS_INTVL_SEGMENT_STEP]; 541 step_size_ns = scmi_iio_convert_interval_to_ns(step_size); 542 convert_ns_to_freq(step_size_ns, &hz, &uhz); 543 sensor->freq_avail[2] = hz; 544 sensor->freq_avail[3] = uhz; 545 546 high_interval = sensor->sensor_info->intervals 547 .desc[SCMI_SENS_INTVL_SEGMENT_HIGH]; 548 high_interval_ns = 549 scmi_iio_convert_interval_to_ns(high_interval); 550 convert_ns_to_freq(high_interval_ns, &hz, &uhz); 551 sensor->freq_avail[4] = hz; 552 sensor->freq_avail[5] = uhz; 553 } else { 554 for (i = 0; i < sensor->sensor_info->intervals.count; i++) { 555 cur_interval = sensor->sensor_info->intervals.desc[i]; 556 cur_interval_ns = 557 scmi_iio_convert_interval_to_ns(cur_interval); 558 convert_ns_to_freq(cur_interval_ns, &hz, &uhz); 559 sensor->freq_avail[i * 2] = hz; 560 sensor->freq_avail[i * 2 + 1] = uhz; 561 } 562 } 563 return 0; 564 } 565 566 static struct iio_dev * 567 scmi_alloc_iiodev(struct scmi_device *sdev, 568 const struct scmi_sensor_proto_ops *ops, 569 struct scmi_protocol_handle *ph, 570 const struct scmi_sensor_info *sensor_info) 571 { 572 struct iio_chan_spec *iio_channels; 573 struct scmi_iio_priv *sensor; 574 enum iio_modifier modifier; 575 enum iio_chan_type type; 576 struct iio_dev *iiodev; 577 struct device *dev = &sdev->dev; 578 const struct scmi_handle *handle = sdev->handle; 579 int i, ret; 580 581 iiodev = devm_iio_device_alloc(dev, sizeof(*sensor)); 582 if (!iiodev) 583 return ERR_PTR(-ENOMEM); 584 585 iiodev->modes = INDIO_DIRECT_MODE; 586 sensor = iio_priv(iiodev); 587 sensor->sensor_ops = ops; 588 sensor->ph = ph; 589 sensor->sensor_info = sensor_info; 590 sensor->sensor_update_nb.notifier_call = scmi_iio_sensor_update_cb; 591 sensor->indio_dev = iiodev; 592 mutex_init(&sensor->lock); 593 594 /* adding one additional channel for timestamp */ 595 iiodev->num_channels = sensor_info->num_axis + 1; 596 iiodev->name = sensor_info->name; 597 iiodev->info = &scmi_iio_info; 598 599 iio_channels = 600 devm_kzalloc(dev, 601 sizeof(*iio_channels) * (iiodev->num_channels), 602 GFP_KERNEL); 603 if (!iio_channels) 604 return ERR_PTR(-ENOMEM); 605 606 ret = scmi_iio_set_sampling_freq_avail(iiodev); 607 if (ret < 0) 608 return ERR_PTR(ret); 609 610 for (i = 0; i < sensor_info->num_axis; i++) { 611 ret = scmi_iio_get_chan_type(sensor_info->axis[i].type, &type); 612 if (ret < 0) 613 return ERR_PTR(ret); 614 615 ret = scmi_iio_get_chan_modifier(sensor_info->axis[i].name, 616 &modifier); 617 if (ret < 0) 618 return ERR_PTR(ret); 619 620 scmi_iio_set_data_channel(&iio_channels[i], type, modifier, 621 sensor_info->axis[i].id); 622 } 623 624 ret = handle->notify_ops->devm_event_notifier_register(sdev, 625 SCMI_PROTOCOL_SENSOR, SCMI_EVENT_SENSOR_UPDATE, 626 &sensor->sensor_info->id, 627 &sensor->sensor_update_nb); 628 if (ret) 629 return dev_err_ptr_probe(&iiodev->dev, ret, 630 "Error in registering sensor update notifier for sensor %s\n", 631 sensor->sensor_info->name); 632 633 scmi_iio_set_timestamp_channel(&iio_channels[i], i); 634 iiodev->channels = iio_channels; 635 return iiodev; 636 } 637 638 static int scmi_iio_dev_probe(struct scmi_device *sdev) 639 { 640 const struct scmi_sensor_info *sensor_info; 641 struct scmi_handle *handle = sdev->handle; 642 const struct scmi_sensor_proto_ops *sensor_ops; 643 struct scmi_protocol_handle *ph; 644 struct device *dev = &sdev->dev; 645 struct iio_dev *scmi_iio_dev; 646 u16 nr_sensors; 647 int err = -ENODEV, i; 648 649 if (!handle) 650 return -ENODEV; 651 652 sensor_ops = handle->devm_protocol_get(sdev, SCMI_PROTOCOL_SENSOR, &ph); 653 if (IS_ERR(sensor_ops)) 654 return dev_err_probe(dev, PTR_ERR(sensor_ops), 655 "SCMI device has no sensor interface\n"); 656 657 nr_sensors = sensor_ops->count_get(ph); 658 if (!nr_sensors) { 659 dev_dbg(dev, "0 sensors found via SCMI bus\n"); 660 return -ENODEV; 661 } 662 663 for (i = 0; i < nr_sensors; i++) { 664 sensor_info = sensor_ops->info_get(ph, i); 665 if (!sensor_info) { 666 return dev_err_probe(dev, -EINVAL, 667 "SCMI sensor %d has missing info\n", i); 668 } 669 670 /* This driver only supports 3-axis accel and gyro, skipping other sensors */ 671 if (sensor_info->num_axis != SCMI_IIO_NUM_OF_AXIS) 672 continue; 673 674 /* This driver only supports 3-axis accel and gyro, skipping other sensors */ 675 if (sensor_info->axis[0].type != METERS_SEC_SQUARED && 676 sensor_info->axis[0].type != RADIANS_SEC) 677 continue; 678 679 scmi_iio_dev = scmi_alloc_iiodev(sdev, sensor_ops, ph, 680 sensor_info); 681 if (IS_ERR(scmi_iio_dev)) { 682 return dev_err_probe(dev, PTR_ERR(scmi_iio_dev), 683 "failed to allocate IIO device for sensor %s\n", 684 sensor_info->name); 685 } 686 687 err = devm_iio_kfifo_buffer_setup(&scmi_iio_dev->dev, 688 scmi_iio_dev, 689 &scmi_iio_buffer_ops); 690 if (err < 0) { 691 return dev_err_probe(dev, err, 692 "IIO buffer setup error at sensor %s\n", 693 sensor_info->name); 694 } 695 696 err = devm_iio_device_register(dev, scmi_iio_dev); 697 if (err) 698 return dev_err_probe(dev, err, 699 "IIO device registration failed at sensor %s\n", 700 sensor_info->name); 701 } 702 return err; 703 } 704 705 static const struct scmi_device_id scmi_id_table[] = { 706 { SCMI_PROTOCOL_SENSOR, "iiodev" }, 707 { } 708 }; 709 710 MODULE_DEVICE_TABLE(scmi, scmi_id_table); 711 712 static struct scmi_driver scmi_iiodev_driver = { 713 .name = "scmi-sensor-iiodev", 714 .probe = scmi_iio_dev_probe, 715 .id_table = scmi_id_table, 716 }; 717 718 module_scmi_driver(scmi_iiodev_driver); 719 720 MODULE_AUTHOR("Jyoti Bhayana <jbhayana@google.com>"); 721 MODULE_DESCRIPTION("SCMI IIO Driver"); 722 MODULE_LICENSE("GPL v2"); 723