Right now it is possible to only enable some of the x/y/z channels, for example you can enable accel_z without x or y but if you actually do that what you get is actually only the x channel. Fix this by reformatting the hardware sample to only include the requested channels. Signed-off-by: Crestez Dan Leonard <leonard.crestez@xxxxxxxxx> --- drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 8 +++ drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 107 ++++++++++++++++++++++++++++- 2 files changed, 112 insertions(+), 3 deletions(-) diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h index 85f9b50..ec1401d 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h @@ -152,6 +152,11 @@ struct inv_mpu6050_state { /* Value of I2C_MST_STATUS after slv4_done */ u8 slv4_done_status; #endif + +#define INV_MPU6050_MAX_SCAN_ELEMENTS 7 + unsigned int scan_offsets[INV_MPU6050_MAX_SCAN_ELEMENTS]; + unsigned int scan_lengths[INV_MPU6050_MAX_SCAN_ELEMENTS]; + bool realign_required; }; /*register and associated bit definition*/ @@ -274,6 +279,9 @@ enum inv_mpu6050_scan { INV_MPU6050_SCAN_TIMESTAMP, }; +#define INV_MPU6050_SCAN_MASK_ACCEL 0x07 +#define INV_MPU6050_SCAN_MASK_GYRO 0x38 + enum inv_mpu6050_filter_e { INV_MPU6050_FILTER_256HZ_NOLPF2 = 0, INV_MPU6050_FILTER_188HZ, diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c index 56ee1e2..49e503c 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c @@ -24,6 +24,90 @@ #include <linux/spi/spi.h> #include "inv_mpu_iio.h" +static void inv_mpu_get_scan_offsets( + struct iio_dev *indio_dev, + const unsigned long *mask, + const unsigned int masklen, + unsigned int *scan_offsets) +{ + unsigned int pos = 0; + + if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) { + scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0; + scan_offsets[INV_MPU6050_SCAN_ACCL_Y] = pos + 2; + scan_offsets[INV_MPU6050_SCAN_ACCL_Z] = pos + 4; + pos += 6; + } + if (*mask & INV_MPU6050_SCAN_MASK_GYRO) { + scan_offsets[INV_MPU6050_SCAN_GYRO_X] = pos + 0; + scan_offsets[INV_MPU6050_SCAN_GYRO_Y] = pos + 2; + scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4; + pos += 6; + } +} + +/* This is slowish but relatively common */ +static const struct iio_chan_spec * +iio_chan_by_scan_index(struct iio_dev *indio_dev, int index) +{ + int i; + + for (i = 0; i < indio_dev->num_channels; ++i) + if (indio_dev->channels[i].scan_index == index) + return &indio_dev->channels[i]; + + return NULL; +} + +static int iio_check_scan_offsets_aligned( + struct iio_dev *indio_dev, + const unsigned long *mask, + unsigned int masklen, + unsigned int *scan_offsets) +{ + int scan_index; + unsigned int pos = 0; + const struct iio_chan_spec *chan; + + for_each_set_bit(scan_index, mask, masklen) { + chan = iio_chan_by_scan_index(indio_dev, scan_index); + if (scan_offsets[scan_index] != pos) + return false; + pos = ALIGN(pos + chan->scan_type.storagebits / 8, + chan->scan_type.storagebits / 8); + } + + return true; +} + +static void iio_get_scan_lengths(struct iio_dev *indio_dev, unsigned int *scan_length) +{ + int i; + const struct iio_chan_spec *chan; + + for (i = 0; i < indio_dev->num_channels; ++i) { + chan = &indio_dev->channels[i]; + if (chan->scan_index < 0) + continue; + scan_length[chan->scan_index] = chan->scan_type.storagebits / 8; + } +} + +static void iio_realign_sample( + struct iio_dev *indio_dev, + const u8 *ibuf, u8 *obuf, + const unsigned int *scan_offset, + const unsigned int *scan_lengths) +{ + unsigned int pos = 0; + int i; + + for_each_set_bit(i, indio_dev->active_scan_mask, indio_dev->masklength) { + memcpy(&obuf[pos], &ibuf[scan_offset[i]], scan_lengths[i]); + pos = ALIGN(pos + scan_lengths[i], scan_lengths[i]); + } +} + static void inv_clear_kfifo(struct inv_mpu6050_state *st) { unsigned long flags; @@ -38,7 +122,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev) { int result; u8 d; - struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); + const unsigned long *mask = indio_dev->active_scan_mask; + unsigned int masklen = indio_dev->masklength; /* disable interrupt */ result = regmap_update_bits(st->map, st->reg->int_enable, @@ -93,6 +179,13 @@ int inv_reset_fifo(struct iio_dev *indio_dev) if (result) goto reset_fifo_fail; + /* check realign required */ + inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets); + st->realign_required = !iio_check_scan_offsets_aligned( + indio_dev, mask, masklen, st->scan_offsets); + if (st->realign_required) + iio_get_scan_lengths(indio_dev, st->scan_lengths); + return 0; reset_fifo_fail: @@ -201,8 +294,16 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) if (result == 0) timestamp = 0; - result = iio_push_to_buffers_with_timestamp(indio_dev, data, - timestamp); + if (st->realign_required) { + u8 adata[INV_MPU6050_OUTPUT_DATA_SIZE]; + iio_realign_sample(indio_dev, data, adata, + st->scan_offsets, st->scan_lengths); + result = iio_push_to_buffers_with_timestamp( + indio_dev, adata, timestamp); + } else { + result = iio_push_to_buffers_with_timestamp( + indio_dev, data, timestamp); + } if (result) goto flush_fifo; fifo_count -= bytes_per_datum; -- 2.5.5 -- To unsubscribe from this list: send the line "unsubscribe devicetree" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html