Don't try to access buffer data of a channel by its scan index. Instead, calculate its offset in the buffer. This is necessary, as the scan index of a channel does not represent its position in a buffer - the buffer will contain data for enabled channels only, affecting data offsets and alignment. While at it, also fix minor style issue in probe. Reported-by: Chris Morgan <macromorgan@xxxxxxxxxxx> Closes: https://lore.kernel.org/linux-input/20220408212857.9583-1-macroalpha82@xxxxxxxxx/ Signed-off-by: Artur Rojek <contact@xxxxxxxxxxxxxx> Tested-by: Paul Cercueil <paul@xxxxxxxxxxxxxxx> --- v2: - provide new implementation for calculating channel offsets - cache the resulting offsets - fix minor style issue in probe - drop the "Fixes" tag drivers/input/joystick/adc-joystick.c | 102 +++++++++++++++++++++++--- 1 file changed, 90 insertions(+), 12 deletions(-) diff --git a/drivers/input/joystick/adc-joystick.c b/drivers/input/joystick/adc-joystick.c index c0deff5d4282..2f9f0cae8f95 100644 --- a/drivers/input/joystick/adc-joystick.c +++ b/drivers/input/joystick/adc-joystick.c @@ -10,6 +10,7 @@ #include <linux/module.h> #include <linux/platform_device.h> #include <linux/property.h> +#include <linux/sort.h> #include <asm/unaligned.h> @@ -25,6 +26,7 @@ struct adc_joystick { struct iio_cb_buffer *buffer; struct adc_joystick_axis *axes; struct iio_channel *chans; + int *offsets; int num_chans; bool polled; }; @@ -47,35 +49,38 @@ static int adc_joystick_handle(const void *data, void *private) { struct adc_joystick *joy = private; enum iio_endian endianness; - int bytes, msb, val, idx, i; - const u16 *data_u16; + int bytes, msb, val, off, i; + const u8 *chan_data; bool sign; bytes = joy->chans[0].channel->scan_type.storagebits >> 3; for (i = 0; i < joy->num_chans; ++i) { - idx = joy->chans[i].channel->scan_index; endianness = joy->chans[i].channel->scan_type.endianness; msb = joy->chans[i].channel->scan_type.realbits - 1; sign = tolower(joy->chans[i].channel->scan_type.sign) == 's'; + off = joy->offsets[i]; + + if (off < 0) + return -EINVAL; + + chan_data = (const u8 *)data + off; switch (bytes) { case 1: - val = ((const u8 *)data)[idx]; + val = *chan_data; break; case 2: - data_u16 = (const u16 *)data + idx; - /* * Data is aligned to the sample size by IIO core. * Call `get_unaligned_xe16` to hide type casting. */ if (endianness == IIO_BE) - val = get_unaligned_be16(data_u16); + val = get_unaligned_be16(chan_data); else if (endianness == IIO_LE) - val = get_unaligned_le16(data_u16); + val = get_unaligned_le16(chan_data); else /* IIO_CPU */ - val = *data_u16; + val = *(const u16 *)chan_data; break; default: return -EINVAL; @@ -94,6 +99,69 @@ static int adc_joystick_handle(const void *data, void *private) return 0; } +static int adc_joystick_si_cmp(const void *a, const void *b, const void *priv) +{ + const struct iio_channel *chans = priv; + + return chans[*(int *)a].channel->scan_index - + chans[*(int *)b].channel->scan_index; +} + +static int *adc_joystick_get_chan_offsets(struct iio_channel *chans, int count) +{ + struct iio_dev *indio_dev = chans[0].indio_dev; + const struct iio_chan_spec *ch; + int *offsets, *si_order; + int idx, i, si, length, offset = 0; + + offsets = kmalloc_array(count, sizeof(int), GFP_KERNEL); + if (!offsets) + return ERR_PTR(-ENOMEM); + + si_order = kmalloc_array(count, sizeof(int), GFP_KERNEL); + if (!si_order) { + kfree(offsets); + return ERR_PTR(-ENOMEM); + } + + for (i = 0; i < count; ++i) + si_order[i] = i; + /* Channels in buffer are ordered by scan index. Sort to match that. */ + sort_r(si_order, count, sizeof(int), adc_joystick_si_cmp, NULL, chans); + + for (i = 0; i < count; ++i) { + idx = si_order[i]; + ch = chans[idx].channel; + si = ch->scan_index; + + if (si < 0 || !test_bit(si, indio_dev->active_scan_mask)) { + offsets[idx] = -1; + continue; + } + + /* Channels sharing scan indices also share the samples. */ + if (idx > 0 && si == chans[idx - 1].channel->scan_index) { + offsets[idx] = offsets[idx - 1]; + continue; + } + + offsets[idx] = offset; + + length = ch->scan_type.storagebits / 8; + if (ch->scan_type.repeat > 1) + length *= ch->scan_type.repeat; + + /* Account for channel alignment. */ + if (offset % length) + offset += length - (offset % length); + offset += length; + } + + kfree(si_order); + + return offsets; +} + static int adc_joystick_open(struct input_dev *dev) { struct adc_joystick *joy = input_get_drvdata(dev); @@ -101,10 +169,19 @@ static int adc_joystick_open(struct input_dev *dev) int ret; ret = iio_channel_start_all_cb(joy->buffer); - if (ret) + if (ret) { dev_err(devp, "Unable to start callback buffer: %d\n", ret); + return ret; + } - return ret; + joy->offsets = adc_joystick_get_chan_offsets(joy->chans, + joy->num_chans); + if (IS_ERR(joy->offsets)) { + dev_err(devp, "Unable to allocate channel offsets\n"); + return PTR_ERR(joy->offsets); + } + + return 0; } static void adc_joystick_close(struct input_dev *dev) @@ -112,6 +189,7 @@ static void adc_joystick_close(struct input_dev *dev) struct adc_joystick *joy = input_get_drvdata(dev); iio_channel_stop_all_cb(joy->buffer); + kfree(joy->offsets); } static void adc_joystick_cleanup(void *data) @@ -269,7 +347,7 @@ static int adc_joystick_probe(struct platform_device *pdev) error = devm_add_action_or_reset(dev, adc_joystick_cleanup, joy->buffer); - if (error) { + if (error) { dev_err(dev, "Unable to add action\n"); return error; } -- 2.40.1
