TLDR: One bug and some nits.
On Thu, Jun 16, 2022 at 12:00:06PM +0200, Jacopo Mondi wrote:
> +static irqreturn_t bno08x_dump_cargo(int irq, void *dev_id)
> +{
> + struct iio_dev *iio_dev = dev_id;
> + struct bno08x_dev *bno08x = iio_priv(iio_dev);
> + struct i2c_client *client = bno08x->client;
> + u8 *cargo = bno08x->cargo.buffer;
> + size_t to_read;
> + size_t len;
> + int ret;
> +
> + mutex_lock(&bno08x->cargo.mutex);
> +
> + /* Read only the header first to know how many bytes we expect to receive. */
> + ret = i2c_master_recv(client, bno08x->cargo.buffer, BNO08x_SHTP_HDR_SIZE);
> + if (ret != BNO08x_SHTP_HDR_SIZE)
> + goto out_unlock;
> +
> + /*
> + * Clear the top bit: it means a cargo is a continuation of the last one.
> + * Ignore it for now.
> + */
> + len = (cargo[BNO08x_SHTP_HDR_LEN_MSB] << 8 |
> + cargo[BNO08x_SHTP_HDR_LEN_LSB]) & ~BIT(15);
> +
> + if (len == 0)
> + goto out_complete;
> +
> + if (len > BNO08x_CARGO_BUFFER_SIZE)
> + dev_warn(&client->dev,
> + "Cargo size exceeds buffer: content will be unusable\n");
> +
> + /*
> + * Read the full cargo now that we know its length.
> + *
> + * If the reported length exceeds the max transfer size, read the cargo
> + * in chunks. Its content will be unusable though.
> + */
> + to_read = len;
> + len = min(len, BNO08x_CARGO_BUFFER_SIZE);
> + while (to_read) {
This exit condition doesn't work. The min() needs to be done inside the
loop. Otherise if len is set to BNO08x_CARGO_BUFFER_SIZE + 1 then the
second iteration should receive 1 byte but it instead recieves
BNO08x_CARGO_BUFFER_SIZE bytes. The to_read will go into negative
numbers and keep looping until something breaks.
> + memset(bno08x->cargo.buffer, 0, len);
> +
> + ret = i2c_master_recv(client, bno08x->cargo.buffer, len);
> + if (ret != len) {
> + dev_err(&client->dev,
> + "Failed to read cargo of size %lu: %d\n",
> + len, ret);
> + goto out_complete;
> + }
> +
> + to_read -= len;
> + }
> +
> + bno08x->cargo.len = len;
This should be been the original len instead of min() value.
> +
> +out_complete:
> + if (atomic_read(&bno08x->cargo.waiters) == 0)
> + goto out_unlock;
> +
> + /*
> + * Wake up all waiters and let them read the cargo buffer.
> + *
> + * Unlock the cargo mutex only after all waiters are done, to avoid
> + * this interrupt handler re-writing the buffer content with the next
> + * cargo before waiters have read it.
> + */
> + complete_all(&bno08x->cargo_ready);
> +
> + wait_for_completion(&bno08x->waiters_done);
> + reinit_completion(&bno08x->waiters_done);
> +
> +out_unlock:
> + mutex_unlock(&bno08x->cargo.mutex);
> +
> + return IRQ_HANDLED;
> +}
> +
> +static int bno08x_write_cargo(struct bno08x_dev *bno08x,
> + enum bno08x_shtp_channels channel,
> + u8 *cargo, u16 length)
> +{
> + u16 cargo_length = length + BNO08x_SHTP_HDR_SIZE;
> + struct i2c_client *client = bno08x->client;
> + int ret;
> +
> + /* Assemble header */
> + cargo[BNO08x_SHTP_HDR_LEN_LSB] = cargo_length & 0xff;
> + cargo[BNO08x_SHTP_HDR_LEN_MSB] = cargo_length >> 8;
> + cargo[BNO08x_SHTP_HDR_CHAN] = channel;
> + cargo[BNO08x_SHTP_HDR_SEQ] = bno08x->seq_num[channel]++;
> +
> + ret = i2c_master_send(client, cargo, cargo_length);
> + if (ret < 0) {
> + dev_err(&client->dev, "Failed to write cargo to channel %u: %d\n",
> + channel, ret);
> + return ret;
> + }
> +
> + return 0;
> +}
> +
> +static int bno08x_wait_for_cargo_timeout(struct bno08x_dev *bno08x, u8 *cargo,
> + size_t len)
> +{
> + static const unsigned long complete_timeout = 500; /* msecs */
> + struct i2c_client *client = bno08x->client;
> + int ret = 0;
> +
> + atomic_inc(&bno08x->cargo.waiters);
> +
> + if (!wait_for_completion_timeout(&bno08x->cargo_ready,
> + msecs_to_jiffies(complete_timeout))) {
> + dev_dbg(&client->dev, "Wait for chip interrupt timeout.\n");
> + ret = -EIO;
> + goto out_unlock;
> + }
> +
> + /* The caller is not interested in the data. */
> + if (!len)
> + goto out_unlock;
> +
> + /*
> + * Multiple readers might want to inspect the cargo content in response
> + * to the complete_all(cargo_ready) signal.
> + *
> + * Copy data into the caller buffer to allow multiple threads to
> + * access the cargo content independently.
> + */
> + ret = min(len, bno08x->cargo.len);
> + memcpy(cargo, bno08x->cargo.buffer, ret);
> +
> +out_unlock:
> + /* The last waiter unlocks the cargo read routine. */
> + if (atomic_dec_return(&bno08x->cargo.waiters) == 0) {
> + reinit_completion(&bno08x->cargo_ready);
> + complete(&bno08x->waiters_done);
> + }
> +
> + return ret;
> +}
> +
> +static const struct iio_chan_spec bno08x_iio_channels[] = {
> + {
> + .type = IIO_ROT,
> + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
> + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),
> + .scan_index = BNO08x_ROT_SCAN_INDEX,
> + .scan_type = {
> + .sign = 'u',
> + .realbits = 16,
> + .storagebits = 16,
> + .shift = 0,
> + .endianness = IIO_LE,
> + },
> + },
> +};
> +
> +static int bno08x_enable_feature_report(struct bno08x_dev *bno08x, u8 report_id)
> +{
> + struct i2c_client *client = bno08x->client;
> + /* Reporting interval hardcoded to 5 msec. */
> + unsigned int report_interval_usec = 5000;
> + u8 cargo[BNO08x_SHTP_MAX_SIZE] = {};
> + u8 *data = cargo + BNO08x_SHTP_HDR_SIZE;
> + unsigned int max_attempts = 50;
> + int ret;
> +
> + if (bno08x->enabled_reports_mask & BIT(report_id))
> + return 0;
> +
> + /*
> + * Enable reporting the requested feature. The full "feature set
> + * request" package size is 17 bytes.
> + */
> + data[0] = BNO08x_SHTP_SET_FEATURE_CMD;
> + data[1] = report_id;
> +
> + data[5] = (report_interval_usec >> 0);
> + data[6] = (report_interval_usec >> 8);
> + data[7] = (report_interval_usec >> 16);
> + data[8] = (report_interval_usec >> 24);
> +
> + ret = bno08x_write_cargo(bno08x, BNO08x_SHTP_CONTROL_CHAN, cargo, 17);
> + if (ret)
> + return ret;
> +
> + /*
> + * Verify the requested feature is enabled inspecting the
> + * 'feature request status" response cargo.
> + */
> + memset(cargo, 0, BNO08x_SHTP_MAX_SIZE);
> + data[0] = BNO08x_SHTP_GET_FEATURE_REQ;
> + data[1] = report_id;
> +
> + ret = bno08x_write_cargo(bno08x, BNO08x_SHTP_CONTROL_CHAN, cargo, 2);
> + if (ret)
> + return ret;
> +
> + /*
> + * Wait for a "get feature response". The datasheet says it will "arrive
> + * some time in the future". Read up to 10 packets then give up.
> + */
> + while (--max_attempts) {
> + memset(cargo, 0, 5);
> + ret = bno08x_wait_for_cargo_timeout(bno08x, cargo, 5);
> + if (ret < 0)
> + continue;
> +
> + if (ret < 5)
> + continue;
> +
> + if (data[0] == BNO08x_SHTP_GET_FEATURE_RESP ||
> + data[1] == report_id)
> + break;
> + }
> + if (!max_attempts) {
> + dev_err(&client->dev,
> + "Failed to parse GET_FEATURE_RESPONSE: bad header\n");
> + return -EINVAL;
> + }
> +
> + bno08x->enabled_reports_mask |= BIT(report_id);
> + dev_dbg(&client->dev, "Reporting of feature %x enabled!\n", report_id);
> +
> + return 0;
> +}
> +
> +static int bno08x_read_raw_rotation(struct bno08x_dev *bno08x, int *vals,
> + int *val_len, int max_len)
> +{
> + struct i2c_client *client = bno08x->client;
> + unsigned int max_attempts = 50;
> + u8 cargo[24];
> + int ret;
> +
> + if (max_len < 3)
> + return -EINVAL;
> +
> + ret = bno08x_enable_feature_report(bno08x, BNO08x_REPORTID_ROTATION_VEC);
> + if (ret)
> + return ret;
> +
> + /*
> + * Read the rotation vector in I, J and K quaternions.
> + *
> + * We're going to receive several other cargos before an actual rotation
> + * vector input report, so we parse the cargo fields and only proceed
> + * with a cargo that refers to the rotation vector report id.
> + *
> + * Use a sentinel to avoid looping forever, unfortunately we can't
> + * really know how many cargos we have to discard before receiving the
> + * 'right' one.
> + */
> + while (--max_attempts) {
> + ret = bno08x_wait_for_cargo_timeout(bno08x, cargo, 24);
> + if (ret < 0)
> + continue;
> +
> + if (ret != 24)
> + continue;
> +
> + /*
> + * Parse the cargo content to make sure it's a well-formed input
> + * report containing rotation vector information.
> + *
> + * The layout of an input report cargo is reported by Figure 5.2
> + * of "BNO080/85/86 Datasheet 1000-3927 v.1.11".
> + *
> + * The rotation vector input report is 24 bytes long.
> + */
> + if (cargo[2] == BNO08x_SHTP_REPORTS_CHAN &&
> + cargo[4] == BNO08x_REPORTID_TIMESTAMP_BASE &&
> + cargo[9] == BNO08x_REPORTID_ROTATION_VEC)
> + break;
> + }
> + if (!max_attempts) {
> + dev_err(&client->dev, "Failed to receive the correct input report\n");
> + return -EINVAL;
> + }
> +
> + /* Read the I, J, K quaternions. */
> + vals[0] = cargo[13] | (cargo[14] << 8);
> + vals[1] = cargo[15] | (cargo[16] << 8);
> + vals[2] = cargo[17] | (cargo[18] << 8);
> + *val_len = 3;
> +
> + return IIO_VAL_INT_MULTIPLE;
> +}
> +
> +static int bno08x_read_raw_multi(struct iio_dev *indio_dev,
> + struct iio_chan_spec const *chan, int max_len,
> + int *vals, int *val_len, long mask)
> +{
> + struct bno08x_dev *bno08x = iio_priv(indio_dev);
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_RAW:
> + return bno08x_read_raw_rotation(bno08x, vals, val_len, max_len);
> +
> + case IIO_CHAN_INFO_SCALE:
> + vals[0] = 0;
> + return IIO_VAL_INT;
> +
> + default:
> + return -EINVAL;
> + }
> +
> + return 0;
> +}
> +
> +static const struct iio_info bno08x_iio_info = {
> + .read_raw_multi = bno08x_read_raw_multi,
> +};
> +
> +static int bno08x_data_rdy_trigger_set_state(struct iio_trigger *trig,
> + bool enable)
> +{
> + struct iio_dev *iio_dev = iio_trigger_get_drvdata(trig);
> + struct bno08x_dev *bno08x = iio_priv(iio_dev);
> + int ret;
> +
> + if (!enable)
> + return 0;
> +
> + ret = bno08x_enable_feature_report(bno08x, BNO08x_REPORTID_ROTATION_VEC);
> + if (ret)
> + return ret;
> +
> + return 0;
> +}
> +
> +static const struct iio_trigger_ops bno08x_trigger_ops = {
> + .set_trigger_state = &bno08x_data_rdy_trigger_set_state,
> +};
> +
> +static irqreturn_t bno08x_trigger_handler(int irq, void *p)
> +{
> + struct iio_poll_func *pf = p;
> + struct iio_dev *iio_dev = pf->indio_dev;
> + struct bno08x_dev *bno08x = iio_priv(iio_dev);
> + u8 cargo[BNO08x_CARGO_BUFFER_SIZE];
> + int ret;
> +
> + ret = bno08x_wait_for_cargo_timeout(bno08x, cargo, BNO08x_CARGO_BUFFER_SIZE);
> + if (ret < 0)
> + goto done;
> +
> + if (ret < 24)
> + goto done;
> +
> + /* Make sure the cargo matches the active scan channel. */
> + if (*iio_dev->active_scan_mask & BIT(BNO08x_ROT_SCAN_INDEX) &&
> + cargo[2] == BNO08x_SHTP_REPORTS_CHAN &&
> + cargo[4] == BNO08x_REPORTID_TIMESTAMP_BASE &&
> + cargo[9] == BNO08x_REPORTID_ROTATION_VEC) {
> + u16 data;
> +
> + /* Unit quaternion I component. */
> + data = le16_to_cpu(cargo[13]);
> + iio_push_to_buffers_with_timestamp(iio_dev, &data, pf->timestamp);
> +
> + /* Unit quaternion J component. */
> + data = le16_to_cpu(cargo[15]);
> + iio_push_to_buffers_with_timestamp(iio_dev, &data, pf->timestamp);
> +
> + /* Unit quaternion K component. */
> + data = le16_to_cpu(cargo[17]);
> + iio_push_to_buffers_with_timestamp(iio_dev, &data, pf->timestamp);
> + }
> +
> +done:
> + iio_trigger_notify_done(iio_dev->trig);
> +
> + return IRQ_HANDLED;
> +}
> +
> +static int bno08x_check_prod_id(struct bno08x_dev *bno08x)
> +{
> + struct i2c_client *client = bno08x->client;
> + u8 cargo[BNO08x_SHTP_MAX_SIZE] = {};
> + u8 *data = cargo + BNO08x_SHTP_HDR_SIZE;
> + int ret;
> +
> + data[0] = BNO08x_SHTP_PROD_ID_REQ;
> + ret = bno08x_write_cargo(bno08x, BNO08x_SHTP_CONTROL_CHAN, cargo, 2);
> + if (ret)
> + return ret;
> +
> + while (true) {
> + ret = bno08x_wait_for_cargo_timeout(bno08x, cargo, 20);
> + if (ret < 0) {
> + dev_dbg(&client->dev, "Failed to read PROD_ID: %d\n", ret);
> + return ret;
> + }
> +
> + if (ret >= 20 && data[0] == BNO08x_SHTP_PROD_ID)
Instead of >= 20 it's clearer to say == 20.
> + break;
> + }
> +
> + dev_dbg(&client->dev, "FW version: 0x%x.%x\n", data[2], data[3]);
> +
> + return 0;
> +}
> +
> +static int bno08x_soft_reset(struct bno08x_dev *bno08x)
> +{
> + struct i2c_client *client = bno08x->client;
> + u8 cargo[20] = {};
> + int ret;
> +
> + /* Soft reset the device: write 0x01 to EXECUTABLE channel. */
> + cargo[BNO08x_SHTP_HDR_SIZE] = BNO08x_SHTP_SOFT_RESET;
> + ret = bno08x_write_cargo(bno08x, BNO08x_SHTP_EXECTUABLE_CHAN, cargo, 1);
> + if (ret) {
> + dev_err(&client->dev, "Failed to soft-reset BNO08x\n");
> + return ret;
> + }
> +
> + /*
> + * Now read the all the channel advertisments packets.
> + * We don't care about the content.
> + */
> + do {
> + ret = bno08x_wait_for_cargo_timeout(bno08x, cargo, 0);
> + if (ret < 0) {
> + dev_dbg(&client->dev,
> + "Failed to read channel advertisments: %d\n", ret);
> + return ret;
> + }
> + } while (ret != 0);
This condition can never be true so the loop can be deleted.
> +
> + /*
> + * Give the chip some time to stabilize. There's no mention of any
> + * delay required after startup in the manual, but this makes operation
> + * stable through module load/unload.
> + */
> + usleep_range(15000, 20000);
> +
> + return 0;
> +}
> +
> +static int bno08x_probe(struct i2c_client *client)
> +{
> + struct bno08x_dev *bno08x;
> + struct iio_dev *iio_dev;
> + int ret;
> +
> + iio_dev = devm_iio_device_alloc(&client->dev, sizeof(*bno08x));
> + if (!iio_dev)
> + return -ENOMEM;
> +
> + dev_set_drvdata(&client->dev, iio_dev);
> +
> + bno08x = iio_priv(iio_dev);
> + bno08x->client = client;
> +
> + mutex_init(&bno08x->cargo.mutex);
> + init_completion(&bno08x->cargo_ready);
> + init_completion(&bno08x->waiters_done);
> +
> + iio_dev->info = &bno08x_iio_info;
> + iio_dev->name = DRIVER_NAME;
> + iio_dev->channels = bno08x_iio_channels;
> + iio_dev->num_channels = ARRAY_SIZE(bno08x_iio_channels);
> + iio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_TRIGGERED;
> +
> + ret = devm_iio_triggered_buffer_setup(&client->dev, iio_dev,
> + iio_pollfunc_store_time,
> + bno08x_trigger_handler, NULL);
> + if (ret)
> + return ret;
> +
> + bno08x->trig = devm_iio_trigger_alloc(&client->dev, "%s-dev%d",
> + DRIVER_NAME,
> + iio_device_id(iio_dev));
> + if (!bno08x->trig)
> + return -ENOMEM;
> +
> + if (client->irq > 0) {
> + ret = devm_request_threaded_irq(&client->dev, client->irq,
> + bno08x_interrupt,
> + bno08x_dump_cargo,
> + IRQF_TRIGGER_FALLING,
> + iio_dev->name, iio_dev);
> + if (ret)
> + return ret;
> + }
> +
> + bno08x->trig->dev.parent = &client->dev;
> + bno08x->trig->ops = &bno08x_trigger_ops;
> + iio_trigger_set_drvdata(bno08x->trig, iio_dev);
> +
> + ret = devm_iio_trigger_register(&client->dev, bno08x->trig);
> + if (ret)
> + return ret;
> +
> + iio_dev->trig = iio_trigger_get(bno08x->trig);
> +
> + ret = bno08x_soft_reset(bno08x);
> + if (ret)
> + return ret;
> +
> + bno08x_check_prod_id(bno08x);
Check the return value from this?
> +
> + return devm_iio_device_register(&client->dev, iio_dev);
> +}
regards,
dan carpenter
