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