Re: [PATCH 2/2] staging: iio: imu: Add CEVA BNO08x driver

[Dan Carpenter <dan.carpenter@oracle.com>]

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