1 files changed, 190 insertions, 90 deletions

diff --git a/meta-openbmc-mods/meta-common/recipes-core/interfaces/libmctp/smbus.c b/meta-openbmc-mods/meta-common/recipes-core/interfaces/libmctp/smbus.c
index 8679a5185..d7c396444 100644
--- a/meta-openbmc-mods/meta-common/recipes-core/interfaces/libmctp/smbus.c
+++ b/meta-openbmc-mods/meta-common/recipes-core/interfaces/libmctp/smbus.c
@@ -2,6 +2,7 @@
 
 #include <assert.h>
 #include <stdbool.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -24,7 +25,6 @@
 
 struct mctp_binding_smbus {
 	struct mctp_binding binding;
-	struct mctp *mctp;
 	int out_fd;
 	int in_fd;
 
@@ -50,6 +50,8 @@ struct mctp_binding_smbus {
 #define MCTP_SOURCE_SLAVE_ADDRESS 0x21
 
 #define SMBUS_PEC_BYTE_SIZE 1
+#define SMBUS_COMMAND_CODE_SIZE 1
+#define SMBUS_LENGTH_FIELD_SIZE 1
 
 struct mctp_smbus_header_tx {
 	uint8_t source_slave_address;
@@ -62,54 +64,133 @@ struct mctp_smbus_header_rx {
 	uint8_t source_slave_address;
 };
 
+#define POLYCHECK (0x1070U << 3)
+static uint8_t crc8_calculate(uint16_t d)
+{
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		if (d & 0x8000) {
+			d = d ^ POLYCHECK;
+		}
+		d = d << 1;
+	}
+
+	return (uint8_t)(d >> 8);
+}
+
+/* Incremental CRC8 over count bytes in the array pointed to by p */
+static uint8_t pec_calculate(uint8_t crc, uint8_t *p, size_t count)
+{
+	int i;
+
+	for (i = 0; i < count; i++) {
+		crc = crc8_calculate((crc ^ p[i]) << 8);
+	}
+
+	return crc;
+}
+
+static uint8_t calculate_pec_byte(uint8_t *buf, size_t len, uint8_t address,
+				  uint16_t flags)
+{
+	uint8_t addr = (address << 1) | (flags & I2C_M_RD ? 1 : 0);
+	uint8_t pec = pec_calculate(0, &addr, 1);
+	pec = pec_calculate(pec, buf, len);
+
+	return pec;
+}
+
+static int mctp_smbus_tx(struct mctp_binding_smbus *smbus, uint8_t len)
+{
+
+#ifdef I2C_M_HOLD
+	/* Hold message */
+	static uint16_t holdtimeout = 1000; // timeout in ms.
+	struct i2c_msg msg[2] =
+#else  // !I2C_M_HOLD
+	struct i2c_msg msg[1] =
+#endif // I2C_M_HOLD
+		{{.addr = MCTP_SLAVE_ADDRESS,
+		  .flags = 0,
+		  .len = len,
+		  .buf = (__uint8_t *)smbus->txbuf}
+#ifdef I2C_M_HOLD
+		 ,
+		 {.addr = 0,
+		  .flags = I2C_M_HOLD,
+		  .len = sizeof(holdtimeout),
+		  .buf = (__uint8_t *)&holdtimeout}
+#endif // I2C_M_HOLD
+		};
+
+#ifdef I2C_M_HOLD
+	struct i2c_rdwr_ioctl_data msgrdwr = {&msg, 2};
+#else  // !I2C_M_HOLD
+	struct i2c_rdwr_ioctl_data msgrdwr = {&msg, 1};
+#endif // I2C_M_HOLD
+
+	return ioctl(smbus->out_fd, I2C_RDWR, &msgrdwr);
+}
+
+#ifdef I2C_M_HOLD
+static int mctp_smbus_unhold_bus(struct mctp_binding_smbus *smbus)
+{
+	/* Unhold message */
+	static uint16_t holdtimeout = 0; // unhold
+	struct i2c_msg holdmsg = {0, I2C_M_HOLD, sizeof(holdtimeout),
+				  (__uint8_t *)&holdtimeout};
+
+	struct i2c_rdwr_ioctl_data msgrdwr = {&holdmsg, 1};
+
+	return ioctl(smbus->out_fd, I2C_RDWR, &msgrdwr);
+}
+#endif // I2C_M_HOLD
+
 static int mctp_binding_smbus_tx(struct mctp_binding *b,
 				 struct mctp_pktbuf *pkt)
 {
 	struct mctp_binding_smbus *smbus = binding_to_smbus(b);
 	struct mctp_smbus_header_tx *hdr;
-	uint8_t *buf;
-	size_t len;
-	int r;
-	struct i2c_msg msg;
-	struct i2c_rdwr_ioctl_data data;
+	size_t pkt_length;
 
-	data.msgs = &msg;
-	data.nmsgs = 1;
+	uint8_t i2c_message_buf[256];
+	uint8_t *buf_ptr;
+	uint8_t i2c_message_len;
 
 	uint16_t timeout = 1000;
-	int i;
 
 	/* the length field in the header excludes smbus framing
 	 * and escape sequences */
-	len = mctp_pktbuf_size(pkt);
+	pkt_length = mctp_pktbuf_size(pkt);
 
-	hdr = (void *)smbus->txbuf;
+	buf_ptr = (void *)smbus->txbuf;
+	*buf_ptr = MCTP_COMMAND_CODE;
+	buf_ptr++;
+	*buf_ptr = pkt_length + sizeof(*hdr);
+	buf_ptr++;
 
-	// hdr->len = len + 1;  // +1 for PET byte
+	hdr = (void *)buf_ptr;
 	hdr->source_slave_address = MCTP_SOURCE_SLAVE_ADDRESS;
+	buf_ptr = (buf_ptr + sizeof(*hdr));
+	memcpy(buf_ptr, &pkt->data[pkt->start], pkt_length);
+	buf_ptr = buf_ptr + pkt_length;
 
-	buf = (void *)(hdr + sizeof(*hdr));
+	uint8_t pec_byte = calculate_pec_byte(
+		smbus->txbuf,
+		SMBUS_COMMAND_CODE_SIZE + SMBUS_LENGTH_FIELD_SIZE + sizeof(*hdr)
+			+ pkt_length,
+		MCTP_SLAVE_ADDRESS, 0);
 
-	if (len + sizeof(*hdr) > sizeof(smbus->txbuf))
-		return -1;
-
-	memcpy(buf, &pkt->data[pkt->start], len);
+	*buf_ptr = pec_byte;
 
-	buf += len;
+	i2c_message_len = SMBUS_COMMAND_CODE_SIZE + SMBUS_LENGTH_FIELD_SIZE
+			  + sizeof(*hdr) + pkt_length
+			  + SMBUS_PEC_BYTE_SIZE; // command code, length,
+						 // header, data, pec byte
 
-	if (ioctl(smbus->out_fd, I2C_SLAVE, MCTP_SLAVE_ADDRESS) < 0) {
-		mctp_prerr("Can't set slave");
-		return -1;
-	}
-	if (ioctl(smbus->out_fd, I2C_PEC, 1) < 0) {
-		mctp_prerr("Cant set PEC byte");
-		return -1;
-	}
-
-	if (i2c_smbus_write_block_data(smbus->out_fd, MCTP_COMMAND_CODE,
-				       sizeof(*hdr) + len, (void *)smbus->txbuf)
-	    < 0) {
-		mctp_prerr("Failed to send");
+	if (mctp_smbus_tx(smbus, i2c_message_len)) {
+		mctp_prerr("Can't hold mux");
 		return -1;
 	}
 
@@ -119,64 +200,83 @@ static int mctp_binding_smbus_tx(struct mctp_binding *b,
 #ifdef MCTP_FILEIO
 int mctp_smbus_read(struct mctp_binding_smbus *smbus)
 {
-	ssize_t len;
-	int r;
+	ssize_t len = 0;
 	struct mctp_smbus_header_rx *hdr;
+	int ret = 0;
 
-	r = lseek(smbus->in_fd, 0, SEEK_SET);
-	if (r < 0) {
-		mctp_prerr("Failed to seek");
-		return -1;
-	}
-	len = read(smbus->in_fd, smbus->rxbuf, sizeof(smbus->rxbuf));
-	if (len < sizeof(*hdr)) {
-		// TODO Don't return an error here, as it seems to happen from
-		// time to time, even with a properly written poll loop,
-		// although it's not clear why.
-		return 0;
-	}
+	do {
+		ret = lseek(smbus->in_fd, 0, SEEK_SET);
+		if (ret < 0) {
+			mctp_prerr("Failed to seek");
+			ret = -1;
+		}
 
-	hdr = (void *)smbus->rxbuf;
-	if (hdr->destination_slave_address
-	    != (MCTP_SOURCE_SLAVE_ADDRESS & ~1)) {
-		mctp_prerr("Got bad slave address %d",
-			   hdr->destination_slave_address);
-		return 0;
-	}
-	if (hdr->command_code != MCTP_COMMAND_CODE) {
-		mctp_prerr("Got bad command code %d", hdr->command_code);
-		// Not a payload intended for us
-		return 0;
-	}
+		len = read(smbus->in_fd, smbus->rxbuf, sizeof(smbus->rxbuf));
+		if (len < sizeof(*hdr)) {
+			// This condition hits from from time to time, even with
+			// a properly written poll loop, although it's not clear
+			// why. Return an error so that the upper layer can
+			// retry.
+			ret = 0;
+			break;
+		}
 
-	if (hdr->byte_count != (len - sizeof(*hdr))) {
-		// Got an incorrectly sized payload
-		mctp_prerr("Got smbus payload sized %d, expecting %d",
-			   hdr->byte_count, len - sizeof(*hdr));
-		return 0;
-	}
+		hdr = (void *)smbus->rxbuf;
+		if (hdr->destination_slave_address
+		    != (MCTP_SOURCE_SLAVE_ADDRESS & ~1)) {
+			mctp_prerr("Got bad slave address %d",
+				   hdr->destination_slave_address);
+			ret = 0;
+			break;
+		}
+		if (hdr->command_code != MCTP_COMMAND_CODE) {
+			mctp_prerr("Got bad command code %d",
+				   hdr->command_code);
+			// Not a payload intended for us
+			ret = 0;
+			break;
+		}
 
-	if (len < 0) {
-		mctp_prerr("can't read from smbus device: %m");
-		return -1;
-	}
+		if (hdr->byte_count != (len - sizeof(*hdr))) {
+			// Got an incorrectly sized payload
+			mctp_prerr("Got smbus payload sized %d, expecting %d",
+				   hdr->byte_count, len - sizeof(*hdr));
+			ret = 0;
+			break;
+		}
 
-	smbus->rx_pkt = mctp_pktbuf_alloc(0);
-	assert(smbus->rx_pkt);
+		if (len < 0) {
+			mctp_prerr("can't read from smbus device: %m");
+			ret = -1;
+			break;
+		}
 
-	if (mctp_pktbuf_push(smbus->rx_pkt, &smbus->rxbuf[sizeof(*hdr)],
-			     len - sizeof(*hdr) - SMBUS_PEC_BYTE_SIZE)
-	    != 0) {
-		mctp_prerr("Can't push tok pktbuf: %m");
-		return -1;
-	}
+		smbus->rx_pkt = mctp_pktbuf_alloc(0);
+		assert(smbus->rx_pkt);
+
+		if (mctp_pktbuf_push(smbus->rx_pkt, &smbus->rxbuf[sizeof(*hdr)],
+				     len - sizeof(*hdr) - SMBUS_PEC_BYTE_SIZE)
+		    != 0) {
+			mctp_prerr("Can't push tok pktbuf: %m");
+			ret = -1;
+			break;
+		}
 
-	mctp_bus_rx(&(smbus->binding), smbus->rx_pkt);
+		mctp_bus_rx(&(smbus->binding), smbus->rx_pkt);
 
-	mctp_pktbuf_free(smbus->rx_pkt);
-	smbus->rx_pkt = NULL;
+		mctp_pktbuf_free(smbus->rx_pkt);
+		smbus->rx_pkt = NULL;
 
-	return 0;
+	} while (0);
+
+#ifdef I2C_M_HOLD
+	if (mctp_smbus_unhold_bus(smbus)) {
+		mctp_prerr("Can't hold mux");
+		ret = -1;
+	}
+#endif // I2C_M_HOLD
+
+	return ret;
 }
 
 int mctp_smbus_get_in_fd(struct mctp_binding_smbus *smbus)
@@ -193,17 +293,13 @@ int mctp_smbus_open_bus(struct mctp_binding_smbus *smbus, int out_bus_num,
 			int root_bus_num)
 {
 	char filename[60];
-	size_t filename_size;
+	size_t filename_size = 0;
 	char slave_mqueue[20];
-	size_t mqueue_size;
-
-	int fd;
-
-	size_t size;
-	int address_7_bit;
+	size_t mqueue_size = 0;
+	int fd = 0;
+	size_t size = sizeof(filename);
+	int address_7_bit = MCTP_SOURCE_SLAVE_ADDRESS >> 1;
 
-	address_7_bit = MCTP_SOURCE_SLAVE_ADDRESS >> 1;
-	size = sizeof(filename);
 	snprintf(filename, size,
 		 "/sys/bus/i2c/devices/i2c-%d/%d-%04x/slave-mqueue",
 		 root_bus_num, root_bus_num,
@@ -263,7 +359,6 @@ int mctp_smbus_open_bus(struct mctp_binding_smbus *smbus, int out_bus_num,
 
 	return 0;
 }
-
 #endif
 
 void mctp_smbus_register_bus(struct mctp_binding_smbus *smbus,
@@ -271,8 +366,8 @@ void mctp_smbus_register_bus(struct mctp_binding_smbus *smbus,
 {
 	assert(smbus->out_fd >= 0);
 	assert(smbus->in_fd >= 0);
-	smbus->mctp = mctp;
 	smbus->bus_id = mctp_register_bus(mctp, &smbus->binding, eid);
+	mctp_binding_set_tx_enabled(&smbus->binding, true);
 }
 
 struct mctp_binding_smbus *mctp_smbus_init(void)
@@ -291,3 +386,8 @@ struct mctp_binding_smbus *mctp_smbus_init(void)
 
 	return smbus;
 }
+
+void mctp_smbus_free(struct mctp_binding_smbus *smbus)
+{
+	__mctp_free(smbus);
+}
\ No newline at end of file