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/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");