drivers/iio/common/scmi_sensors/scmi_iio.c

   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
  22 #define SCMI_IIO_NUM_OF_AXIS 3
  23
  24 struct scmi_iio_priv {
  25         const struct scmi_sensor_proto_ops *sensor_ops;
  26         struct scmi_protocol_handle *ph;
  27         const struct scmi_sensor_info *sensor_info;
  28         struct iio_dev *indio_dev;
  29         /* adding one additional channel for timestamp */
  30         s64 iio_buf[SCMI_IIO_NUM_OF_AXIS + 1];
  31         struct notifier_block sensor_update_nb;
  32         u32 *freq_avail;
  33 };
  34
  35 static int scmi_iio_sensor_update_cb(struct notifier_block *nb,
  36                                      unsigned long event, void *data)
  37 {
  38         struct scmi_sensor_update_report *sensor_update = data;
  39         struct iio_dev *scmi_iio_dev;
  40         struct scmi_iio_priv *sensor;
  41         s8 tstamp_scale;
  42         u64 time, time_ns;
  43         int i;
  44
  45         if (sensor_update->readings_count == 0)
  46                 return NOTIFY_DONE;
  47
  48         sensor = container_of(nb, struct scmi_iio_priv, sensor_update_nb);
  49
  50         for (i = 0; i < sensor_update->readings_count; i++)
  51                 sensor->iio_buf[i] = sensor_update->readings[i].value;
  52
  53         if (!sensor->sensor_info->timestamped) {
  54                 time_ns = ktime_to_ns(sensor_update->timestamp);
  55         } else {
  56                 /*
  57                  *  All the axes are supposed to have the same value for timestamp.
  58                  *  We are just using the values from the Axis 0 here.
  59                  */
  60                 time = sensor_update->readings[0].timestamp;
  61
  62                 /*
  63                  *  Timestamp returned by SCMI is in seconds and is equal to
  64                  *  time * power-of-10 multiplier(tstamp_scale) seconds.
  65                  *  Converting the timestamp to nanoseconds below.
  66                  */
  67                 tstamp_scale = sensor->sensor_info->tstamp_scale +
  68                                const_ilog2(NSEC_PER_SEC) / const_ilog2(10);
  69                 if (tstamp_scale < 0) {
  70                         do_div(time, int_pow(10, abs(tstamp_scale)));
  71                         time_ns = time;
  72                 } else {
  73                         time_ns = time * int_pow(10, tstamp_scale);
  74                 }
  75         }
  76
  77         scmi_iio_dev = sensor->indio_dev;
  78         iio_push_to_buffers_with_timestamp(scmi_iio_dev, sensor->iio_buf,
  79                                            time_ns);
  80         return NOTIFY_OK;
  81 }
  82
  83 static int scmi_iio_buffer_preenable(struct iio_dev *iio_dev)
  84 {
  85         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
  86         u32 sensor_config = 0;
  87         int err;
  88
  89         if (sensor->sensor_info->timestamped)
  90                 sensor_config |= FIELD_PREP(SCMI_SENS_CFG_TSTAMP_ENABLED_MASK,
  91                                             SCMI_SENS_CFG_TSTAMP_ENABLE);
  92
  93         sensor_config |= FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK,
  94                                     SCMI_SENS_CFG_SENSOR_ENABLE);
  95         err = sensor->sensor_ops->config_set(sensor->ph,
  96                                              sensor->sensor_info->id,
  97                                              sensor_config);
  98         if (err)
  99                 dev_err(&iio_dev->dev, "Error in enabling sensor %s err %d",
 100                         sensor->sensor_info->name, err);
 101
 102         return err;
 103 }
 104
 105 static int scmi_iio_buffer_postdisable(struct iio_dev *iio_dev)
 106 {
 107         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
 108         u32 sensor_config = 0;
 109         int err;
 110
 111         sensor_config |= FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK,
 112                                     SCMI_SENS_CFG_SENSOR_DISABLE);
 113         err = sensor->sensor_ops->config_set(sensor->ph,
 114                                              sensor->sensor_info->id,
 115                                              sensor_config);
 116         if (err) {
 117                 dev_err(&iio_dev->dev,
 118                         "Error in disabling sensor %s with err %d",
 119                         sensor->sensor_info->name, err);
 120         }
 121
 122         return err;
 123 }
 124
 125 static const struct iio_buffer_setup_ops scmi_iio_buffer_ops = {
 126         .preenable = scmi_iio_buffer_preenable,
 127         .postdisable = scmi_iio_buffer_postdisable,
 128 };
 129
 130 static int scmi_iio_set_odr_val(struct iio_dev *iio_dev, int val, int val2)
 131 {
 132         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
 133         const unsigned long UHZ_PER_HZ = 1000000UL;
 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 * UHZ_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,  UHZ_PER_HZ);
 160         mult = scnprintf(buf, sizeof(buf), "%llu", sf) - 1;
 161
 162         sec = int_pow(10, mult) * UHZ_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_raw(struct iio_dev *iio_dev,
 283                              struct iio_chan_spec const *ch, int *val,
 284                              int *val2, long mask)
 285 {
 286         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
 287         s8 scale;
 288         int ret;
 289
 290         switch (mask) {
 291         case IIO_CHAN_INFO_SCALE:
 292                 scale = sensor->sensor_info->axis[ch->scan_index].scale;
 293                 if (scale < 0) {
 294                         *val = 1;
 295                         *val2 = int_pow(10, abs(scale));
 296                         return IIO_VAL_FRACTIONAL;
 297                 }
 298                 *val = int_pow(10, scale);
 299                 return IIO_VAL_INT;
 300         case IIO_CHAN_INFO_SAMP_FREQ:
 301                 ret = scmi_iio_get_odr_val(iio_dev, val, val2);
 302                 return ret ? ret : IIO_VAL_INT_PLUS_MICRO;
 303         default:
 304                 return -EINVAL;
 305         }
 306 }
 307
 308 static const struct iio_info scmi_iio_info = {
 309         .read_raw = scmi_iio_read_raw,
 310         .read_avail = scmi_iio_read_avail,
 311         .write_raw = scmi_iio_write_raw,
 312 };
 313
 314 static ssize_t scmi_iio_get_raw_available(struct iio_dev *iio_dev,
 315                                           uintptr_t private,
 316                                           const struct iio_chan_spec *chan,
 317                                           char *buf)
 318 {
 319         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
 320         u64 resolution, rem;
 321         s64 min_range, max_range;
 322         s8 exponent, scale;
 323         int len = 0;
 324
 325         /*
 326          * All the axes are supposed to have the same value for range and resolution.
 327          * We are just using the values from the Axis 0 here.
 328          */
 329         if (sensor->sensor_info->axis[0].extended_attrs) {
 330                 min_range = sensor->sensor_info->axis[0].attrs.min_range;
 331                 max_range = sensor->sensor_info->axis[0].attrs.max_range;
 332                 resolution = sensor->sensor_info->axis[0].resolution;
 333                 exponent = sensor->sensor_info->axis[0].exponent;
 334                 scale = sensor->sensor_info->axis[0].scale;
 335
 336                 /*
 337                  * To provide the raw value for the resolution to the userspace,
 338                  * need to divide the resolution exponent by the sensor scale
 339                  */
 340                 exponent = exponent - scale;
 341                 if (exponent < 0) {
 342                         rem = do_div(resolution,
 343                                      int_pow(10, abs(exponent))
 344                                      );
 345                         len = scnprintf(buf, PAGE_SIZE,
 346                                         "[%lld %llu.%llu %lld]\n", min_range,
 347                                         resolution, rem, max_range);
 348                 } else {
 349                         resolution = resolution * int_pow(10, exponent);
 350                         len = scnprintf(buf, PAGE_SIZE, "[%lld %llu %lld]\n",
 351                                         min_range, resolution, max_range);
 352                 }
 353         }
 354         return len;
 355 }
 356
 357 static const struct iio_chan_spec_ext_info scmi_iio_ext_info[] = {
 358         {
 359                 .name = "raw_available",
 360                 .read = scmi_iio_get_raw_available,
 361                 .shared = IIO_SHARED_BY_TYPE,
 362         },
 363         {},
 364 };
 365
 366 static void scmi_iio_set_timestamp_channel(struct iio_chan_spec *iio_chan,
 367                                            int scan_index)
 368 {
 369         iio_chan->type = IIO_TIMESTAMP;
 370         iio_chan->channel = -1;
 371         iio_chan->scan_index = scan_index;
 372         iio_chan->scan_type.sign = 'u';
 373         iio_chan->scan_type.realbits = 64;
 374         iio_chan->scan_type.storagebits = 64;
 375 }
 376
 377 static void scmi_iio_set_data_channel(struct iio_chan_spec *iio_chan,
 378                                       enum iio_chan_type type,
 379                                       enum iio_modifier mod, int scan_index)
 380 {
 381         iio_chan->type = type;
 382         iio_chan->modified = 1;
 383         iio_chan->channel2 = mod;
 384         iio_chan->info_mask_separate = BIT(IIO_CHAN_INFO_SCALE);
 385         iio_chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ);
 386         iio_chan->info_mask_shared_by_type_available =
 387                 BIT(IIO_CHAN_INFO_SAMP_FREQ);
 388         iio_chan->scan_index = scan_index;
 389         iio_chan->scan_type.sign = 's';
 390         iio_chan->scan_type.realbits = 64;
 391         iio_chan->scan_type.storagebits = 64;
 392         iio_chan->scan_type.endianness = IIO_LE;
 393         iio_chan->ext_info = scmi_iio_ext_info;
 394 }
 395
 396 static int scmi_iio_get_chan_modifier(const char *name,
 397                                       enum iio_modifier *modifier)
 398 {
 399         char *pch, mod;
 400
 401         if (!name)
 402                 return -EINVAL;
 403
 404         pch = strrchr(name, '_');
 405         if (!pch)
 406                 return -EINVAL;
 407
 408         mod = *(pch + 1);
 409         switch (mod) {
 410         case 'X':
 411                 *modifier = IIO_MOD_X;
 412                 return 0;
 413         case 'Y':
 414                 *modifier = IIO_MOD_Y;
 415                 return 0;
 416         case 'Z':
 417                 *modifier = IIO_MOD_Z;
 418                 return 0;
 419         default:
 420                 return -EINVAL;
 421         }
 422 }
 423
 424 static int scmi_iio_get_chan_type(u8 scmi_type, enum iio_chan_type *iio_type)
 425 {
 426         switch (scmi_type) {
 427         case METERS_SEC_SQUARED:
 428                 *iio_type = IIO_ACCEL;
 429                 return 0;
 430         case RADIANS_SEC:
 431                 *iio_type = IIO_ANGL_VEL;
 432                 return 0;
 433         default:
 434                 return -EINVAL;
 435         }
 436 }
 437
 438 static u64 scmi_iio_convert_interval_to_ns(u32 val)
 439 {
 440         u64 sensor_update_interval =
 441                 SCMI_SENS_INTVL_GET_SECS(val) * NSEC_PER_SEC;
 442         u64 sensor_interval_mult;
 443         int mult;
 444
 445         mult = SCMI_SENS_INTVL_GET_EXP(val);
 446         if (mult < 0) {
 447                 sensor_interval_mult = int_pow(10, abs(mult));
 448                 do_div(sensor_update_interval, sensor_interval_mult);
 449         } else {
 450                 sensor_interval_mult = int_pow(10, mult);
 451                 sensor_update_interval =
 452                         sensor_update_interval * sensor_interval_mult;
 453         }
 454         return sensor_update_interval;
 455 }
 456
 457 static int scmi_iio_set_sampling_freq_avail(struct iio_dev *iio_dev)
 458 {
 459         u64 cur_interval_ns, low_interval_ns, high_interval_ns, step_size_ns,
 460                 hz, uhz;
 461         unsigned int cur_interval, low_interval, high_interval, step_size;
 462         struct scmi_iio_priv *sensor = iio_priv(iio_dev);
 463         int i;
 464
 465         sensor->freq_avail =
 466                 devm_kzalloc(&iio_dev->dev,
 467                              sizeof(*sensor->freq_avail) *
 468                                      (sensor->sensor_info->intervals.count * 2),
 469                              GFP_KERNEL);
 470         if (!sensor->freq_avail)
 471                 return -ENOMEM;
 472
 473         if (sensor->sensor_info->intervals.segmented) {
 474                 low_interval = sensor->sensor_info->intervals
 475                                        .desc[SCMI_SENS_INTVL_SEGMENT_LOW];
 476                 low_interval_ns = scmi_iio_convert_interval_to_ns(low_interval);
 477                 convert_ns_to_freq(low_interval_ns, &hz, &uhz);
 478                 sensor->freq_avail[0] = hz;
 479                 sensor->freq_avail[1] = uhz;
 480
 481                 step_size = sensor->sensor_info->intervals
 482                                     .desc[SCMI_SENS_INTVL_SEGMENT_STEP];
 483                 step_size_ns = scmi_iio_convert_interval_to_ns(step_size);
 484                 convert_ns_to_freq(step_size_ns, &hz, &uhz);
 485                 sensor->freq_avail[2] = hz;
 486                 sensor->freq_avail[3] = uhz;
 487
 488                 high_interval = sensor->sensor_info->intervals
 489                                         .desc[SCMI_SENS_INTVL_SEGMENT_HIGH];
 490                 high_interval_ns =
 491                         scmi_iio_convert_interval_to_ns(high_interval);
 492                 convert_ns_to_freq(high_interval_ns, &hz, &uhz);
 493                 sensor->freq_avail[4] = hz;
 494                 sensor->freq_avail[5] = uhz;
 495         } else {
 496                 for (i = 0; i < sensor->sensor_info->intervals.count; i++) {
 497                         cur_interval = sensor->sensor_info->intervals.desc[i];
 498                         cur_interval_ns =
 499                                 scmi_iio_convert_interval_to_ns(cur_interval);
 500                         convert_ns_to_freq(cur_interval_ns, &hz, &uhz);
 501                         sensor->freq_avail[i * 2] = hz;
 502                         sensor->freq_avail[i * 2 + 1] = uhz;
 503                 }
 504         }
 505         return 0;
 506 }
 507
 508 static struct iio_dev *
 509 scmi_alloc_iiodev(struct scmi_device *sdev,
 510                   const struct scmi_sensor_proto_ops *ops,
 511                   struct scmi_protocol_handle *ph,
 512                   const struct scmi_sensor_info *sensor_info)
 513 {
 514         struct iio_chan_spec *iio_channels;
 515         struct scmi_iio_priv *sensor;
 516         enum iio_modifier modifier;
 517         enum iio_chan_type type;
 518         struct iio_dev *iiodev;
 519         struct device *dev = &sdev->dev;
 520         const struct scmi_handle *handle = sdev->handle;
 521         int i, ret;
 522
 523         iiodev = devm_iio_device_alloc(dev, sizeof(*sensor));
 524         if (!iiodev)
 525                 return ERR_PTR(-ENOMEM);
 526
 527         iiodev->modes = INDIO_DIRECT_MODE;
 528         sensor = iio_priv(iiodev);
 529         sensor->sensor_ops = ops;
 530         sensor->ph = ph;
 531         sensor->sensor_info = sensor_info;
 532         sensor->sensor_update_nb.notifier_call = scmi_iio_sensor_update_cb;
 533         sensor->indio_dev = iiodev;
 534
 535         /* adding one additional channel for timestamp */
 536         iiodev->num_channels = sensor_info->num_axis + 1;
 537         iiodev->name = sensor_info->name;
 538         iiodev->info = &scmi_iio_info;
 539
 540         iio_channels =
 541                 devm_kzalloc(dev,
 542                              sizeof(*iio_channels) * (iiodev->num_channels),
 543                              GFP_KERNEL);
 544         if (!iio_channels)
 545                 return ERR_PTR(-ENOMEM);
 546
 547         ret = scmi_iio_set_sampling_freq_avail(iiodev);
 548         if (ret < 0)
 549                 return ERR_PTR(ret);
 550
 551         for (i = 0; i < sensor_info->num_axis; i++) {
 552                 ret = scmi_iio_get_chan_type(sensor_info->axis[i].type, &type);
 553                 if (ret < 0)
 554                         return ERR_PTR(ret);
 555
 556                 ret = scmi_iio_get_chan_modifier(sensor_info->axis[i].name,
 557                                                  &modifier);
 558                 if (ret < 0)
 559                         return ERR_PTR(ret);
 560
 561                 scmi_iio_set_data_channel(&iio_channels[i], type, modifier,
 562                                           sensor_info->axis[i].id);
 563         }
 564
 565         ret = handle->notify_ops->devm_event_notifier_register(sdev,
 566                                 SCMI_PROTOCOL_SENSOR, SCMI_EVENT_SENSOR_UPDATE,
 567                                 &sensor->sensor_info->id,
 568                                 &sensor->sensor_update_nb);
 569         if (ret) {
 570                 dev_err(&iiodev->dev,
 571                         "Error in registering sensor update notifier for sensor %s err %d",
 572                         sensor->sensor_info->name, ret);
 573                 return ERR_PTR(ret);
 574         }
 575
 576         scmi_iio_set_timestamp_channel(&iio_channels[i], i);
 577         iiodev->channels = iio_channels;
 578         return iiodev;
 579 }
 580
 581 static int scmi_iio_dev_probe(struct scmi_device *sdev)
 582 {
 583         const struct scmi_sensor_info *sensor_info;
 584         struct scmi_handle *handle = sdev->handle;
 585         const struct scmi_sensor_proto_ops *sensor_ops;
 586         struct scmi_protocol_handle *ph;
 587         struct device *dev = &sdev->dev;
 588         struct iio_dev *scmi_iio_dev;
 589         u16 nr_sensors;
 590         int err = -ENODEV, i;
 591
 592         if (!handle)
 593                 return -ENODEV;
 594
 595         sensor_ops = handle->devm_protocol_get(sdev, SCMI_PROTOCOL_SENSOR, &ph);
 596         if (IS_ERR(sensor_ops)) {
 597                 dev_err(dev, "SCMI device has no sensor interface\n");
 598                 return PTR_ERR(sensor_ops);
 599         }
 600
 601         nr_sensors = sensor_ops->count_get(ph);
 602         if (!nr_sensors) {
 603                 dev_dbg(dev, "0 sensors found via SCMI bus\n");
 604                 return -ENODEV;
 605         }
 606
 607         for (i = 0; i < nr_sensors; i++) {
 608                 sensor_info = sensor_ops->info_get(ph, i);
 609                 if (!sensor_info) {
 610                         dev_err(dev, "SCMI sensor %d has missing info\n", i);
 611                         return -EINVAL;
 612                 }
 613
 614                 /* This driver only supports 3-axis accel and gyro, skipping other sensors */
 615                 if (sensor_info->num_axis != SCMI_IIO_NUM_OF_AXIS)
 616                         continue;
 617
 618                 /* This driver only supports 3-axis accel and gyro, skipping other sensors */
 619                 if (sensor_info->axis[0].type != METERS_SEC_SQUARED &&
 620                     sensor_info->axis[0].type != RADIANS_SEC)
 621                         continue;
 622
 623                 scmi_iio_dev = scmi_alloc_iiodev(sdev, sensor_ops, ph,
 624                                                  sensor_info);
 625                 if (IS_ERR(scmi_iio_dev)) {
 626                         dev_err(dev,
 627                                 "failed to allocate IIO device for sensor %s: %ld\n",
 628                                 sensor_info->name, PTR_ERR(scmi_iio_dev));
 629                         return PTR_ERR(scmi_iio_dev);
 630                 }
 631
 632                 err = devm_iio_kfifo_buffer_setup(&scmi_iio_dev->dev,
 633                                                   scmi_iio_dev,
 634                                                   INDIO_BUFFER_SOFTWARE,
 635                                                   &scmi_iio_buffer_ops);
 636                 if (err < 0) {
 637                         dev_err(dev,
 638                                 "IIO buffer setup error at sensor %s: %d\n",
 639                                 sensor_info->name, err);
 640                         return err;
 641                 }
 642
 643                 err = devm_iio_device_register(dev, scmi_iio_dev);
 644                 if (err) {
 645                         dev_err(dev,
 646                                 "IIO device registration failed at sensor %s: %d\n",
 647                                 sensor_info->name, err);
 648                         return err;
 649                 }
 650         }
 651         return err;
 652 }
 653
 654 static const struct scmi_device_id scmi_id_table[] = {
 655         { SCMI_PROTOCOL_SENSOR, "iiodev" },
 656         {},
 657 };
 658
 659 MODULE_DEVICE_TABLE(scmi, scmi_id_table);
 660
 661 static struct scmi_driver scmi_iiodev_driver = {
 662         .name = "scmi-sensor-iiodev",
 663         .probe = scmi_iio_dev_probe,
 664         .id_table = scmi_id_table,
 665 };
 666
 667 module_scmi_driver(scmi_iiodev_driver);
 668
 669 MODULE_AUTHOR("Jyoti Bhayana <jbhayana@google.com>");
 670 MODULE_DESCRIPTION("SCMI IIO Driver");
 671 MODULE_LICENSE("GPL v2");