[PATCH] i3c: master: dw: Clamp GETMRL/GETMWL to controller FIFO limits

[Shubham Patil]

The DW master rejects private SDR transfers larger than
caps.datafifodepth with -EOPNOTSUPP. Targets often report MRL/MWL
values larger than that FIFO, so the core stores limits the controller
cannot meet.

After a successful GETMRL/GETMWL, issue Direct SETMRL/SETMWL to the
same target with lengths capped to the data FIFO (in bytes), then
rewrite the GET payload so the core keeps the same values. Only update
the GET buffer once SET is acked, so a failed SET does not leave the
core and the target disagreeing.

GETMRL is variable length: the optional third byte is max IBI payload
and is only present if the target returned it. Clamp that IBI byte to
the IBI queue depth from QUEUE_SIZE_CAPABILITY.IBI_BUF_SIZE (bits 19:16
at 0xe8, encoded as 2^(n+1) dwords).

Rename the unused EXTENDED_CAPABILITY macro at 0xe8 to the databook
name QUEUE_SIZE_CAPABILITY.

Signed-off-by: Shubham Patil <shubhamsanjay.patil@xxxxxxx>
---
 drivers/i3c/master/dw-i3c-master.c | 149 ++++++++++++++++++++++++++++-
 drivers/i3c/master/dw-i3c-master.h |   1 +
 2 files changed, 149 insertions(+), 1 deletion(-)

diff --git a/drivers/i3c/master/dw-i3c-master.c b/drivers/i3c/master/dw-i3c-master.c
index 4563d8761ba0..51defcb57761 100644
--- a/drivers/i3c/master/dw-i3c-master.c
+++ b/drivers/i3c/master/dw-i3c-master.c
@@ -203,7 +203,13 @@
 #define BUS_IDLE_TIMING			0xd8
 #define I3C_VER_ID			0xe0
 #define I3C_VER_TYPE			0xe4
-#define EXTENDED_CAPABILITY		0xe8
+#define QUEUE_SIZE_CAPABILITY		0xe8
+#define QUEUE_SIZE_CAPABILITY_IBI_BUF(x)	(((x) & GENMASK(19, 16)) >> 16)
+/*
+ * IBI_BUF_SIZE is encoded as 2^(field + 1) dwords: the smallest buffer is
+ * 2 dwords and each increment of the field doubles the depth.
+ */
+#define QUEUE_SIZE_IBI_BUF_MIN_DWORDS	2
 #define SLAVE_CONFIG			0xec
 
 #define DYN_ADDR_LO_MASK GENMASK(4, 0)
@@ -844,6 +850,130 @@ static int dw_i3c_ccc_get(struct dw_i3c_master *master, struct i3c_ccc_cmd *ccc)
 	return ret;
 }
 
