1
2
3
4
5
6
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
24struct 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
30 s64 iio_buf[SCMI_IIO_NUM_OF_AXIS + 1];
31 struct notifier_block sensor_update_nb;
32 u32 *freq_avail;
33};
34
35static 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
58
59
60 time = sensor_update->readings[0].timestamp;
61
62
63
64
65
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
83static 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
105static 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
125static const struct iio_buffer_setup_ops scmi_iio_buffer_ops = {
126 .preenable = scmi_iio_buffer_preenable,
127 .postdisable = scmi_iio_buffer_postdisable,
128};
129
130static 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
152
153
154
155
156
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
197static 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
214static 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
235static 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
246static 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
282static 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
308static 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
314static 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
327
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
338
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
357static 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
366static 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
377static 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
396static 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
424static 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
438static 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
457static 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
508static struct iio_dev *
509scmi_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
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
581static 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
615 if (sensor_info->num_axis != SCMI_IIO_NUM_OF_AXIS)
616 continue;
617
618
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
654static const struct scmi_device_id scmi_id_table[] = {
655 { SCMI_PROTOCOL_SENSOR, "iiodev" },
656 {},
657};
658
659MODULE_DEVICE_TABLE(scmi, scmi_id_table);
660
661static 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
667module_scmi_driver(scmi_iiodev_driver);
668
669MODULE_AUTHOR("Jyoti Bhayana <jbhayana@google.com>");
670MODULE_DESCRIPTION("SCMI IIO Driver");
671MODULE_LICENSE("GPL v2");
672
