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(kt: 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(base: 10, exp: tstamp_scale); |
78 | } |
79 | } |
80 | |
81 | scmi_iio_dev = sensor->indio_dev; |
82 | iio_push_to_buffers_with_timestamp(indio_dev: scmi_iio_dev, data: sensor->iio_buf, |
83 | timestamp: 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(indio_dev: 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(indio_dev: 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(indio_dev: 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, size: sizeof(buf), fmt: "%llu" , sf) - 1; |
164 | |
165 | sec = int_pow(base: 10, exp: 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(indio_dev: 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(lock: &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(indio_dev: 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(indio_dev: 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(base: 10, abs(mult)); |
273 | do_div(sensor_update_interval, sensor_interval_mult); |
274 | } else { |
275 | sensor_interval_mult = int_pow(base: 10, exp: mult); |
276 | sensor_update_interval = |
277 | sensor_update_interval * sensor_interval_mult; |
278 | } |
279 | |
280 | convert_ns_to_freq(interval_ns: sensor_update_interval, hz: &hz, uhz: &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(indio_dev: 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(indio_dev: 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(base: 10, abs(scale)); |
346 | return IIO_VAL_FRACTIONAL; |
347 | } |
348 | *val = int_pow(base: 10, exp: 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(indio_dev: 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(indio_dev: 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(indio_dev: 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 | fmt: "[%lld %llu.%llu %lld]\n" , min_range, |
405 | resolution, rem, max_range); |
406 | } else { |
407 | resolution = resolution * int_pow(base: 10, exp: exponent); |
408 | len = sysfs_emit(buf, fmt: "[%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(base: 10, abs(mult)); |
507 | do_div(sensor_update_interval, sensor_interval_mult); |
508 | } else { |
509 | sensor_interval_mult = int_pow(base: 10, exp: 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(indio_dev: iio_dev); |
522 | int i; |
523 | |
524 | sensor->freq_avail = |
525 | devm_kzalloc(dev: &iio_dev->dev, |
526 | size: 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(val: low_interval); |
536 | convert_ns_to_freq(interval_ns: low_interval_ns, hz: &hz, uhz: &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(val: step_size); |
543 | convert_ns_to_freq(interval_ns: step_size_ns, hz: &hz, uhz: &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(val: high_interval); |
551 | convert_ns_to_freq(interval_ns: high_interval_ns, hz: &hz, uhz: &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(val: cur_interval); |
559 | convert_ns_to_freq(interval_ns: cur_interval_ns, hz: &hz, uhz: &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(parent: dev, sizeof_priv: sizeof(*sensor)); |
583 | if (!iiodev) |
584 | return ERR_PTR(error: -ENOMEM); |
585 | |
586 | iiodev->modes = INDIO_DIRECT_MODE; |
587 | sensor = iio_priv(indio_dev: 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 | size: sizeof(*iio_channels) * (iiodev->num_channels), |
603 | GFP_KERNEL); |
604 | if (!iio_channels) |
605 | return ERR_PTR(error: -ENOMEM); |
606 | |
607 | ret = scmi_iio_set_sampling_freq_avail(iio_dev: iiodev); |
608 | if (ret < 0) |
609 | return ERR_PTR(error: ret); |
610 | |
611 | for (i = 0; i < sensor_info->num_axis; i++) { |
612 | ret = scmi_iio_get_chan_type(scmi_type: sensor_info->axis[i].type, iio_type: &type); |
613 | if (ret < 0) |
614 | return ERR_PTR(error: ret); |
615 | |
616 | ret = scmi_iio_get_chan_modifier(name: sensor_info->axis[i].name, |
617 | modifier: &modifier); |
618 | if (ret < 0) |
619 | return ERR_PTR(error: ret); |
620 | |
621 | scmi_iio_set_data_channel(iio_chan: &iio_channels[i], type, mod: modifier, |
622 | scan_index: 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(error: ret); |
634 | } |
635 | |
636 | scmi_iio_set_timestamp_channel(iio_chan: &iio_channels[i], scan_index: 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(ptr: sensor_ops)) { |
657 | dev_err(dev, "SCMI device has no sensor interface\n" ); |
658 | return PTR_ERR(ptr: 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, ops: sensor_ops, ph, |
684 | sensor_info); |
685 | if (IS_ERR(ptr: 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(ptr: 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" ); |
731 | |