+/*
+ * Cap the limits a target reported through GETMRL to what this controller can
+ * actually transfer, so the core never asks for a private read the data FIFO
+ * cannot hold. The optional IBI payload byte is capped to the IBI queue depth
+ * instead; since that byte is a u8, the IBI cap only ever applies to
+ * controllers whose IBI queue is smaller than 255 bytes.
+ *
+ * Direct SETMRL is optional, so a target may implement GETMRL and NACK the SET.
+ * Clamp the values handed back to the core either way: a failed SET only means
+ * the target keeps its own larger limit, which is harmless as long as the core
+ * stays within ours.
+ */
+static int dw_i3c_master_clamp_mrl(struct dw_i3c_master *master,
+				   struct i3c_ccc_cmd *ccc)
+{
+	u16 max_fifo_bytes = master->caps.datafifodepth * sizeof(u32);
+	u32 max_ibi_bytes = master->caps.ibififodepth * sizeof(u32);
+	u16 actual_len = ccc->dests[0].payload.actual_len;
+	struct i3c_ccc_cmd_dest set_dest = { };
+	struct i3c_ccc_cmd set_cmd = { };
+	struct i3c_ccc_mrl set_mrl;
+	struct i3c_ccc_mrl *mrl;
+	bool clamp_ibi = false;
+	bool clamp_read;
+	u8 ibi_len = 0;
+	u16 read_len;
+	int ret;
+
+	/* Need at least the 2-byte max read length field to act on. */
+	if (actual_len < 2)
+		return 0;
+
+	mrl = ccc->dests[0].payload.data;
+	read_len = be16_to_cpu(mrl->read_len);
+	clamp_read = read_len > max_fifo_bytes;
+
+	/* Optional third byte is valid only if the target returned it. */
+	if (actual_len > 2) {
+		ibi_len = mrl->ibi_len;
+		clamp_ibi = max_ibi_bytes && ibi_len > max_ibi_bytes;
+	}
+
+	if (!clamp_read && !clamp_ibi)
+		return 0;
+
+	set_mrl.read_len = cpu_to_be16(clamp_read ? max_fifo_bytes : read_len);
+	if (actual_len > 2)
+		set_mrl.ibi_len = clamp_ibi ? max_ibi_bytes : ibi_len;
+
+	set_dest.addr = ccc->dests[0].addr;
+	set_dest.payload.data = &set_mrl;
+	set_dest.payload.len = actual_len;
+
+	set_cmd.rnw = 0;
+	set_cmd.id = I3C_CCC_SETMRL(false);
+	set_cmd.ndests = 1;
+	set_cmd.dests = &set_dest;
+
+	ret = dw_i3c_ccc_set(master, &set_cmd);
+	if (ret)
+		dev_dbg(&master->base.dev,
+			"SETMRL not accepted by target: %d\n", ret);
+
+	if (clamp_read) {
+		mrl->read_len = cpu_to_be16(max_fifo_bytes);
+		dev_dbg(&master->base.dev,
+			"clamped target MRL from %u to %u bytes (FIFO depth limit)\n",
+			read_len, max_fifo_bytes);
+	}
+	if (clamp_ibi) {
+		mrl->ibi_len = max_ibi_bytes;
+		dev_dbg(&master->base.dev,
+			"clamped target IBI len from %u to %u bytes (IBI buffer limit)\n",
+			ibi_len, max_ibi_bytes);
+	}
+
+	return 0;
+}
+
+/* Same contract as dw_i3c_master_clamp_mrl(), for the write direction. */
+static int dw_i3c_master_clamp_mwl(struct dw_i3c_master *master,
+				   struct i3c_ccc_cmd *ccc)
+{
+	u16 max_fifo_bytes = master->caps.datafifodepth * sizeof(u32);
+	struct i3c_ccc_cmd_dest set_dest = { };
+	struct i3c_ccc_cmd set_cmd = { };
+	struct i3c_ccc_mwl set_mwl;
+	struct i3c_ccc_mwl *mwl;
+	u16 write_len;
+	int ret;
+
+	if (ccc->dests[0].payload.actual_len < 2)
+		return 0;
+
+	mwl = ccc->dests[0].payload.data;
+	write_len = be16_to_cpu(mwl->len);
+
+	if (write_len <= max_fifo_bytes)
+		return 0;
+
+	set_mwl.len = cpu_to_be16(max_fifo_bytes);
+
+	set_dest.addr = ccc->dests[0].addr;
+	set_dest.payload.data = &set_mwl;
+	set_dest.payload.len = sizeof(set_mwl);
+
+	set_cmd.rnw = 0;
+	set_cmd.id = I3C_CCC_SETMWL(false);
+	set_cmd.ndests = 1;
+	set_cmd.dests = &set_dest;
+
+	ret = dw_i3c_ccc_set(master, &set_cmd);
+	if (ret)
+		dev_dbg(&master->base.dev,
+			"SETMWL not accepted by target: %d\n", ret);
+
+	mwl->len = cpu_to_be16(max_fifo_bytes);
+	dev_dbg(&master->base.dev,
+		"clamped target MWL from %u to %u bytes (FIFO depth limit)\n",
+		write_len, max_fifo_bytes);
+
+	return 0;
+}
+
 static int dw_i3c_master_send_ccc_cmd(struct i3c_master_controller *m,
 				      struct i3c_ccc_cmd *ccc)
 {
@@ -866,6 +996,18 @@ static int dw_i3c_master_send_ccc_cmd(struct i3c_master_controller *m,
 	else
 		ret = dw_i3c_ccc_set(master, ccc);
 
+	/*
+	 * Clamp GETMRL/GETMWL responses to the data FIFO depth, and the
+	 * optional GETMRL IBI byte to the IBI queue depth. The GET itself has
+	 * already succeeded, so its result is never overridden here.
+	 */
+	if (!ret && ccc->rnw) {
+		if (ccc->id == I3C_CCC_GETMRL)
+			dw_i3c_master_clamp_mrl(master, ccc);
+		else if (ccc->id == I3C_CCC_GETMWL)
+			dw_i3c_master_clamp_mwl(master, ccc);
+	}
+
 	pm_runtime_put_autosuspend(master->dev);
 	return ret;
 }
@@ -1728,6 +1870,11 @@ int dw_i3c_common_probe(struct dw_i3c_master *master,
 	ret = readl(master->regs + DATA_BUFFER_STATUS_LEVEL);
 	master->caps.datafifodepth = DATA_BUFFER_STATUS_LEVEL_TX(ret);
 
+	/* Read the IBI data buffer size advertised by the controller. */
+	ret = readl(master->regs + QUEUE_SIZE_CAPABILITY);
+	master->caps.ibififodepth = QUEUE_SIZE_IBI_BUF_MIN_DWORDS <<
+				    QUEUE_SIZE_CAPABILITY_IBI_BUF(ret);
+
 	ret = readl(master->regs + DEVICE_ADDR_TABLE_POINTER);
 	master->datstartaddr = ret;
 	master->maxdevs = ret >> 16;
diff --git a/drivers/i3c/master/dw-i3c-master.h b/drivers/i3c/master/dw-i3c-master.h
index 17ad817d1f8e..54c3912374c8 100644
--- a/drivers/i3c/master/dw-i3c-master.h
+++ b/drivers/i3c/master/dw-i3c-master.h
@@ -15,6 +15,7 @@
 struct dw_i3c_master_caps {
 	u8 cmdfifodepth;
 	u8 datafifodepth;
+	u32 ibififodepth;
 };
 
 struct dw_i3c_dat_entry {
-- 
2.34.1