thunderbolt: NVM fixes and consolidation

index 7288aaf01ae6..5631319f7b20 100644
@@ -366,15 +366,15 @@ int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
 			void *buf, size_t size)
 {
 	unsigned int retries = DMA_PORT_RETRIES;
-	unsigned int offset;
-
-	offset = address & 3;
-	address = address & ~3;
 
 	do {
-		u32 nbytes = min_t(u32, size, MAIL_DATA_DWORDS * 4);
+		unsigned int offset;
+		size_t nbytes;
 		int ret;
 
+		offset = address & 3;
+		nbytes = min_t(size_t, size + offset, MAIL_DATA_DWORDS * 4);
+
 		ret = dma_port_flash_read_block(dma, address, dma->buf,
 						ALIGN(nbytes, 4));
 		if (ret) {

@@ -386,6 +386,7 @@ int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
 			return ret;
 		}
 
+		nbytes -= offset;
 		memcpy(buf, dma->buf + offset, nbytes);
 
 		size -= nbytes;

index 680bc738dd66..671d72af8ba1 100644
@@ -68,15 +68,15 @@ static int usb4_do_read_data(u16 address, void *buf, size_t size,
 	unsigned int retries = USB4_DATA_RETRIES;
 	unsigned int offset;
 
-	offset = address & 3;
-	address = address & ~3;
-
 	do {
-		size_t nbytes = min_t(size_t, size, USB4_DATA_DWORDS * 4);
 		unsigned int dwaddress, dwords;
 		u8 data[USB4_DATA_DWORDS * 4];
+		size_t nbytes;
 		int ret;
 
+		offset = address & 3;
+		nbytes = min_t(size_t, size + offset, USB4_DATA_DWORDS * 4);
+
 		dwaddress = address / 4;
 		dwords = ALIGN(nbytes, 4) / 4;
 

@@ -87,6 +87,7 @@ static int usb4_do_read_data(u16 address, void *buf, size_t size,
 			return ret;
 		}
 
+		nbytes -= offset;
 		memcpy(buf, data + offset, nbytes);
 
 		size -= nbytes;

index 29de6d95c6e7..3a5336913cca 100644
@@ -164,6 +164,101 @@ void tb_nvm_free(struct tb_nvm *nvm)
 	kfree(nvm);
 }
 
+/**
+ * tb_nvm_read_data() - Read data from NVM
+ * @address: Start address on the flash
+ * @buf: Buffer where the read data is copied
+ * @size: Size of the buffer in bytes
+ * @retries: Number of retries if block read fails
+ * @read_block: Function that reads block from the flash
+ * @read_block_data: Data passsed to @read_block
+ *
+ * This is a generic function that reads data from NVM or NVM like
+ * device.
+ *
+ * Returns %0 on success and negative errno otherwise.
+ */
+int tb_nvm_read_data(unsigned int address, void *buf, size_t size,
+		     unsigned int retries, read_block_fn read_block,
+		     void *read_block_data)
+{
+	do {
+		unsigned int dwaddress, dwords, offset;
+		u8 data[NVM_DATA_DWORDS * 4];
+		size_t nbytes;
+		int ret;
+
+		offset = address & 3;
+		nbytes = min_t(size_t, size + offset, NVM_DATA_DWORDS * 4);
+
+		dwaddress = address / 4;
+		dwords = ALIGN(nbytes, 4) / 4;
+
+		ret = read_block(read_block_data, dwaddress, data, dwords);
+		if (ret) {
+			if (ret != -ENODEV && retries--)
+				continue;
+			return ret;
+		}
+
+		nbytes -= offset;
+		memcpy(buf, data + offset, nbytes);
+
+		size -= nbytes;
+		address += nbytes;
+		buf += nbytes;
+	} while (size > 0);
+
+	return 0;
+}
+
+/**
+ * tb_nvm_write_data() - Write data to NVM
+ * @address: Start address on the flash
+ * @buf: Buffer where the data is copied from
+ * @size: Size of the buffer in bytes
+ * @retries: Number of retries if the block write fails
+ * @write_block: Function that writes block to the flash
+ * @write_block_data: Data passwd to @write_block
+ *
+ * This is generic function that writes data to NVM or NVM like device.
+ *
+ * Returns %0 on success and negative errno otherwise.
+ */
+int tb_nvm_write_data(unsigned int address, const void *buf, size_t size,
+		      unsigned int retries, write_block_fn write_block,
+		      void *write_block_data)
+{
+	do {
+		unsigned int offset, dwaddress;
+		u8 data[NVM_DATA_DWORDS * 4];
+		size_t nbytes;
+		int ret;
+
+		offset = address & 3;
+		nbytes = min_t(u32, size + offset, NVM_DATA_DWORDS * 4);
+
+		memcpy(data + offset, buf, nbytes);
+
+		dwaddress = address / 4;
+		ret = write_block(write_block_data, dwaddress, data, nbytes / 4);
+		if (ret) {
+			if (ret == -ETIMEDOUT) {
+				if (retries--)
+					continue;
+				ret = -EIO;
+			}
+			return ret;
+		}
+
+		size -= nbytes;
+		address += nbytes;
+		buf += nbytes;
+	} while (size > 0);
+
+	return 0;
+}
+
 void tb_nvm_exit(void)
 {
 	ida_destroy(&nvm_ida);

index 9790e9f13d2b..d9d1adc4cfd3 100644
@@ -20,6 +20,7 @@
 
 #define NVM_MIN_SIZE		SZ_32K
 #define NVM_MAX_SIZE		SZ_512K
+#define NVM_DATA_DWORDS		16
 
 /* Intel specific NVM offsets */
 #define NVM_DEVID		0x05

@@ -674,6 +675,16 @@ int tb_nvm_add_non_active(struct tb_nvm *nvm, size_t size,
 void tb_nvm_free(struct tb_nvm *nvm);
 void tb_nvm_exit(void);
 
+typedef int (*read_block_fn)(void *, unsigned int, void *, size_t);
+typedef int (*write_block_fn)(void *, unsigned int, const void *, size_t);
+
+int tb_nvm_read_data(unsigned int address, void *buf, size_t size,
+		     unsigned int retries, read_block_fn read_block,
+		     void *read_block_data);
+int tb_nvm_write_data(unsigned int address, const void *buf, size_t size,
+		      unsigned int retries, write_block_fn write_next_block,
+		      void *write_block_data);
+
 struct tb_switch *tb_switch_alloc(struct tb *tb, struct device *parent,
 				  u64 route);
 struct tb_switch *tb_switch_alloc_safe_mode(struct tb *tb,

194 lines not on this page · raw message

index 5631319f7b20..9f20c7bbf0ce 100644
@@ -299,15 +299,13 @@ static int dma_port_request(struct tb_dma_port *dma, u32 in,
 	return status_to_errno(out);
 }
 
-static int dma_port_flash_read_block(struct tb_dma_port *dma, u32 address,
-				     void *buf, u32 size)
+static int dma_port_flash_read_block(void *data, unsigned int dwaddress,
+				     void *buf, size_t dwords)
 {
+	struct tb_dma_port *dma = data;
 	struct tb_switch *sw = dma->sw;
-	u32 in, dwaddress, dwords;
 	int ret;
-
-	dwaddress = address / 4;
-	dwords = size / 4;
+	u32 in;
 
 	in = MAIL_IN_CMD_FLASH_READ << MAIL_IN_CMD_SHIFT;
 	if (dwords < MAIL_DATA_DWORDS)

@@ -323,14 +321,13 @@ static int dma_port_flash_read_block(struct tb_dma_port *dma, u32 address,
 			     dma->base + MAIL_DATA, dwords, DMA_PORT_TIMEOUT);
 }
 
-static int dma_port_flash_write_block(struct tb_dma_port *dma, u32 address,
-				      const void *buf, u32 size)
+static int dma_port_flash_write_block(void *data, unsigned int dwaddress,
+				      const void *buf, size_t dwords)
 {
+	struct tb_dma_port *dma = data;
 	struct tb_switch *sw = dma->sw;
-	u32 in, dwaddress, dwords;
 	int ret;
-
-	dwords = size / 4;
+	u32 in;
 
 	/* Write the block to MAIL_DATA registers */
 	ret = dma_port_write(sw->tb->ctl, buf, tb_route(sw), dma->port,

@@ -341,12 +338,8 @@ static int dma_port_flash_write_block(struct tb_dma_port *dma, u32 address,
 	in = MAIL_IN_CMD_FLASH_WRITE << MAIL_IN_CMD_SHIFT;
 
 	/* CSS header write is always done to the same magic address */
-	if (address >= DMA_PORT_CSS_ADDRESS) {
-		dwaddress = DMA_PORT_CSS_ADDRESS;
+	if (dwaddress >= DMA_PORT_CSS_ADDRESS)
 		in |= MAIL_IN_CSS;
-	} else {
-		dwaddress = address / 4;
-	}
 
 	in |= ((dwords - 1) << MAIL_IN_DWORDS_SHIFT) & MAIL_IN_DWORDS_MASK;
 	in |= (dwaddress << MAIL_IN_ADDRESS_SHIFT) & MAIL_IN_ADDRESS_MASK;

@@ -365,36 +358,8 @@ static int dma_port_flash_write_block(struct tb_dma_port *dma, u32 address,
 int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
 			void *buf, size_t size)
 {
-	unsigned int retries = DMA_PORT_RETRIES;
-
-	do {
-		unsigned int offset;
-		size_t nbytes;
-		int ret;
-
-		offset = address & 3;
-		nbytes = min_t(size_t, size + offset, MAIL_DATA_DWORDS * 4);
-
-		ret = dma_port_flash_read_block(dma, address, dma->buf,
-						ALIGN(nbytes, 4));
-		if (ret) {
-			if (ret == -ETIMEDOUT) {
-				if (retries--)
-					continue;
-				ret = -EIO;
-			}
-			return ret;
-		}
-
-		nbytes -= offset;
-		memcpy(buf, dma->buf + offset, nbytes);
-
-		size -= nbytes;
-		address += nbytes;
-		buf += nbytes;
-	} while (size > 0);
-
-	return 0;
+	return tb_nvm_read_data(address, buf, size, DMA_PORT_RETRIES,
+				dma_port_flash_read_block, dma);
 }
 
 /**

@@ -411,40 +376,11 @@ int dma_port_flash_read(struct tb_dma_port *dma, unsigned int address,
 int dma_port_flash_write(struct tb_dma_port *dma, unsigned int address,
 			 const void *buf, size_t size)
 {
-	unsigned int retries = DMA_PORT_RETRIES;
-	unsigned int offset;
-
-	if (address >= DMA_PORT_CSS_ADDRESS) {
-		offset = 0;
-		if (size > DMA_PORT_CSS_MAX_SIZE)
-			return -E2BIG;
-	} else {
-		offset = address & 3;
-		address = address & ~3;
-	}
-
-	do {
-		u32 nbytes = min_t(u32, size, MAIL_DATA_DWORDS * 4);
-		int ret;
+	if (address >= DMA_PORT_CSS_ADDRESS && size > DMA_PORT_CSS_MAX_SIZE)
+		return -E2BIG;
 
-		memcpy(dma->buf + offset, buf, nbytes);
-
-		ret = dma_port_flash_write_block(dma, address, buf, nbytes);
-		if (ret) {
-			if (ret == -ETIMEDOUT) {
-				if (retries--)
-					continue;
-				ret = -EIO;
-			}
-			return ret;
-		}
-
-		size -= nbytes;
-		address += nbytes;
-		buf += nbytes;
-	} while (size > 0);
-
-	return 0;
+	return tb_nvm_write_data(address, buf, size, DMA_PORT_RETRIES,
+				 dma_port_flash_write_block, dma);
 }
 
 /**