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