bno055_ser_core.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Serial line interface for Bosh BNO055 IMU (via serdev).
  4 * This file implements serial communication up to the register read/write
  5 * level.
  6 *
  7 * Copyright (C) 2021-2022 Istituto Italiano di Tecnologia
  8 * Electronic Design Laboratory
  9 * Written by Andrea Merello <andrea.merello@iit.it>
 10 *
 11 * This driver is based on
 12 *	Plantower PMS7003 particulate matter sensor driver
 13 *	Which is
 14 *	Copyright (c) Tomasz Duszynski <tduszyns@gmail.com>
 15 */
 16
 17#include <linux/completion.h>
 18#include <linux/device.h>
 19#include <linux/errno.h>
 20#include <linux/jiffies.h>
 21#include <linux/kernel.h>
 22#include <linux/mod_devicetable.h>
 23#include <linux/module.h>
 24#include <linux/mutex.h>
 25#include <linux/regmap.h>
 26#include <linux/serdev.h>
 27
 28#include "bno055_ser_trace.h"
 29#include "bno055.h"
 30
 31/*
 32 * Register writes cmd have the following format
 33 * +------+------+-----+-----+----- ... ----+
 34 * | 0xAA | 0xOO | REG | LEN | payload[LEN] |
 35 * +------+------+-----+-----+----- ... ----+
 36 *
 37 * Register write responses have the following format
 38 * +------+----------+
 39 * | 0xEE | ERROCODE |
 40 * +------+----------+
 41 *
 42 * .. except when writing the SYS_RST bit (i.e. triggering a system reset); in
 43 * case the IMU accepts the command, then it resets without responding. We don't
 44 * handle this (yet) here (so we inform the common bno055 code not to perform
 45 * sw resets - bno055 on serial bus basically requires the hw reset pin).
 46 *
 47 * Register read have the following format
 48 * +------+------+-----+-----+
 49 * | 0xAA | 0xO1 | REG | LEN |
 50 * +------+------+-----+-----+
 51 *
 52 * Successful register read response have the following format
 53 * +------+-----+----- ... ----+
 54 * | 0xBB | LEN | payload[LEN] |
 55 * +------+-----+----- ... ----+
 56 *
 57 * Failed register read response have the following format
 58 * +------+--------+
 59 * | 0xEE | ERRCODE|  (ERRCODE always > 1)
 60 * +------+--------+
 61 *
 62 * Error codes are
 63 * 01: OK
 64 * 02: read/write FAIL
 65 * 04: invalid address
 66 * 05: write on RO
 67 * 06: wrong start byte
 68 * 07: bus overrun
 69 * 08: len too high
 70 * 09: len too low
 71 * 10: bus RX byte timeout (timeout is 30mS)
 72 *
 73 *
 74 * **WORKAROUND ALERT**
 75 *
 76 * Serial communication seems very fragile: the BNO055 buffer seems to overflow
 77 * very easy; BNO055 seems able to sink few bytes, then it needs a brief pause.
 78 * On the other hand, it is also picky on timeout: if there is a pause > 30mS in
 79 * between two bytes then the transaction fails (IMU internal RX FSM resets).
 80 *
 81 * BNO055 has been seen also failing to process commands in case we send them
 82 * too close each other (or if it is somehow busy?)
 83 *
 84 * In particular I saw these scenarios:
 85 * 1) If we send 2 bytes per time, then the IMU never(?) overflows.
 86 * 2) If we send 4 bytes per time (i.e. the full header), then the IMU could
 87 *    overflow, but it seem to sink all 4 bytes, then it returns error.
 88 * 3) If we send more than 4 bytes, the IMU could overflow, and I saw it sending
 89 *    error after 4 bytes are sent; we have troubles in synchronizing again,
 90 *    because we are still sending data, and the IMU interprets it as the 1st
 91 *    byte of a new command.
 92 *
 93 * While we must avoid case 3, we could send 4 bytes per time and eventually
 94 * retry in case of failure; this seemed convenient for reads (which requires
 95 * TXing exactly 4 bytes), however it has been seen that, depending by the IMU
 96 * settings (e.g. LPF), failures became less or more frequent; in certain IMU
 97 * configurations they are very rare, but in certain others we keeps failing
 98 * even after like 30 retries.
 99 *
100 * So, we just split TXes in [2-bytes + delay] steps, and still keep an eye on
101 * the IMU response; in case it overflows (which is now unlikely), we retry.
102 */
103
104/*
105 * Read operation overhead:
106 *  4 bytes req + 2byte resp hdr.
107 *  6 bytes = 60 bit (considering 1start + 1stop bits).
108 *  60/115200 = ~520uS + about 2500mS delay -> ~3mS
109 * In 3mS we could read back about 34 bytes that means 17 samples, this means
110 * that in case of scattered reads in which the gap is 17 samples or less it is
111 * still convenient to go for a burst.
112 * We have to take into account also IMU response time - IMU seems to be often
113 * reasonably quick to respond, but sometimes it seems to be in some "critical
114 * section" in which it delays handling of serial protocol. Because of this we
115 * round-up to 22, which is the max number of samples, always bursting indeed.
116 */
117#define BNO055_SER_XFER_BURST_BREAK_THRESHOLD 22
118
119struct bno055_ser_priv {
120	enum {
121		CMD_NONE,
122		CMD_READ,
123		CMD_WRITE,
124	} expect_response;
125	int expected_data_len;
126	u8 *response_buf;
127
128	/**
129	 * enum cmd_status - represent the status of a command sent to the HW.
130	 * @STATUS_CRIT: The command failed: the serial communication failed.
131	 * @STATUS_OK:   The command executed successfully.
132	 * @STATUS_FAIL: The command failed: HW responded with an error.
133	 */
134	enum {
135		STATUS_CRIT = -1,
136		STATUS_OK = 0,
137		STATUS_FAIL = 1,
138	} cmd_status;
139
140	/*
141	 * Protects all the above fields, which are accessed in behalf of both
142	 * the serdev RX callback and the regmap side
143	 */
144	struct mutex lock;
145
146	/* Only accessed in serdev RX callback context*/
147	struct {
148		enum {
149			RX_IDLE,
150			RX_START,
151			RX_DATA,
152		} state;
153		int databuf_count;
154		int expected_len;
155		int type;
156	} rx;
157
158	/* Never accessed in behalf of serdev RX callback context */
159	bool cmd_stale;
160
161	struct completion cmd_complete;
162	struct serdev_device *serdev;
163};
164
165static int bno055_ser_send_chunk(struct bno055_ser_priv *priv, const u8 *data, int len)
166{
167	int ret;
168
169	trace_send_chunk(len, data);
170	ret = serdev_device_write(priv->serdev, data, len, msecs_to_jiffies(25));
171	if (ret < 0)
172		return ret;
173
174	if (ret < len)
175		return -EIO;
176
177	return 0;
178}
179
180/*
181 * Send a read or write command.
182 * 'data' can be NULL (used in read case). 'len' parameter is always valid; in
183 * case 'data' is non-NULL then it must match 'data' size.
184 */
185static int bno055_ser_do_send_cmd(struct bno055_ser_priv *priv,
186				  bool read, int addr, int len, const u8 *data)
187{
188	u8 hdr[] = {0xAA, read, addr, len};
189	int chunk_len;
190	int ret;
191
192	ret = bno055_ser_send_chunk(priv, hdr, 2);
193	if (ret)
194		goto fail;
195	usleep_range(2000, 3000);
196	ret = bno055_ser_send_chunk(priv, hdr + 2, 2);
197	if (ret)
198		goto fail;
199
200	if (read)
201		return 0;
202
203	while (len) {
204		chunk_len = min(len, 2);
205		usleep_range(2000, 3000);
206		ret = bno055_ser_send_chunk(priv, data, chunk_len);
207		if (ret)
208			goto fail;
209		data += chunk_len;
210		len -= chunk_len;
211	}
212
213	return 0;
214fail:
215	/* waiting more than 30mS should clear the BNO055 internal state */
216	usleep_range(40000, 50000);
217	return ret;
218}
219
220static int bno055_ser_send_cmd(struct bno055_ser_priv *priv,
221			       bool read, int addr, int len, const u8 *data)
222{
223	const int retry_max = 5;
224	int retry = retry_max;
225	int ret = 0;
226
227	/*
228	 * In case previous command was interrupted we still need to wait it to
229	 * complete before we can issue new commands
230	 */
231	if (priv->cmd_stale) {
232		ret = wait_for_completion_interruptible_timeout(&priv->cmd_complete,
233								msecs_to_jiffies(100));
234		if (ret == -ERESTARTSYS)
235			return -ERESTARTSYS;
236
237		priv->cmd_stale = false;
238		/* if serial protocol broke, bail out */
239		if (priv->cmd_status == STATUS_CRIT)
240			return -EIO;
241	}
242
243	/*
244	 * Try to convince the IMU to cooperate.. as explained in the comments
245	 * at the top of this file, the IMU could also refuse the command (i.e.
246	 * it is not ready yet); retry in this case.
247	 */
248	do {
249		mutex_lock(&priv->lock);
250		priv->expect_response = read ? CMD_READ : CMD_WRITE;
251		reinit_completion(&priv->cmd_complete);
252		mutex_unlock(&priv->lock);
253
254		if (retry != retry_max)
255			trace_cmd_retry(read, addr, retry_max - retry);
256		ret = bno055_ser_do_send_cmd(priv, read, addr, len, data);
257		if (ret)
258			continue;
259
260		ret = wait_for_completion_interruptible_timeout(&priv->cmd_complete,
261								msecs_to_jiffies(100));
262		if (ret == -ERESTARTSYS) {
263			priv->cmd_stale = true;
264			return -ERESTARTSYS;
265		}
266
267		if (!ret)
268			return -ETIMEDOUT;
269
270		if (priv->cmd_status == STATUS_OK)
271			return 0;
272		if (priv->cmd_status == STATUS_CRIT)
273			return -EIO;
274
275		/* loop in case priv->cmd_status == STATUS_FAIL */
276	} while (--retry);
277
278	if (ret < 0)
279		return ret;
280	if (priv->cmd_status == STATUS_FAIL)
281		return -EINVAL;
282	return 0;
283}
284
285static int bno055_ser_write_reg(void *context, const void *_data, size_t count)
286{
287	const u8 *data = _data;
288	struct bno055_ser_priv *priv = context;
289
290	if (count < 2) {
291		dev_err(&priv->serdev->dev, "Invalid write count %zu", count);
292		return -EINVAL;
293	}
294
295	trace_write_reg(data[0], data[1]);
296	return bno055_ser_send_cmd(priv, 0, data[0], count - 1, data + 1);
297}
298
299static int bno055_ser_read_reg(void *context,
300			       const void *_reg, size_t reg_size,
301			       void *val, size_t val_size)
302{
303	int ret;
304	int reg_addr;
305	const u8 *reg = _reg;
306	struct bno055_ser_priv *priv = context;
307
308	if (val_size > 128) {
309		dev_err(&priv->serdev->dev, "Invalid read valsize %zu", val_size);
310		return -EINVAL;
311	}
312
313	reg_addr = *reg;
314	trace_read_reg(reg_addr, val_size);
315	mutex_lock(&priv->lock);
316	priv->expected_data_len = val_size;
317	priv->response_buf = val;
318	mutex_unlock(&priv->lock);
319
320	ret = bno055_ser_send_cmd(priv, 1, reg_addr, val_size, NULL);
321
322	mutex_lock(&priv->lock);
323	priv->response_buf = NULL;
324	mutex_unlock(&priv->lock);
325
326	return ret;
327}
328
329/*
330 * Handler for received data; this is called from the receiver callback whenever
331 * it got some packet from the serial bus. The status tells us whether the
332 * packet is valid (i.e. header ok && received payload len consistent wrt the
333 * header). It's now our responsibility to check whether this is what we
334 * expected, of whether we got some unexpected, yet valid, packet.
335 */
336static void bno055_ser_handle_rx(struct bno055_ser_priv *priv, int status)
337{
338	mutex_lock(&priv->lock);
339	switch (priv->expect_response) {
340	case CMD_NONE:
341		dev_warn(&priv->serdev->dev, "received unexpected, yet valid, data from sensor");
342		mutex_unlock(&priv->lock);
343		return;
344
345	case CMD_READ:
346		priv->cmd_status = status;
347		if (status == STATUS_OK &&
348		    priv->rx.databuf_count != priv->expected_data_len) {
349			/*
350			 * If we got here, then the lower layer serial protocol
351			 * seems consistent with itself; if we got an unexpected
352			 * amount of data then signal it as a non critical error
353			 */
354			priv->cmd_status = STATUS_FAIL;
355			dev_warn(&priv->serdev->dev,
356				 "received an unexpected amount of, yet valid, data from sensor");
357		}
358		break;
359
360	case CMD_WRITE:
361		priv->cmd_status = status;
362		break;
363	}
364
365	priv->expect_response = CMD_NONE;
366	mutex_unlock(&priv->lock);
367	complete(&priv->cmd_complete);
368}
369
370/*
371 * Serdev receiver FSM. This tracks the serial communication and parse the
372 * header. It pushes packets to bno055_ser_handle_rx(), eventually communicating
373 * failures (i.e. malformed packets).
374 * Ideally it doesn't know anything about upper layer (i.e. if this is the
375 * packet we were really expecting), but since we copies the payload into the
376 * receiver buffer (that is not valid when i.e. we don't expect data), we
377 * snoop a bit in the upper layer..
378 * Also, we assume to RX one pkt per time (i.e. the HW doesn't send anything
379 * unless we require to AND we don't queue more than one request per time).
380 */
381static size_t bno055_ser_receive_buf(struct serdev_device *serdev,
382				     const u8 *buf, size_t size)
383{
384	int status;
385	struct bno055_ser_priv *priv = serdev_device_get_drvdata(serdev);
386	size_t remaining = size;
387
388	if (size == 0)
389		return 0;
390
391	trace_recv(size, buf);
392	switch (priv->rx.state) {
393	case RX_IDLE:
394		/*
395		 * New packet.
396		 * Check for its 1st byte that identifies the pkt type.
397		 */
398		if (buf[0] != 0xEE && buf[0] != 0xBB) {
399			dev_err(&priv->serdev->dev,
400				"Invalid packet start %x", buf[0]);
401			bno055_ser_handle_rx(priv, STATUS_CRIT);
402			break;
403		}
404		priv->rx.type = buf[0];
405		priv->rx.state = RX_START;
406		remaining--;
407		buf++;
408		priv->rx.databuf_count = 0;
409		fallthrough;
410
411	case RX_START:
412		/*
413		 * Packet RX in progress, we expect either 1-byte len or 1-byte
414		 * status depending by the packet type.
415		 */
416		if (remaining == 0)
417			break;
418
419		if (priv->rx.type == 0xEE) {
420			if (remaining > 1) {
421				dev_err(&priv->serdev->dev, "EE pkt. Extra data received");
422				status = STATUS_CRIT;
423			} else {
424				status = (buf[0] == 1) ? STATUS_OK : STATUS_FAIL;
425			}
426			bno055_ser_handle_rx(priv, status);
427			priv->rx.state = RX_IDLE;
428			break;
429
430		} else {
431			/*priv->rx.type == 0xBB */
432			priv->rx.state = RX_DATA;
433			priv->rx.expected_len = buf[0];
434			remaining--;
435			buf++;
436		}
437		fallthrough;
438
439	case RX_DATA:
440		/* Header parsed; now receiving packet data payload */
441		if (remaining == 0)
442			break;
443
444		if (priv->rx.databuf_count + remaining > priv->rx.expected_len) {
445			/*
446			 * This is an inconsistency in serial protocol, we lost
447			 * sync and we don't know how to handle further data
448			 */
449			dev_err(&priv->serdev->dev, "BB pkt. Extra data received");
450			bno055_ser_handle_rx(priv, STATUS_CRIT);
451			priv->rx.state = RX_IDLE;
452			break;
453		}
454
455		mutex_lock(&priv->lock);
456		/*
457		 * NULL e.g. when read cmd is stale or when no read cmd is
458		 * actually pending.
459		 */
460		if (priv->response_buf &&
461		    /*
462		     * Snoop on the upper layer protocol stuff to make sure not
463		     * to write to an invalid memory. Apart for this, let's the
464		     * upper layer manage any inconsistency wrt expected data
465		     * len (as long as the serial protocol is consistent wrt
466		     * itself (i.e. response header is consistent with received
467		     * response len.
468		     */
469		    (priv->rx.databuf_count + remaining <= priv->expected_data_len))
470			memcpy(priv->response_buf + priv->rx.databuf_count,
471			       buf, remaining);
472		mutex_unlock(&priv->lock);
473
474		priv->rx.databuf_count += remaining;
475
476		/*
477		 * Reached expected len advertised by the IMU for the current
478		 * packet. Pass it to the upper layer (for us it is just valid).
479		 */
480		if (priv->rx.databuf_count == priv->rx.expected_len) {
481			bno055_ser_handle_rx(priv, STATUS_OK);
482			priv->rx.state = RX_IDLE;
483		}
484		break;
485	}
486
487	return size;
488}
489
490static const struct serdev_device_ops bno055_ser_serdev_ops = {
491	.receive_buf = bno055_ser_receive_buf,
492	.write_wakeup = serdev_device_write_wakeup,
493};
494
495static const struct regmap_bus bno055_ser_regmap_bus = {
496	.write = bno055_ser_write_reg,
497	.read = bno055_ser_read_reg,
498};
499
500static int bno055_ser_probe(struct serdev_device *serdev)
501{
502	struct bno055_ser_priv *priv;
503	struct regmap *regmap;
504	int ret;
505
506	priv = devm_kzalloc(&serdev->dev, sizeof(*priv), GFP_KERNEL);
507	if (!priv)
508		return -ENOMEM;
509
510	serdev_device_set_drvdata(serdev, priv);
511	priv->serdev = serdev;
512	mutex_init(&priv->lock);
513	init_completion(&priv->cmd_complete);
514
515	serdev_device_set_client_ops(serdev, &bno055_ser_serdev_ops);
516	ret = devm_serdev_device_open(&serdev->dev, serdev);
517	if (ret)
518		return ret;
519
520	if (serdev_device_set_baudrate(serdev, 115200) != 115200) {
521		dev_err(&serdev->dev, "Cannot set required baud rate");
522		return -EIO;
523	}
524
525	ret = serdev_device_set_parity(serdev, SERDEV_PARITY_NONE);
526	if (ret) {
527		dev_err(&serdev->dev, "Cannot set required parity setting");
528		return ret;
529	}
530	serdev_device_set_flow_control(serdev, false);
531
532	regmap = devm_regmap_init(&serdev->dev, &bno055_ser_regmap_bus,
533				  priv, &bno055_regmap_config);
534	if (IS_ERR(regmap))
535		return dev_err_probe(&serdev->dev, PTR_ERR(regmap),
536				     "Unable to init register map");
537
538	return bno055_probe(&serdev->dev, regmap,
539			    BNO055_SER_XFER_BURST_BREAK_THRESHOLD, false);
540}
541
542static const struct of_device_id bno055_ser_of_match[] = {
543	{ .compatible = "bosch,bno055" },
544	{ }
545};
546MODULE_DEVICE_TABLE(of, bno055_ser_of_match);
547
548static struct serdev_device_driver bno055_ser_driver = {
549	.driver = {
550		.name = "bno055-ser",
551		.of_match_table = bno055_ser_of_match,
552	},
553	.probe = bno055_ser_probe,
554};
555module_serdev_device_driver(bno055_ser_driver);
556
557MODULE_AUTHOR("Andrea Merello <andrea.merello@iit.it>");
558MODULE_DESCRIPTION("Bosch BNO055 serdev interface");
559MODULE_IMPORT_NS("IIO_BNO055");
560MODULE_LICENSE("GPL");