From: Clark Wang <xiaoning.wang@nxp.com>
Date: Fri, 25 Oct 2019 05:05:22 +0000 (+0800)
Subject: MLK-22850 i2c: rpmsg-imx: add msg_id for checking response id
X-Git-Tag: rel_imx_4.19.35_1.1.0~2
X-Git-Url: https://git.somdevices.com/?a=commitdiff_plain;h=71c5490732baa290476c22a5904cd2909e01e7a2;p=linux.git

MLK-22850 i2c: rpmsg-imx: add msg_id for checking response id

M4 deals with the rpmsg msg by FIFO. When a timeout occurs on Acore
side, it might impact the next several rpmsg requests and cause the
these requests timeout, too.

For this case, add a msg_id in the unused buffer field, check the
request id of response each time.

Signed-off-by: Clark Wang <xiaoning.wang@nxp.com>
(cherry picked from commit 8a94d641b4648a52bcfc43a41e8e8c084e64d2e7)
---

diff --git a/drivers/i2c/busses/i2c-rpmsg-imx.c b/drivers/i2c/busses/i2c-rpmsg-imx.c
index 9d0561f8cb19..fd3fcf622cd9 100644
--- a/drivers/i2c/busses/i2c-rpmsg-imx.c
+++ b/drivers/i2c/busses/i2c-rpmsg-imx.c
@@ -112,6 +112,8 @@ struct i2c_rpmsg_info {
 
 	u8 bus_id;
 	u16 addr;
+	bool check_msg_id;
+	u8 msg_id;
 };
 
 static struct i2c_rpmsg_info i2c_rpmsg;
@@ -128,10 +130,27 @@ static int i2c_rpmsg_cb(struct rpmsg_device *rpdev, void *data, int len,
 	if (msg->header.type != I2C_RPMSG_TYPE_RESPONSE)
 		return -EINVAL;
 
+	if (i2c_rpmsg.check_msg_id && msg->buf[msg->len] != i2c_rpmsg.msg_id) {
+		dev_err(&rpdev->dev,
+		"expected msg_id:%d, received msg_id:%d, drop this recv\n",
+		i2c_rpmsg.msg_id, msg->buf[msg->len]);
+
+		/*
+		 * This response does not match the request id.
+		 * Drop it and wait for the right response.
+		 */
+		return 0;
+	}
+
 	if (msg->bus_id != i2c_rpmsg.bus_id || msg->addr != i2c_rpmsg.addr) {
 		dev_err(&rpdev->dev,
 		"expected bus_id:%d, addr:%2x, received bus_id:%d, addr:%2x\n",
 		i2c_rpmsg.bus_id, i2c_rpmsg.addr, msg->bus_id, msg->addr);
+
+		/*
+		 * The bus_id or addr of this response does not match the
+		 * request, but the msg_id match. So return error.
+		 */
 		return -EINVAL;
 	}
 
@@ -164,7 +183,10 @@ static int rpmsg_xfer(struct i2c_rpmsg_msg *rmsg, struct i2c_rpmsg_info *info)
 	ret = wait_for_completion_timeout(&info->cmd_complete,
 					msecs_to_jiffies(I2C_RPMSG_TIMEOUT));
 	if (!ret) {
-		dev_err(&info->rpdev->dev, "%s failed: timeout\n", __func__);
+		dev_err(&info->rpdev->dev, "%s failed: timeout, "
+				"target busid=%-2d, addr=0x%02X, %s\n",
+				__func__, rmsg->bus_id, rmsg->addr,
+				(rmsg->flags & I2C_M_RD) ? "R" : "W");
 		return -ETIMEDOUT;
 	}
 
@@ -209,6 +231,16 @@ static int i2c_rpmsg_read(struct i2c_msg *msg, struct i2c_rpmsg_info *info,
 		rmsg.flags = msg->flags;
 	rmsg.len = (msg->len);
 
+	/*
+	 * Add a message id for each send msg in the unused buffer field,
+	 * which will be send back by M4 in the buffer. So we can check
+	 * if this received frame is the actual response for my request
+	 * frame. This is useful when the timeouts occur in several
+	 * consecutive frames.
+	 */
+	if (info->check_msg_id)
+		rmsg.buf[rmsg.len] = info->msg_id;
+
 	reinit_completion(&info->cmd_complete);
 
 	ret = rpmsg_xfer(&rmsg, info);
@@ -262,6 +294,9 @@ int i2c_rpmsg_write(struct i2c_msg *msg, struct i2c_rpmsg_info *info,
 	for (i = 0; i < rmsg.len; i++)
 		rmsg.buf[i] = msg->buf[i];
 
+	if (info->check_msg_id)
+		rmsg.buf[rmsg.len] = info->msg_id;
+
 	reinit_completion(&info->cmd_complete);
 
 	ret = rpmsg_xfer(&rmsg, info);
@@ -331,6 +366,19 @@ static int i2c_rpbus_xfer(struct i2c_adapter *adapter,
 
 		i2c_rpmsg.bus_id = rdata->adapter.nr;
 		i2c_rpmsg.addr = pmsg->addr;
+		if (i2c_rpmsg.msg_id >= 0xFF)
+			i2c_rpmsg.msg_id = 0;
+		i2c_rpmsg.msg_id++;
+
+		if (pmsg->len == I2C_RPMSG_MAX_BUF_SIZE)
+			/*
+			 * The msg length is too long, cannot add msg_id
+			 * for this request. Will not check the msg_id
+			 * when receive response.
+			 */
+			i2c_rpmsg.check_msg_id = false;
+		else
+			i2c_rpmsg.check_msg_id = true;
 
 		if (pmsg->flags & I2C_M_RD) {
 			ret = i2c_rpmsg_read(pmsg, &i2c_rpmsg,