diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c index dbc5e16fbde4..76f91696f66a 100644 --- a/drivers/iio/common/st_sensors/st_sensors_core.c +++ b/drivers/iio/common/st_sensors/st_sensors_core.c @@ -498,6 +498,7 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev, u8 *outdata; struct st_sensor_data *sdata = iio_priv(indio_dev); unsigned int byte_for_channel; + u32 tmp; byte_for_channel = DIV_ROUND_UP(ch->scan_type.realbits + ch->scan_type.shift, 8); @@ -508,12 +509,22 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev, if (err < 0) return err; - if (byte_for_channel == 1) - *data = (s8)*outdata; - else if (byte_for_channel == 2) - *data = (s16)get_unaligned_le16(outdata); - else if (byte_for_channel == 3) - *data = (s32)sign_extend32(get_unaligned_le24(outdata), 23); + if (byte_for_channel == 1) { + tmp = *outdata; + } else if (byte_for_channel == 2) { + if (ch->scan_type.endianness == IIO_BE) + tmp = get_unaligned_be16(outdata); + else + tmp = get_unaligned_le16(outdata); + } else if (byte_for_channel == 3) { + if (ch->scan_type.endianness == IIO_BE) + tmp = get_unaligned_be24(outdata); + else + tmp = get_unaligned_le24(outdata); + } else { + return -EINVAL; + } + *data = sign_extend32(tmp, BYTES_TO_BITS(byte_for_channel) - 1); return 0; }