Re: [RFC v2 7/7] iio: inv_mpu6050: Expose channels from slave sensors
From: Jonathan Cameron
Date: Sun May 29 2016 - 12:04:04 EST
On 18/05/16 16:00, Crestez Dan Leonard wrote:
> This works by copying the iio_chan_specs from the slave device and
> republishing them as if they belonged to the MPU itself. All
> read/write_raw operations are forwarded to the other driver.
>
> The original device is still registered with linux as a normal driver
> and works normally and you can poke at it to configure stuff like sample
> rates and scaling factors.
>
> Buffer values are read from the MPU fifo, allowing a much higher
> sampling rate.
>
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@xxxxxxxxx>
Looks pretty good to me - a few comments inline.
Looking forward to the next revision!
I'm in an odd state of being amazed at how little code it took to support this and
how fiddly some corners of it really are ;)
Jonathan
p.s. I definitely need an Ack from Ge on this btw!
> ---
> .../devicetree/bindings/iio/imu/inv_mpu6050.txt | 47 ++-
> drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 387 ++++++++++++++++++++-
> drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 74 +++-
> drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 65 +++-
> drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 9 +
> 5 files changed, 556 insertions(+), 26 deletions(-)
>
> diff --git a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> index 778d076..07d572a 100644
> --- a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> +++ b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> @@ -11,8 +11,8 @@ Optional properties:
> - mount-matrix: an optional 3x3 mounting rotation matrix
> - invensense,i2c-aux-master: operate aux i2c in "master" mode (default is bypass).
>
> -The MPU has an auxiliary i2c bus for additional sensors. Devices attached this
> -way can be described as for a regular linux i2c bus.
> +It is possible to attach auxiliary sensors to the MPU and have them be handled
> +by linux. Those auxiliary sensors are described as an i2c bus.
>
> It is possible to interact with aux devices in "bypass" or "master" mode. In
> "bypass" mode the auxiliary SDA/SCL lines are connected directly to the main i2c
> @@ -25,7 +25,31 @@ In "master" mode aux devices are listed behind a "i2c@1" node with reg = <1>;
> The master and bypass modes are not supported at the same time. The
> "invensense,i2c-aux-master" property must be set to activate master mode.
> Bypass mode is generally faster but master mode also works when the MPU is
> -connected via SPI.
> +connected via SPI. Enabling master mode is required for automated external
> +readings.
> +
> +
> +It is possible to configure the MPU to automatically fetch reading for
> +devices connected on the auxiliary i2c bus without CPU intervention. When this
> +is done the driver will present the channels of the slaved devices as if they
> +belong to the MPU device itself.
> +
> +Reads and write to config values (like scaling factors) are forwarded to the
> +other driver while data reads are handled from MPU registers. The channels are
> +also available through the MPU's iio triggered buffer mechanism. This feature
> +can allow sampling up to 24 bytes from 4 slaves at a much higher rate.
> +
> +This is specified in device tree as an "invensense,external-sensors" node with
> +children indexed by slave ids 0 to 3. The child nodes identifying each external
> +sensor reading have the following properties:
> + - reg: 0 to 3 slave index
> + - invensense,addr : the I2C address to read from
> + - invensense,reg : the I2C register to read from
> + - invensense,len : read length from the device
> + - invensense,external-channels : list of slaved channels specified by scan_index.
> +
> +The sum of storagebits for external channels must equal the read length. Only
> +16bit channels are currently supported.
Interesting - so right now the core mangling would work, you just don't want
the complex available_scan_masks list, though to my mind that wouldn't be all that
difficult to autogenerate and if we let each sensor set of channel be represented
by a single letter can be written out as all the values a 7 bit binary number can
take, or 128 entries.
Just about worth the addition of a callback to provide the smallest one currently
relevant to the core.
>
> Example:
> mpu6050@68 {
> @@ -65,7 +89,8 @@ Example describing mpu9150 (which includes an ak9875 on chip):
> };
> };
>
> -Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
> +Example describing a mpu6500 via SPI with an hmc5883l on aux i2c configured for
> +automatic external readings as slave 0:
> mpu6500@0 {
> compatible = "invensense,mpu6500";
> reg = <0x0>;
> @@ -73,6 +98,8 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
> interrupt-parent = <&gpio1>;
> interrupts = <31 1>;
>
> + invensense,i2c-aux-master;
> +
> #address-cells = <1>;
> #size-cells = <0>;
> i2c@1 {
> @@ -85,4 +112,16 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
> reg = <0x1e>;
> };
> };
> +
> + invensense,external-sensors {
> + #address-cells = <1>;
> + #size-cells = <0>;
> + hmc5833l@0 {
> + reg = <0>;
> + invensense,addr = <0x1e>;
> + invensense,reg = <3>;
> + invensense,len = <6>;
> + invensense,external-channels = <0 1 2>;
> + };
> + };
> };
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index 76683b8..715b2e4 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -126,6 +126,9 @@ static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
> return true;
> if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
> return true;
> + if (reg < INV_MPU6050_REG_EXT_SENS_DATA_00 + INV_MPU6050_CNT_EXT_SENS_DATA &&
> + reg >= INV_MPU6050_REG_EXT_SENS_DATA_00)
> + return true;
> switch (reg) {
> case INV_MPU6050_REG_TEMPERATURE:
> case INV_MPU6050_REG_TEMPERATURE + 1:
> @@ -335,8 +338,24 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
> struct iio_chan_spec const *chan,
> int *val, int *val2, long mask)
> {
> - struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
Minor white space fixup like this should be in a separate patch. This is a complex
enough beast as it is, don't add padding!
> int ret = 0;
> + int chan_index;
> +
> + if (chan > indio_dev->channels + indio_dev->num_channels ||
> + chan < indio_dev->channels)
> + return -EINVAL;
> + chan_index = chan - indio_dev->channels;
> + if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
> + struct inv_mpu_ext_chan_state *ext_chan_state =
> + &st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
> + struct inv_mpu_ext_sens_state *ext_sens_state =
> + &st->ext_sens[ext_chan_state->ext_sens_index];
> + struct iio_dev *orig_dev = ext_sens_state->orig_dev;
> + const struct iio_chan_spec *orig_chan =
> + &orig_dev->channels[ext_chan_state->orig_chan_index];
> + return orig_dev->info->read_raw(orig_dev, orig_chan, val, val2, mask);
> + }
>
> switch (mask) {
> case IIO_CHAN_INFO_RAW:
> @@ -518,9 +537,25 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
> struct iio_chan_spec const *chan,
> int val, int val2, long mask)
> {
> - struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + int chan_index;
> int result;
>
> + if (chan > indio_dev->channels + indio_dev->num_channels ||
> + chan < indio_dev->channels)
> + return -EINVAL;
> + chan_index = chan - indio_dev->channels;
> + if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
> + struct inv_mpu_ext_chan_state *ext_chan_state =
> + &st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
> + struct inv_mpu_ext_sens_state *ext_sens_state =
> + &st->ext_sens[ext_chan_state->ext_sens_index];
> + struct iio_dev *orig_dev = ext_sens_state->orig_dev;
> + const struct iio_chan_spec *orig_chan =
> + &orig_dev->channels[ext_chan_state->orig_chan_index];
> + return orig_dev->info->write_raw(orig_dev, orig_chan, val, val2, mask);
> + }
> +
> mutex_lock(&indio_dev->mlock);
> /*
> * we should only update scale when the chip is disabled, i.e.
> @@ -809,6 +844,346 @@ static const struct iio_info mpu_info = {
> .validate_trigger = inv_mpu6050_validate_trigger,
> };
>
> +extern struct device_type iio_device_type;
> +
> +static int iio_device_from_i2c_client_match(struct device *dev, void *data)
> +{
> + return dev->type == &iio_device_type;
> +}
> +
> +static struct iio_dev* iio_device_from_i2c_client(struct i2c_client* i2c)
> +{
> + struct device *child;
> +
> + child = device_find_child(&i2c->dev, NULL, iio_device_from_i2c_client_match);
> +
> + return (child ? dev_to_iio_dev(child) : NULL);
> +}
> +
> +static int inv_mpu_slave_enable(struct inv_mpu6050_state *st, int index, bool state)
> +{
> + return regmap_update_bits(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
> + INV_MPU6050_BIT_I2C_SLV_EN,
> + state ? INV_MPU6050_BIT_I2C_SLV_EN : 0);
> +}
> +
> +/* Enable slaves based on scan mask */
> +int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st,
> + const unsigned long mask)
> +{
> + int i, result;
> +
> + for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> + long slave_mask = st->ext_sens[i].scan_mask;
> + result = inv_mpu_slave_enable(st, i, slave_mask & mask);
> + if (result)
> + return result;
> + }
> +
> + return 0;
> +}
> +
> +static int inv_mpu_parse_one_ext_sens(struct device *dev,
> + struct device_node *np,
> + struct inv_mpu_ext_sens_spec *spec)
> +{
> + int result;
> + u32 addr, reg, len;
> +
> + result = of_property_read_u32(np, "invensense,addr", &addr);
> + if (result)
> + return result;
> + result = of_property_read_u32(np, "invensense,reg", ®);
> + if (result)
> + return result;
> + result = of_property_read_u32(np, "invensense,len", &len);
> + if (result)
> + return result;
> +
> + spec->addr = addr;
> + spec->reg = reg;
> + spec->len = len;
> +
> + result = of_property_count_u32_elems(np, "invensense,external-channels");
> + if (result < 0)
> + return result;
> + spec->num_ext_channels = result;
> + spec->ext_channels = devm_kmalloc(dev, spec->num_ext_channels * sizeof(*spec->ext_channels), GFP_KERNEL);
> + if (!spec->ext_channels)
> + return -ENOMEM;
> + result = of_property_read_u32_array(np, "invensense,external-channels",
> + spec->ext_channels,
> + spec->num_ext_channels);
> + if (result)
> + return result;
> +
> + return 0;
> +}
> +
> +static int inv_mpu_parse_ext_sens(struct device *dev,
> + struct device_node *node,
> + struct inv_mpu_ext_sens_spec *specs)
> +{
> + struct device_node *child;
> + int result;
> + u32 reg;
> +
> + for_each_available_child_of_node(node, child) {
> + result = of_property_read_u32(child, "reg", ®);
> + if (result)
> + return result;
> + if (reg > INV_MPU6050_MAX_EXT_SENSORS) {
> + dev_err(dev, "External sensor index %u out of range, max %d\n",
> + reg, INV_MPU6050_MAX_EXT_SENSORS);
> + return -EINVAL;
> + }
> + result = inv_mpu_parse_one_ext_sens(dev, child, &specs[reg]);
> + if (result)
> + return result;
> + }
> +
> + return 0;
> +}
> +
> +static int inv_mpu_get_ext_sens_spec(struct iio_dev *indio_dev)
> +{
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> + struct device_node *node;
> + int result;
> +
> + node = of_get_child_by_name(dev->of_node, "invensense,external-sensors");
> + if (node) {
> + result = inv_mpu_parse_ext_sens(dev, node, st->ext_sens_spec);
> + if (result)
> + dev_err(dev, "Failed to parsing external-sensors devicetree data\n");
> + return result;
> + }
> +
> + return 0;
> +}
> +
> +/* Struct used while enumerating devices and matching them */
> +struct inv_mpu_handle_ext_sensor_fnarg
> +{
> + struct iio_dev *indio_dev;
> +
> + /* Current scan index */
> + int scan_index;
> + /* Current channel index */
> + int chan_index;
> + /* Non-const pointer to channels */
> + struct iio_chan_spec *channels;
> +};
> +
> +/*
> + * Write initial configuration of a slave at probe time.
> + *
> + * This is mostly fixed except for enabling/disabling individual slaves.
> + */
> +static int inv_mpu_config_external_read(struct inv_mpu6050_state *st, int index,
> + const struct inv_mpu_ext_sens_spec *spec)
> +{
> + int result;
> +
> + result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(index),
> + INV_MPU6050_BIT_I2C_SLV_RW | spec->addr);
> + if (result)
> + return result;
> + result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(index), spec->reg);
> + if (result)
> + return result;
> + result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
> + spec->len);
> + if (result)
> + return result;
> +
> + return result;
> +}
> +
> +/* Handle one device */
> +static int inv_mpu_handle_slave_device(
> + struct inv_mpu_handle_ext_sensor_fnarg *fnarg,
> + int slave_index,
> + struct iio_dev *orig_dev)
> +{
> + int i, j;
> + int data_offset;
> + struct iio_dev *indio_dev = fnarg->indio_dev;
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct device *mydev = regmap_get_device(st->map);
> + struct inv_mpu_ext_sens_spec *ext_sens_spec =
> + &st->ext_sens_spec[slave_index];
> + struct inv_mpu_ext_sens_state *ext_sens_state =
> + &st->ext_sens[slave_index];
> +
> + dev_info(mydev, "slave %d is device %s\n",
> + slave_index, orig_dev->name ?: dev_name(&orig_dev->dev));
> + ext_sens_state->orig_dev = orig_dev;
> + ext_sens_state->scan_mask = 0;
> + data_offset = 0;
> +
> + /* handle channels: */
> + for (i = 0; i < ext_sens_spec->num_ext_channels; ++i) {
> + int orig_chan_index = -1;
> + const struct iio_chan_spec *orig_chan_spec;
> + struct iio_chan_spec *chan_spec;
> + struct inv_mpu_ext_chan_state *chan_state;
> +
> + for (j = 0; j < orig_dev->num_channels; ++j)
> + if (orig_dev->channels[j].scan_index == ext_sens_spec->ext_channels[i]) {
> + orig_chan_index = j;
> + break;
> + }
> +
> + if (orig_chan_index < 0) {
> + dev_err(mydev, "Could not find slave channel with scan_index %d\n",
> + ext_sens_spec->ext_channels[i]);
> + }
> +
> + orig_chan_spec = &orig_dev->channels[orig_chan_index];
> + chan_spec = &fnarg->channels[INV_MPU6050_NUM_INT_CHAN + fnarg->chan_index];
> + chan_state = &st->ext_chan[fnarg->chan_index];
> +
> + chan_state->ext_sens_index = slave_index;
> + chan_state->orig_chan_index = orig_chan_index;
> + chan_state->data_offset = data_offset;
> + memcpy(chan_spec, orig_chan_spec, sizeof(struct iio_chan_spec));
> + chan_spec->scan_index = fnarg->scan_index;
> + ext_sens_state->scan_mask |= (1 << chan_spec->scan_index);
> +
> + fnarg->scan_index++;
> + fnarg->chan_index++;
> + data_offset += chan_spec->scan_type.storagebits / 8;
> + dev_info(mydev, "Reading external channel #%d scan_index %d data_offset %d"
> + " from original device %s chan #%d scan_index %d\n",
> + fnarg->chan_index, chan_spec->scan_index, chan_state->data_offset,
> + orig_dev->name ?: dev_name(&orig_dev->dev), orig_chan_index, orig_chan_spec->scan_index);
> + }
> + if (ext_sens_spec->len != data_offset) {
> + dev_err(mydev, "slave %d length mismatch between "
> + "i2c slave read length (%d) and "
> + "sum of channel sizes (%d)\n",
> + slave_index, ext_sens_spec->len, data_offset);
> + return -EINVAL;
> + }
> +
> + return inv_mpu_config_external_read(st, slave_index, ext_sens_spec);
> +}
> +
> +/* device_for_each_child enum function */
> +static int inv_mpu_handle_ext_sensor_fn(struct device *dev, void *data)
> +{
> + struct inv_mpu_handle_ext_sensor_fnarg *fnarg = data;
> + struct iio_dev *indio_dev = fnarg->indio_dev;
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct i2c_client *client;
> + struct iio_dev *orig_dev;
> + int i, result;
> +
> + client = i2c_verify_client(dev);
> + if (!client)
> + return 0;
> + orig_dev = iio_device_from_i2c_client(client);
> + if (!orig_dev)
> + return 0;
> +
> + for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> + if (st->ext_sens_spec[i].addr != client->addr)
> + continue;
> + if (st->ext_sens[i].orig_dev) {
> + dev_warn(&indio_dev->dev, "already found slave with at addr %d\n", client->addr);
> + continue;
> + }
> +
> + result = inv_mpu_handle_slave_device(fnarg, i, orig_dev);
> + if (result)
> + return result;
> + }
> + return 0;
> +}
> +
> +static int inv_mpu6050_handle_ext_sensors(struct iio_dev *indio_dev)
> +{
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> + struct inv_mpu_handle_ext_sensor_fnarg fnarg = {
> + .indio_dev = indio_dev,
> + .chan_index = 0,
> + .scan_index = INV_MPU6050_SCAN_TIMESTAMP,
> + };
> + int i, result;
> + int num_ext_chan = 0, sum_data_len = 0;
> + int num_scan_elements;
> +
> + inv_mpu_get_ext_sens_spec(indio_dev);
> + for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> + num_ext_chan += st->ext_sens_spec[i].num_ext_channels;
> + sum_data_len += st->ext_sens_spec[i].len;
> + }
> + if (sum_data_len > INV_MPU6050_CNT_EXT_SENS_DATA) {
> + dev_err(dev, "Too many bytes from external sensors:"
> + " requested=%d max=%d\n",
> + sum_data_len, INV_MPU6050_CNT_EXT_SENS_DATA);
> + return -EINVAL;
> + }
> +
> + /* Allocate scan_offsets/scan_lengths */
> + num_scan_elements = INV_MPU6050_NUM_INT_SCAN_ELEMENTS + num_ext_chan;
> + st->scan_offsets = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
> + if (!st->scan_offsets)
> + return -ENOMEM;
> + st->scan_lengths = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
> + if (!st->scan_lengths)
> + return -ENOMEM;
> +
> + /* Zero length means nothing to do, just publish internal channels */
> + if (!sum_data_len) {
> + indio_dev->channels = inv_mpu_channels;
> + indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN;
> + BUILD_BUG_ON(ARRAY_SIZE(inv_mpu_channels) != INV_MPU6050_NUM_INT_CHAN);
> + return 0;
> + }
> +
> + indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN + num_ext_chan;
> + indio_dev->channels = fnarg.channels = devm_kmalloc(dev,
> + indio_dev->num_channels * sizeof(struct iio_chan_spec),
> + GFP_KERNEL);
> + if (!fnarg.channels)
> + return -ENOMEM;
> + memcpy(fnarg.channels, inv_mpu_channels, sizeof(inv_mpu_channels));
> + memset(fnarg.channels + INV_MPU6050_NUM_INT_CHAN, 0,
> + num_ext_chan * sizeof(struct iio_chan_spec));
> +
> + st->ext_chan = devm_kzalloc(dev, num_ext_chan * sizeof(*st->ext_chan), GFP_KERNEL);
> + if (!st->ext_chan)
> + return -ENOMEM;
> +
> + result = inv_mpu6050_set_power_itg(st, true);
> + if (result < 0)
> + return result;
> +
> + result = device_for_each_child(&st->aux_master_adapter.dev, &fnarg,
> + inv_mpu_handle_ext_sensor_fn);
> + if (result)
> + goto out_disable;
> + /* Timestamp channel has index 0 and last scan_index */
> + fnarg.channels[0].scan_index = fnarg.scan_index;
> +
> + if (fnarg.chan_index != num_ext_chan) {
> + dev_err(&indio_dev->dev, "Failed to match all external channels!\n");
> + result = -EINVAL;
> + goto out_disable;
> + }
> +
> + result = inv_mpu6050_set_power_itg(st, false);
> + return result;
> +
> +out_disable:
> + inv_mpu6050_set_power_itg(st, false);
> + return result;
> +}
> +
> /**
> * inv_check_and_setup_chip() - check and setup chip.
> */
> @@ -1140,8 +1515,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
> indio_dev->name = name;
> else
> indio_dev->name = dev_name(dev);
> - indio_dev->channels = inv_mpu_channels;
> - indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
>
> indio_dev->info = &mpu_info;
> indio_dev->modes = INDIO_BUFFER_TRIGGERED;
> @@ -1179,6 +1552,12 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
> }
> #endif
>
> + result = inv_mpu6050_handle_ext_sensors(indio_dev);
> + if (result < 0) {
> + dev_err(dev, "failed to configure channels %d\n", result);
> + goto out_remove_trigger;
> + }
> +
> INIT_KFIFO(st->timestamps);
> spin_lock_init(&st->time_stamp_lock);
> result = iio_device_register(indio_dev);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index ec1401d..56fd943 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -110,6 +110,45 @@ struct inv_mpu6050_hw {
> const struct inv_mpu6050_chip_config *config;
> };
>
> +/* Maximum external sensors */
> +/* These are SLV0-3 in HW, SLV4 is reserved for i2c master */
> +#define INV_MPU6050_MAX_EXT_SENSORS 4
> +
> +/* Number of internal channels */
> +#define INV_MPU6050_NUM_INT_CHAN 8
> +
> +/* Number of internal scan elements (channels with scan_index >= 0) */
> +#define INV_MPU6050_NUM_INT_SCAN_ELEMENTS 7
> +
> +/* Specification for an external sensor */
> +struct inv_mpu_ext_sens_spec {
> + unsigned short addr;
> + u8 reg, len;
> +
> + int num_ext_channels;
> + int *ext_channels;
> +};
> +
> +/* Driver state for each external sensor */
> +struct inv_mpu_ext_sens_state {
> + struct iio_dev *orig_dev;
> +
> + /* Mask of all channels in this sensor */
> + unsigned long scan_mask;
> +};
> +
> +/* Driver state for each external channel */
> +struct inv_mpu_ext_chan_state {
> + /* Index inside state->ext_sens */
> + int ext_sens_index;
> +
> + /* Index inside orig_dev->channels */
> + int orig_chan_index;
> +
> + /* Relative to first offset for this slave */
> + int data_offset;
> +};
> +
> /*
> * struct inv_mpu6050_state - Driver state variables.
> * @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
> @@ -153,10 +192,13 @@ struct inv_mpu6050_state {
> 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];
> + unsigned int *scan_offsets;
> + unsigned int *scan_lengths;
> bool realign_required;
> +
> + struct inv_mpu_ext_sens_spec ext_sens_spec[INV_MPU6050_MAX_EXT_SENSORS];
> + struct inv_mpu_ext_sens_state ext_sens[INV_MPU6050_MAX_EXT_SENSORS];
> + struct inv_mpu_ext_chan_state *ext_chan;
> };
>
> /*register and associated bit definition*/
> @@ -171,6 +213,24 @@ struct inv_mpu6050_state {
> #define INV_MPU6050_REG_FIFO_EN 0x23
> #define INV_MPU6050_BIT_ACCEL_OUT 0x08
> #define INV_MPU6050_BITS_GYRO_OUT 0x70
> +#define INV_MPU6050_BIT_SLV0_FIFO_EN 0x01
> +#define INV_MPU6050_BIT_SLV1_FIFO_EN 0x02
> +#define INV_MPU6050_BIT_SLV2_FIFO_EN 0x04
> +#define INV_MPU6050_BIT_SLV2_FIFO_EN 0x04
> +
> +/* SLV3 fifo enabling is not in REG_FIFO_EN */
> +#define INV_MPU6050_REG_MST_CTRL 0x24
> +#define INV_MPU6050_BIT_SLV3_FIFO_EN 0x20
> +
> +/* For slaves 0-3 */
> +#define INV_MPU6050_REG_I2C_SLV_ADDR(i) (0x25 + 3 * (i))
> +#define INV_MPU6050_REG_I2C_SLV_REG(i) (0x26 + 3 * (i))
> +#define INV_MPU6050_REG_I2C_SLV_CTRL(i) (0x27 + 3 * (i))
> +#define INV_MPU6050_BIT_I2C_SLV_RW 0x80
> +#define INV_MPU6050_BIT_I2C_SLV_EN 0x80
> +#define INV_MPU6050_BIT_I2C_SLV_BYTE_SW 0x40
> +#define INV_MPU6050_BIT_I2C_SLV_REG_DIS 0x20
> +#define INV_MPU6050_BIT_I2C_SLV_REG_GRP 0x10
>
> #define INV_MPU6050_REG_I2C_SLV4_ADDR 0x31
> #define INV_MPU6050_BIT_I2C_SLV4_R 0x80
> @@ -247,8 +307,8 @@ struct inv_mpu6050_state {
> #define INV_MPU6050_GYRO_CONFIG_FSR_SHIFT 3
> #define INV_MPU6050_ACCL_CONFIG_FSR_SHIFT 3
>
> -/* 6 + 6 round up and plus 8 */
> -#define INV_MPU6050_OUTPUT_DATA_SIZE 24
> +/* max is 3*2 accel + 3*2 gyro + 24 external + 8 ts */
> +#define INV_MPU6050_OUTPUT_DATA_SIZE 44
>
> #define INV_MPU6050_REG_INT_PIN_CFG 0x37
> #define INV_MPU6050_BIT_BYPASS_EN 0x2
> @@ -261,6 +321,9 @@ struct inv_mpu6050_state {
> #define INV_MPU6050_MIN_FIFO_RATE 4
> #define INV_MPU6050_ONE_K_HZ 1000
>
> +#define INV_MPU6050_REG_EXT_SENS_DATA_00 0x49
> +#define INV_MPU6050_CNT_EXT_SENS_DATA 24
> +
> #define INV_MPU6050_REG_WHOAMI 117
>
> #define INV_MPU6000_WHOAMI_VALUE 0x68
> @@ -328,6 +391,7 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev);
> void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st);
> int inv_reset_fifo(struct iio_dev *indio_dev);
> int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask);
> +int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st, const unsigned long mask);
> int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
> int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
> int inv_mpu_acpi_create_mux_client(struct i2c_client *client);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 49e503c..919148a 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -30,7 +30,9 @@ static void inv_mpu_get_scan_offsets(
> const unsigned int masklen,
> unsigned int *scan_offsets)
> {
> + struct inv_mpu6050_state *st = iio_priv(indio_dev);
> unsigned int pos = 0;
> + int i, j;
>
> if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) {
> scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0;
> @@ -44,6 +46,24 @@ static void inv_mpu_get_scan_offsets(
> scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4;
> pos += 6;
> }
> + /* HW lays out channels in slave order */
> + for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> + struct inv_mpu_ext_sens_spec *ext_sens_spec;
> + struct inv_mpu_ext_sens_state *ext_sens_state;
> + ext_sens_spec = &st->ext_sens_spec[i];
> + ext_sens_state = &st->ext_sens[i];
> +
> + if (!(ext_sens_state->scan_mask & *mask))
> + continue;
> + for (j = 0; j + INV_MPU6050_NUM_INT_CHAN < indio_dev->num_channels; ++j) {
> + const struct iio_chan_spec *chan;
> + if (st->ext_chan[j].ext_sens_index != i)
> + continue;
> + chan = &indio_dev->channels[j + INV_MPU6050_NUM_INT_CHAN];
> + scan_offsets[chan->scan_index] = pos + st->ext_chan[j].data_offset;
> + }
> + pos += ext_sens_spec->len;
> + }
> }
>
> /* This is slowish but relatively common */
> @@ -138,6 +158,10 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
> result = regmap_write(st->map, st->reg->fifo_en, 0);
> if (result)
> goto reset_fifo_fail;
> + result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
> + INV_MPU6050_BIT_SLV3_FIFO_EN, 0);
> + if (result)
> + goto reset_fifo_fail;
> /* disable fifo reading */
> st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
> result = regmap_write(st->map, st->reg->user_ctrl,
> @@ -155,14 +179,12 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
> inv_clear_kfifo(st);
>
> /* enable interrupt */
> - if (st->chip_config.accl_fifo_enable ||
> - st->chip_config.gyro_fifo_enable) {
> - result = regmap_update_bits(st->map, st->reg->int_enable,
> - INV_MPU6050_BIT_DATA_RDY_EN,
> - INV_MPU6050_BIT_DATA_RDY_EN);
> - if (result)
> - return result;
> - }
> + result = regmap_update_bits(st->map, st->reg->int_enable,
> + INV_MPU6050_BIT_DATA_RDY_EN,
> + INV_MPU6050_BIT_DATA_RDY_EN);
> + if (result)
> + return result;
> +
> /* enable FIFO reading and I2C master interface*/
> st->chip_config.user_ctrl |= INV_MPU6050_BIT_FIFO_EN;
> result = regmap_write(st->map, st->reg->user_ctrl,
> @@ -175,9 +197,22 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
> d |= INV_MPU6050_BITS_GYRO_OUT;
> if (st->chip_config.accl_fifo_enable)
> d |= INV_MPU6050_BIT_ACCEL_OUT;
> + if (*indio_dev->active_scan_mask & st->ext_sens[0].scan_mask)
> + d |= INV_MPU6050_BIT_SLV0_FIFO_EN;
> + if (*indio_dev->active_scan_mask & st->ext_sens[1].scan_mask)
> + d |= INV_MPU6050_BIT_SLV1_FIFO_EN;
> + if (*indio_dev->active_scan_mask & st->ext_sens[2].scan_mask)
> + d |= INV_MPU6050_BIT_SLV2_FIFO_EN;
> result = regmap_write(st->map, st->reg->fifo_en, d);
> if (result)
> goto reset_fifo_fail;
> + if (*indio_dev->active_scan_mask & st->ext_sens[3].scan_mask) {
> + result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
> + INV_MPU6050_BIT_SLV3_FIFO_EN,
> + INV_MPU6050_BIT_SLV3_FIFO_EN);
> + if (result)
> + goto reset_fifo_fail;
> + }
>
> /* check realign required */
> inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets);
> @@ -222,8 +257,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
> struct iio_poll_func *pf = p;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_mpu6050_state *st = iio_priv(indio_dev);
> - size_t bytes_per_datum;
> + size_t bytes_per_datum = 0;
> int result;
> + int i;
> u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
> u16 fifo_count;
> s64 timestamp;
> @@ -236,15 +272,18 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
> spi = i2c ? NULL: to_spi_device(regmap_dev);
>
> mutex_lock(&indio_dev->mlock);
> - if (!(st->chip_config.accl_fifo_enable |
> - st->chip_config.gyro_fifo_enable))
> - goto end_session;
> +
> + /* Sample length */
> bytes_per_datum = 0;
> if (st->chip_config.accl_fifo_enable)
> bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
> -
> if (st->chip_config.gyro_fifo_enable)
> bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
> + for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i)
> + if (st->ext_sens[i].scan_mask & *indio_dev->active_scan_mask)
> + bytes_per_datum += st->ext_sens_spec[i].len;
> + if (!bytes_per_datum)
> + return 0;
>
> /*
> * read fifo_count register to know how many bytes inside FIFO
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index a334ed9..39e791b 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -61,6 +61,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
> if (result)
> return result;
> }
> +
> + result = inv_mpu_slave_enable_mask(st, *indio_dev->active_scan_mask);
> + if (result)
> + return result;
> +
> result = inv_reset_fifo(indio_dev);
> if (result)
> return result;
> @@ -80,6 +85,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
> if (result)
> return result;
>
> + result = inv_mpu_slave_enable_mask(st, 0);
> + if (result)
> + return result;
> +
> result = inv_mpu6050_switch_engine(st, false,
> INV_MPU6050_BIT_PWR_GYRO_STBY);
> if (result)
>