From bc57871a2e4510d38a8716a0a159fa34f3411b18 Mon Sep 17 00:00:00 2001
From: Jan Pohanka <xhpohanka@gmail.com>
Date: Tue, 14 Feb 2017 13:35:56 +0100
Subject: [PATCH] MSP430 BSL needs even counts and addresses

According to (not very good) documentation and empiric testing
BSL expects sizes of buffers sends to both sides to be even.
The same applies to memory addresses.
---
 drivers/rom_bsl.c | 58 ++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 47 insertions(+), 11 deletions(-)

diff --git a/drivers/rom_bsl.c b/drivers/rom_bsl.c
index f7bf353..b465337 100644
--- a/drivers/rom_bsl.c
+++ b/drivers/rom_bsl.c
@@ -96,9 +96,17 @@ static int send_command(struct rom_bsl_device *dev,
 	uint8_t pktbuf[256];
 	uint8_t cklow = 0xff;
 	uint8_t ckhigh = 0xff;
-	int pktlen = data ? len + 4 : 4;
+	int pktlen;
+	int evenlen = len;
 	int i;
 
+	if (len % 2 != 0) {
+	    printc_dbg("Making length even\n");
+	    evenlen = len + 1;
+	}
+
+	pktlen = data ? evenlen + 4 : 4;
+
 	if (pktlen + 6 > sizeof(pktbuf)) {
 		printc_err("rom_bsl: payload too large: %d\n", len);
 		return -1;
@@ -110,11 +118,14 @@ static int send_command(struct rom_bsl_device *dev,
 	pktbuf[3] = pktlen;
 	pktbuf[4] = addr & 0xff;
 	pktbuf[5] = addr >> 8;
-	pktbuf[6] = len & 0xff;
-	pktbuf[7] = len >> 8;
+	pktbuf[6] = evenlen & 0xff;
+	pktbuf[7] = evenlen >> 8;
 
-	if (data)
+	if (data) {
 		memcpy(pktbuf + 8, data, len);
+		if (len != evenlen)
+		    pktbuf[8 + len] = 0xff;
+	}
 
 	for (i = 0; i < pktlen + 4; i += 2)
 		cklow ^= pktbuf[i];
@@ -294,8 +305,25 @@ static int rom_bsl_writemem(device_t dev_base,
 	while (len) {
 		int wlen = len > 100 ? 100 : len;
 		int r;
+		uint8_t memtmp[256];
+		const uint8_t *memptr;
 
-		r = rom_bsl_xfer(dev, CMD_RX_DATA, addr, mem, wlen);
+		if (addr % 2) {
+		    printc_dbg("Memory aligning\n");
+		    memcpy(memtmp + 1, mem, wlen);
+		    memtmp[0] = 0xff;
+		    memptr = memtmp;
+
+		    wlen++;
+		    len++;
+		    addr--;
+		    mem--;
+		}
+		else {
+		    memptr = mem;
+		}
+
+		r = rom_bsl_xfer(dev, CMD_RX_DATA, addr, memptr, wlen);
 
 		if (r < 0) {
 			printc_err("rom_bsl: failed to write to 0x%04x\n",
@@ -323,9 +351,17 @@ static int rom_bsl_readmem(device_t dev_base,
 
 	while (len) {
 		address_t count = len;
+		int align = 0;
 
-		if (count > 128)
-			count = 128;
+		if (addr % 2 != 0) {
+		    printc_dbg("Memory aligning\n");
+		    count++;
+		    addr--;
+		    align = 1;
+		}
+
+		if (count > 220)
+			count = 220;
 
 		if (rom_bsl_xfer(dev, CMD_TX_DATA, addr, NULL, count) < 0) {
 			printc_err("rom_bsl: failed to read memory\n");
@@ -335,10 +371,10 @@ static int rom_bsl_readmem(device_t dev_base,
 		if (count > dev->reply_buf[2])
 			count = dev->reply_buf[2];
 
-		memcpy(mem, dev->reply_buf + 4, count);
-		mem += count;
-		len -= count;
-		addr += count;
+		memcpy(mem, dev->reply_buf + 4 + align, count - align);
+		mem += (count - align);
+		len -= (count - align);
+		addr += (count - align);
 	}
 
 	return 0;