diff options
Diffstat (limited to 'drivers/iio/common/scmi_sensors/scmi_iio.c')
| -rw-r--r-- | drivers/iio/common/scmi_sensors/scmi_iio.c | 144 |
1 files changed, 97 insertions, 47 deletions
diff --git a/drivers/iio/common/scmi_sensors/scmi_iio.c b/drivers/iio/common/scmi_sensors/scmi_iio.c index 7cf2bf282cef..5136ad9ada04 100644 --- a/drivers/iio/common/scmi_sensors/scmi_iio.c +++ b/drivers/iio/common/scmi_sensors/scmi_iio.c @@ -15,9 +15,11 @@ #include <linux/kernel.h> #include <linux/kthread.h> #include <linux/module.h> +#include <linux/mutex.h> #include <linux/scmi_protocol.h> #include <linux/time.h> #include <linux/types.h> +#include <linux/units.h> #define SCMI_IIO_NUM_OF_AXIS 3 @@ -26,6 +28,8 @@ struct scmi_iio_priv { struct scmi_protocol_handle *ph; const struct scmi_sensor_info *sensor_info; struct iio_dev *indio_dev; + /* lock to protect against multiple access to the device */ + struct mutex lock; /* adding one additional channel for timestamp */ s64 iio_buf[SCMI_IIO_NUM_OF_AXIS + 1]; struct notifier_block sensor_update_nb; @@ -62,10 +66,9 @@ static int scmi_iio_sensor_update_cb(struct notifier_block *nb, /* * Timestamp returned by SCMI is in seconds and is equal to * time * power-of-10 multiplier(tstamp_scale) seconds. - * Converting the timestamp to nanoseconds below. + * Converting the timestamp to nanoseconds (10⁹) below. */ - tstamp_scale = sensor->sensor_info->tstamp_scale + - const_ilog2(NSEC_PER_SEC) / const_ilog2(10); + tstamp_scale = sensor->sensor_info->tstamp_scale + 9; if (tstamp_scale < 0) { do_div(time, int_pow(10, abs(tstamp_scale))); time_ns = time; @@ -130,7 +133,6 @@ static const struct iio_buffer_setup_ops scmi_iio_buffer_ops = { static int scmi_iio_set_odr_val(struct iio_dev *iio_dev, int val, int val2) { struct scmi_iio_priv *sensor = iio_priv(iio_dev); - const unsigned long UHZ_PER_HZ = 1000000UL; u64 sec, mult, uHz, sf; u32 sensor_config; char buf[32]; @@ -145,7 +147,7 @@ static int scmi_iio_set_odr_val(struct iio_dev *iio_dev, int val, int val2) return err; } - uHz = val * UHZ_PER_HZ + val2; + uHz = val * MICROHZ_PER_HZ + val2; /* * The seconds field in the sensor interval in SCMI is 16 bits long @@ -155,11 +157,11 @@ static int scmi_iio_set_odr_val(struct iio_dev *iio_dev, int val, int val2) * To calculate the multiplier,we convert the sf into char string and * count the number of characters */ - sf = (u64)uHz * 0xFFFF; - do_div(sf, UHZ_PER_HZ); + sf = uHz * 0xFFFF; + do_div(sf, MICROHZ_PER_HZ); mult = scnprintf(buf, sizeof(buf), "%llu", sf) - 1; - sec = int_pow(10, mult) * UHZ_PER_HZ; + sec = int_pow(10, mult) * MICROHZ_PER_HZ; do_div(sec, uHz); if (sec == 0) { dev_err(&iio_dev->dev, @@ -198,13 +200,14 @@ static int scmi_iio_write_raw(struct iio_dev *iio_dev, struct iio_chan_spec const *chan, int val, int val2, long mask) { + struct scmi_iio_priv *sensor = iio_priv(iio_dev); int err; switch (mask) { case IIO_CHAN_INFO_SAMP_FREQ: - mutex_lock(&iio_dev->mlock); + mutex_lock(&sensor->lock); err = scmi_iio_set_odr_val(iio_dev, val, val2); - mutex_unlock(&iio_dev->mlock); + mutex_unlock(&sensor->lock); return err; default: return -EINVAL; @@ -279,6 +282,52 @@ static int scmi_iio_get_odr_val(struct iio_dev *iio_dev, int *val, int *val2) return 0; } +static int scmi_iio_read_channel_data(struct iio_dev *iio_dev, + struct iio_chan_spec const *ch, int *val, int *val2) +{ + struct scmi_iio_priv *sensor = iio_priv(iio_dev); + u32 sensor_config; + struct scmi_sensor_reading readings[SCMI_IIO_NUM_OF_AXIS]; + int err; + + sensor_config = FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, + SCMI_SENS_CFG_SENSOR_ENABLE); + err = sensor->sensor_ops->config_set( + sensor->ph, sensor->sensor_info->id, sensor_config); + if (err) { + dev_err(&iio_dev->dev, + "Error in enabling sensor %s err %d", + sensor->sensor_info->name, err); + return err; + } + + err = sensor->sensor_ops->reading_get_timestamped( + sensor->ph, sensor->sensor_info->id, + sensor->sensor_info->num_axis, readings); + if (err) { + dev_err(&iio_dev->dev, + "Error in reading raw attribute for sensor %s err %d", + sensor->sensor_info->name, err); + return err; + } + + sensor_config = FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK, + SCMI_SENS_CFG_SENSOR_DISABLE); + err = sensor->sensor_ops->config_set( + sensor->ph, sensor->sensor_info->id, sensor_config); + if (err) { + dev_err(&iio_dev->dev, + "Error in disabling sensor %s err %d", + sensor->sensor_info->name, err); + return err; + } + + *val = lower_32_bits(readings[ch->scan_index].value); + *val2 = upper_32_bits(readings[ch->scan_index].value); + + return IIO_VAL_INT_64; +} + static int scmi_iio_read_raw(struct iio_dev *iio_dev, struct iio_chan_spec const *ch, int *val, int *val2, long mask) @@ -300,6 +349,13 @@ static int scmi_iio_read_raw(struct iio_dev *iio_dev, case IIO_CHAN_INFO_SAMP_FREQ: ret = scmi_iio_get_odr_val(iio_dev, val, val2); return ret ? ret : IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_RAW: + if (!iio_device_claim_direct(iio_dev)) + return -EBUSY; + + ret = scmi_iio_read_channel_data(iio_dev, ch, val, val2); + iio_device_release_direct(iio_dev); + return ret; default: return -EINVAL; } @@ -342,12 +398,12 @@ static ssize_t scmi_iio_get_raw_available(struct iio_dev *iio_dev, rem = do_div(resolution, int_pow(10, abs(exponent)) ); - len = scnprintf(buf, PAGE_SIZE, + len = sysfs_emit(buf, "[%lld %llu.%llu %lld]\n", min_range, resolution, rem, max_range); } else { resolution = resolution * int_pow(10, exponent); - len = scnprintf(buf, PAGE_SIZE, "[%lld %llu %lld]\n", + len = sysfs_emit(buf, "[%lld %llu %lld]\n", min_range, resolution, max_range); } } @@ -360,7 +416,7 @@ static const struct iio_chan_spec_ext_info scmi_iio_ext_info[] = { .read = scmi_iio_get_raw_available, .shared = IIO_SHARED_BY_TYPE, }, - {}, + { } }; static void scmi_iio_set_timestamp_channel(struct iio_chan_spec *iio_chan, @@ -381,7 +437,8 @@ static void scmi_iio_set_data_channel(struct iio_chan_spec *iio_chan, iio_chan->type = type; iio_chan->modified = 1; iio_chan->channel2 = mod; - iio_chan->info_mask_separate = BIT(IIO_CHAN_INFO_SCALE); + iio_chan->info_mask_separate = + BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_RAW); iio_chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ); iio_chan->info_mask_shared_by_type_available = BIT(IIO_CHAN_INFO_SAMP_FREQ); @@ -463,9 +520,9 @@ static int scmi_iio_set_sampling_freq_avail(struct iio_dev *iio_dev) int i; sensor->freq_avail = - devm_kzalloc(&iio_dev->dev, - sizeof(*sensor->freq_avail) * - (sensor->sensor_info->intervals.count * 2), + devm_kcalloc(&iio_dev->dev, + array_size(sensor->sensor_info->intervals.count, 2), + sizeof(*sensor->freq_avail), GFP_KERNEL); if (!sensor->freq_avail) return -ENOMEM; @@ -531,6 +588,7 @@ scmi_alloc_iiodev(struct scmi_device *sdev, sensor->sensor_info = sensor_info; sensor->sensor_update_nb.notifier_call = scmi_iio_sensor_update_cb; sensor->indio_dev = iiodev; + mutex_init(&sensor->lock); /* adding one additional channel for timestamp */ iiodev->num_channels = sensor_info->num_axis + 1; @@ -538,8 +596,8 @@ scmi_alloc_iiodev(struct scmi_device *sdev, iiodev->info = &scmi_iio_info; iio_channels = - devm_kzalloc(dev, - sizeof(*iio_channels) * (iiodev->num_channels), + devm_kcalloc(dev, iiodev->num_channels, + sizeof(*iio_channels), GFP_KERNEL); if (!iio_channels) return ERR_PTR(-ENOMEM); @@ -566,12 +624,10 @@ scmi_alloc_iiodev(struct scmi_device *sdev, SCMI_PROTOCOL_SENSOR, SCMI_EVENT_SENSOR_UPDATE, &sensor->sensor_info->id, &sensor->sensor_update_nb); - if (ret) { - dev_err(&iiodev->dev, - "Error in registering sensor update notifier for sensor %s err %d", - sensor->sensor_info->name, ret); - return ERR_PTR(ret); - } + if (ret) + return dev_err_ptr_probe(&iiodev->dev, ret, + "Error in registering sensor update notifier for sensor %s\n", + sensor->sensor_info->name); scmi_iio_set_timestamp_channel(&iio_channels[i], i); iiodev->channels = iio_channels; @@ -593,10 +649,9 @@ static int scmi_iio_dev_probe(struct scmi_device *sdev) return -ENODEV; sensor_ops = handle->devm_protocol_get(sdev, SCMI_PROTOCOL_SENSOR, &ph); - if (IS_ERR(sensor_ops)) { - dev_err(dev, "SCMI device has no sensor interface\n"); - return PTR_ERR(sensor_ops); - } + if (IS_ERR(sensor_ops)) + return dev_err_probe(dev, PTR_ERR(sensor_ops), + "SCMI device has no sensor interface\n"); nr_sensors = sensor_ops->count_get(ph); if (!nr_sensors) { @@ -607,8 +662,8 @@ static int scmi_iio_dev_probe(struct scmi_device *sdev) for (i = 0; i < nr_sensors; i++) { sensor_info = sensor_ops->info_get(ph, i); if (!sensor_info) { - dev_err(dev, "SCMI sensor %d has missing info\n", i); - return -EINVAL; + return dev_err_probe(dev, -EINVAL, + "SCMI sensor %d has missing info\n", i); } /* This driver only supports 3-axis accel and gyro, skipping other sensors */ @@ -623,37 +678,32 @@ static int scmi_iio_dev_probe(struct scmi_device *sdev) scmi_iio_dev = scmi_alloc_iiodev(sdev, sensor_ops, ph, sensor_info); if (IS_ERR(scmi_iio_dev)) { - dev_err(dev, - "failed to allocate IIO device for sensor %s: %ld\n", - sensor_info->name, PTR_ERR(scmi_iio_dev)); - return PTR_ERR(scmi_iio_dev); + return dev_err_probe(dev, PTR_ERR(scmi_iio_dev), + "failed to allocate IIO device for sensor %s\n", + sensor_info->name); } err = devm_iio_kfifo_buffer_setup(&scmi_iio_dev->dev, scmi_iio_dev, - INDIO_BUFFER_SOFTWARE, &scmi_iio_buffer_ops); if (err < 0) { - dev_err(dev, - "IIO buffer setup error at sensor %s: %d\n", - sensor_info->name, err); - return err; + return dev_err_probe(dev, err, + "IIO buffer setup error at sensor %s\n", + sensor_info->name); } err = devm_iio_device_register(dev, scmi_iio_dev); - if (err) { - dev_err(dev, - "IIO device registration failed at sensor %s: %d\n", - sensor_info->name, err); - return err; - } + if (err) + return dev_err_probe(dev, err, + "IIO device registration failed at sensor %s\n", + sensor_info->name); } return err; } static const struct scmi_device_id scmi_id_table[] = { { SCMI_PROTOCOL_SENSOR, "iiodev" }, - {}, + { } }; MODULE_DEVICE_TABLE(scmi, scmi_id_table); |