[PATCH] i2c: amd: fix SMBus block header and PEC handling in the RX path

[Paritosh Potukuchi <paritoshpotukuchi@gmail.com>]

Strip SMBus block header bytes from the received buffer before
forwarding the payload to the upper layers.

Handles the optional PEC Byte at the end of the received
payload.

Signed-off-by: Paritosh Potukuchi <paritosh.potukuchi@amd.com>
---
 drivers/i2c/busses/i2c-amd-asf-plat.c | 34 +++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/drivers/i2c/busses/i2c-amd-asf-plat.c b/drivers/i2c/busses/i2c-amd-asf-plat.c
index e1699da838c2..daf318ce3bbb 100644
--- a/drivers/i2c/busses/i2c-amd-asf-plat.c
+++ b/drivers/i2c/busses/i2c-amd-asf-plat.c
@@ -46,6 +46,8 @@
 #define ASF_BLOCK_MAX_BYTES	72
 #define ASF_ERROR_STATUS	GENMASK(3, 1)
 
+#define SMBUS_BLOCK_HEADER_SIZE 2
+
 struct amd_asf_dev {
 	struct i2c_adapter adap;
 	void __iomem *eoi_base;
@@ -61,7 +63,7 @@ static void amd_asf_process_target(struct work_struct *work)
 	unsigned short piix4_smba = dev->port_addr->start;
 	u8 data[ASF_BLOCK_MAX_BYTES];
 	u8 bank, reg, cmd;
-	u8 len = 0, idx, val;
+	u8 len = 0, idx, val, offset, data_len, payload_len, has_pec;
 
 	/* Read target status register */
 	reg = inb_p(ASFSLVSTA);
@@ -110,12 +112,40 @@ static void amd_asf_process_target(struct work_struct *work)
 	if (cmd & BIT(0))
 		return;
 
+
+	/*Below is the SMBus block header received on the wire:
+	 *     Command       --  1 Byte
+	 *     Data Length   --  1 Byte (number of data bytes)
+	 *     Data          --  N Bytes
+	 *     PEC           --  optional, 1 Byte
+	 *
+	 *     PEC, if present, is the final byte of the transfer
+	 */
+
+	offset = SMBUS_BLOCK_HEADER_SIZE;
+	if (len < offset)
+		return;
+
+	payload_len = data[1];
+
+	if (payload_len > len - offset)
+		return;
+
+	if (len == offset + payload_len + 1)
+		has_pec = 1;
+	else if (len == offset + payload_len)
+		has_pec = 0;
+	else
+		return;
+
+	data_len = len - has_pec;
+
 	/*
 	 * Although i2c_slave_event() returns an appropriate error code, we
 	 * don't check it here because we're operating in the workqueue context.
 	 */
 	i2c_slave_event(dev->target, I2C_SLAVE_WRITE_REQUESTED, &val);
-	for (idx = 0; idx < len; idx++) {
+	for (idx = offset; idx < data_len; idx++) {
 		val = data[idx];
 		i2c_slave_event(dev->target, I2C_SLAVE_WRITE_RECEIVED, &val);
 	}
-- 
2.43.0