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
 26struct 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
 39static 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
 86static 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
108static 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
128static const struct iio_buffer_setup_ops scmi_iio_buffer_ops = {
129	.preenable = scmi_iio_buffer_preenable,
130	.postdisable = scmi_iio_buffer_postdisable,
131};
132
133static 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
199static 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
217static 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
238static 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
249static 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
285static 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
331static 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
364static 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
370static 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
413static 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
422static 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
433static 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
453static 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
481static 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
495static 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
514static 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
565static struct iio_dev *
566scmi_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
637static 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
704static const struct scmi_device_id scmi_id_table[] = {
705	{ SCMI_PROTOCOL_SENSOR, "iiodev" },
706	{ }
707};
708
709MODULE_DEVICE_TABLE(scmi, scmi_id_table);
710
711static 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
717module_scmi_driver(scmi_iiodev_driver);
718
719MODULE_AUTHOR("Jyoti Bhayana <jbhayana@google.com>");
720MODULE_DESCRIPTION("SCMI IIO Driver");
721MODULE_LICENSE("GPL v2");