added common readmem/writemem implementation with alignment and memory range checking

changed pif and goodfet to use common readmem/writemem

drivers/device.c

@@ -19,7 +19,6 @@
 #include <string.h>
 #include "output.h"
 #include "device.h"
-#include "bytes.h"
 
 device_t device_default;
 
@@ -256,3 +255,215 @@ int device_erase(device_erase_type_t et, address_t addr)
 
 	return device_default->type->erase(device_default, et, addr);
 }
+
+static const struct chipinfo default_chip = {
+		.name		= "DefaultChip",
+		.bits		= 20,
+		.memory		= {
+			{
+				.name		= "DefaultFlash",
+				.type		= CHIPINFO_MEMTYPE_FLASH,
+				.bits		= 20,
+				.mapped		= 1,
+				.size		= 0xff000,
+				.offset		= 0x01000,
+				.seg_size	= 0,
+				.bank_size	= 0,
+				.banks		= 1,
+			},
+			{
+				.name		= "DefaultRam",
+				.type		= CHIPINFO_MEMTYPE_RAM,
+				.bits		= 20,
+				.mapped		= 1,
+				.size		= 0x01000,
+				.offset		= 0x00000,
+				.seg_size	= 0,
+				.bank_size	= 0,
+				.banks		= 1,
+			},
+			{0}
+		},
+	};
+
+/* Given an address range, specified by a start and a size (in bytes),
+ * return a size which is trimmed so as to not overrun a region boundary
+ * in the chip's memory map.
+ *
+ * The single region occupied is optionally returned in m_ret. If the
+ * range doesn't start in a valid region, it's trimmed to the start of
+ * the next valid region, and m_ret is NULL.
+ */
+address_t check_range(const struct chipinfo *chip,
+			     address_t addr, address_t size,
+			     const struct chipinfo_memory **m_ret)
+{
+	if (!chip) {
+		chip = &default_chip;
+	}
+
+	const struct chipinfo_memory *m =
+		chipinfo_find_mem_by_addr(chip, addr);
+
+	if (m) {
+		if (m->offset > addr) {
+			address_t n = m->offset - addr;
+
+			if (size > n)
+				size = n;
+
+			m = NULL;
+		} else if (addr + size > m->offset + m->size) {
+			size = m->offset + m->size - addr;
+		}
+	}
+
+	*m_ret = m;
+
+	return size;
+}
+
+/* Read bytes from device taking care of memory types.
+ * Function read_words is only called for existing memory ranges and
+ * with a word aligned address.
+ * Non-existing memory locations read as 0x55.
+ * returns 0 on success, -1 on failure
+ */
+int readmem(device_t dev, address_t addr,
+		uint8_t *mem, address_t len,
+		int (*read_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			uint8_t *data)
+		)
+{
+	const struct chipinfo_memory *m;
+
+	if (!len)
+		return 0;
+
+	/* Handle unaligned start */
+	if (addr & 1) {
+		uint8_t data[2];
+		check_range(dev->chip, addr - 1, 2, &m);
+		if (!m)
+			data[1] = 0x55;
+		else if (read_words(dev, m, addr - 1, 2, data) < 0)
+			return -1;
+
+		mem[0] = data[1];
+		addr++;
+		mem++;
+		len--;
+	}
+
+	/* Read aligned blocks */
+	while (len >= 2) {
+		int rlen = check_range(dev->chip, addr, len & ~1, &m);
+		if (!m)
+			memset(mem, 0x55, rlen);
+		else {
+			rlen = read_words(dev, m, addr, rlen, mem);
+			if (rlen < 0)
+				return -1;
+		}
+
+		addr += rlen;
+		mem += rlen;
+		len -= rlen;
+	}
+
+	/* Handle unaligned end */
+	if (len) {
+		uint8_t data[2];
+		check_range(dev->chip, addr, 2, &m);
+		if (!m)
+			data[0] = 0x55;
+		else if (read_words(dev, m, addr, 2, data) < 0)
+			return -1;
+
+		mem[0] = data[0];
+	}
+
+	return 0;
+}
+
+/* Write bytes to device taking care of memory types.
+ * Functions write_words and read_words are only called for existing memory ranges and
+ * with a word aligned address and length.
+ * Writes to non-existing memory locations fail.
+ * returns 0 on success, -1 on failure
+ */
+int writemem(device_t dev, address_t addr,
+		const uint8_t *mem, address_t len,
+		int (*write_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			const uint8_t *data),
+		int (*read_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			uint8_t *data)
+		)
+{
+	const struct chipinfo_memory *m;
+
+	if (!len)
+		return 0;
+
+	/* Handle unaligned start */
+	if (addr & 1) {
+		uint8_t data[2];
+		check_range(dev->chip, addr - 1, 2, &m);
+		if (!m)
+			goto fail; // fail on unmapped regions
+
+		if (read_words(dev, m, addr - 1, 2, data) < 0)
+			return -1;
+
+		data[1] = mem[0];
+
+		if (write_words(dev, m, addr - 1, 2, data) < 0)
+			return -1;
+
+		addr++;
+		mem++;
+		len--;
+	}
+
+	while (len >= 2) {
+		int wlen = check_range(dev->chip, addr, len & ~1, &m);
+		if (!m)
+			goto fail; // fail on unmapped regions
+
+		wlen = write_words(dev, m, addr, wlen, mem);
+		if (wlen < 0)
+			return -1;
+
+		addr += wlen;
+		mem += wlen;
+		len -= wlen;
+	}
+
+	/* Handle unaligned end */
+	if (len) {
+		uint8_t data[2];
+		check_range(dev->chip, addr, 2, &m);
+		if (!m)
+			goto fail; // fail on unmapped regions
+
+		if (read_words(dev, m, addr, 2, data) < 0)
+			return -1;
+
+		data[0] = mem[0];
+
+		if (write_words(dev, m, addr, 2, data) < 0)
+			return -1;
+	}
+
+	return 0;
+
+fail:
+	printc_err("writemem failed at 0x%x\n", addr);
+	return -1;
+}

drivers/device.h

@@ -23,6 +23,7 @@
 #include "util.h"
 #include "powerbuf.h"
 #include "chipinfo.h"
+#include "bytes.h"
 
 struct device;
 typedef struct device *device_t;
@@ -182,4 +183,28 @@ extern device_t device_default;
 
 int device_erase(device_erase_type_t et, address_t addr);
 
+address_t check_range(const struct chipinfo *chip,
+			     address_t addr, address_t size,
+			     const struct chipinfo_memory **m_ret);
+
+int readmem(device_t dev, address_t addr,
+		uint8_t *mem, address_t len,
+		int (*read_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			uint8_t *data)
+		);
+
+int writemem(device_t dev, address_t addr,
+		const uint8_t *mem, address_t len,
+		int (*write_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			const uint8_t *data),
+		int (*read_words)(device_t dev,
+			const struct chipinfo_memory *m,
+			address_t addr, address_t len,
+			uint8_t *data)
+		);
+
 #endif

drivers/goodfet.c

@@ -67,6 +67,14 @@
 #define MAX_LEN			1024
 #define MAX_MEM_BLOCK		128
 
+
+struct goodfet {
+	struct device		base;
+
+	sport_t			serial_fd;
+};
+
+
 /************************************************************************
  * GoodFET protocol handling
  */
@@ -200,13 +208,20 @@ fail:
  * GoodFET MSP430 JTAG operations
  */
 
-/* Read a word-aligned block from any kind of memory. */
+/* Read a word-aligned block from any kind of memory.
-static int read_words(sport_t fd, address_t addr,
+ * returns the number of bytes read or -1 on failure
-		      address_t len, uint8_t *data)
+ */
+static int read_words(device_t dev, const struct chipinfo_memory *m,
+		address_t addr, address_t len, uint8_t *data)
 {
+	struct goodfet *gc = (struct goodfet *)dev;
+	sport_t fd = gc->serial_fd;
 	struct packet pkt;
 	uint8_t req[6];
 
+	if (len > MAX_MEM_BLOCK)
+		len = MAX_MEM_BLOCK;
+
 	req[0] = addr;
 	req[1] = addr >> 8;
 	req[2] = addr >> 16;
@@ -227,7 +242,7 @@ static int read_words(sport_t fd, address_t addr,
 	}
 
 	memcpy(data, pkt.data, pkt.len);
-	return 0;
+	return len;
 }
 
 /* Write a word to RAM. */
@@ -277,32 +292,32 @@ static int write_flash_block(sport_t fd, address_t addr,
 	return 0;
 }
 
-/* Write a single byte by reading and rewriting a word. */
+/* Write a word-aligned block to any kind of memory.
-static int write_byte(sport_t fd, address_t addr, uint8_t value)
+ * returns the number of bytes written or -1 on failure
+ */
+static int write_words(device_t dev, const struct chipinfo_memory *m,
+		address_t addr, address_t len, const uint8_t *data)
 {
-	address_t aligned = addr & ~1;
+	struct goodfet *gc = (struct goodfet *)dev;
-	uint8_t data[2];
+	sport_t fd = gc->serial_fd;
 	int r;
 
-	if (read_words(fd, aligned, 2, data) < 0)
+	if (len > MAX_MEM_BLOCK)
-		goto fail;
+		len = MAX_MEM_BLOCK;
 
-	data[addr & 1] = value;
+	if (m->type != CHIPINFO_MEMTYPE_FLASH) {
+		len = 2;
+		r = write_ram_word(fd, addr, r16le(data));
+	} else {
+		r = write_flash_block(fd, addr, len, data);
+	}
 
-	if (addr < 0x1000)
+	if (r < 0) {
-		r = write_ram_word(fd, aligned,
+		printc_err("goodfet: write_words at address 0x%x failed\n", addr);
-				   ((uint16_t)data[0]) |
+		return -1;
-				   (((uint16_t)data[1]) << 8));
+	}
-	else
-		r = write_flash_block(fd, aligned, 2, data);
 
-	if (r < 0)
+	return len;
-		goto fail;
-
-	return 0;
-fail:
-	printc_err("good_fet: write_byte at address 0x%x failed\n", addr);
-	return -1;
 }
 
 static int init_device(sport_t fd)
@@ -358,120 +373,16 @@ static int init_device(sport_t fd)
  * MSPDebug Device interface
  */
 
-struct goodfet {
-	struct device		base;
-
-	sport_t			serial_fd;
-};
-
 static int goodfet_readmem(device_t dev_base, address_t addr,
 			   uint8_t *mem, address_t len)
 {
-	struct goodfet *gc = (struct goodfet *)dev_base;
+	return readmem(dev_base, addr, mem, len, read_words);
-
-	if (!len)
-		return 0;
-
-	/* Handle unaligned start */
-	if (addr & 1) {
-		uint8_t data[2];
-
-		if (read_words(gc->serial_fd, addr ^ 1, 2, data) < 0)
-			goto fail;
-
-		mem[0] = data[1];
-		addr++;
-		mem++;
-		len--;
-	}
-
-	/* Read aligned blocks */
-	while (len >= 2) {
-		int plen = MAX_MEM_BLOCK;
-
-		if (plen > len)
-			plen = len;
-		plen &= ~1;
-
-		if (read_words(gc->serial_fd, addr, plen, mem) < 0)
-			goto fail;
-
-		addr += plen;
-		mem += plen;
-		len -= plen;
-	}
-
-	/* Handle unaligned end */
-	if (len) {
-		uint8_t data[2];
-
-		if (read_words(gc->serial_fd, addr, 2, data) < 0)
-			goto fail;
-
-		mem[0] = data[0];
-	}
-
-	return 0;
-
-fail:
-	printc_err("goodfet: readmem failed at 0x%x\n", addr);
-	return -1;
 }
 
 static int goodfet_writemem(device_t dev_base, address_t addr,
 			    const uint8_t *mem, address_t len)
 {
-	struct goodfet *gc = (struct goodfet *)dev_base;
+	return writemem(dev_base, addr, mem, len, write_words, read_words);
-
-	if (!len)
-		return 0;
-
-	/* Handle unaligned start */
-	if (addr & 1) {
-		if (write_byte(gc->serial_fd, addr, mem[0]) < 0)
-			goto fail;
-
-		addr++;
-		mem++;
-		len--;
-	}
-
-	while (len >= 2) {
-		if (addr < 0x1000) {
-			if (write_ram_word(gc->serial_fd, addr,
-					   ((uint16_t)mem[0]) |
-					   (((uint16_t)mem[1]) << 8)) < 0)
-				goto fail;
-
-			addr += 2;
-			mem += 2;
-			len -= 2;
-		} else {
-			int plen = MAX_MEM_BLOCK;
-
-			if (plen > len)
-				plen = len;
-			plen &= ~1;
-
-			if (write_flash_block(gc->serial_fd, addr,
-					      plen, mem) < 0)
-				goto fail;
-
-			addr += plen;
-			mem += plen;
-			len -= plen;
-		}
-	}
-
-	/* Handle unaligned end */
-	if (len && (write_byte(gc->serial_fd, addr, mem[0]) < 0))
-		goto fail;
-
-	return 0;
-
-fail:
-	printc_err("goodfet: writemem failed at 0x%x\n", addr);
-	return -1;
 }
 
 static int goodfet_setregs(device_t dev_base, const address_t *regs)

drivers/pif.c

@@ -34,15 +34,23 @@
 #include "jtaglib.h"
 #include "ctrlc.h"
 
+struct pif_device {
+  struct device		base;
+  struct jtdev		jtag;
+};
+
 /*============================================================================*/
 /* pif MSP430 JTAG operations */
 
 /*----------------------------------------------------------------------------*/
-/* Read a word-aligned block from any kind of memory */
+/* Read a word-aligned block from any kind of memory
-static int read_words( struct jtdev *p, address_t addr,
+ * returns the number of bytes read or -1 on failure
-		       address_t len,
+ */
-		       uint8_t*  data )
+static int read_words(device_t dev, const struct chipinfo_memory *m,
+		address_t addr, address_t len, uint8_t *data)
 {
+  struct pif_device *pif = (struct pif_device *)dev;
+  struct jtdev *p = &pif->jtag;
   unsigned int index;
   unsigned int word;
 
@@ -52,18 +60,15 @@ static int read_words( struct jtdev *p, address_t addr,
     data[index+1] = (word >> 8) & 0x00ff;
   }
 
-  return p->failed ? -1 : 0;
+  return p->failed ? -1 : len;
 }
 
 /*----------------------------------------------------------------------------*/
 /* Write a word to RAM */
-int write_ram_word( struct jtdev *p, address_t addr,
+static int write_ram_word( struct jtdev *p, address_t addr,
 		    uint16_t  value )
 {
-  unsigned int word;
+  jtag_write_mem( p, 16, addr, value );
-
-  word = ((value & 0x00ff) << 8) | ((value & 0xff00) >> 8);
-  jtag_write_mem( p, 16, addr, word );
 
   return p->failed ? -1 : 0;
 }
@@ -95,29 +100,29 @@ static int write_flash_block( struct jtdev *p, address_t addr,
   return p->failed ? -1 : 0;
 }
 
-/*----------------------------------------------------------------------------*/
+/* Write a word-aligned block to any kind of memory.
-/* Write a single byte by reading and rewriting a word. */
+ * returns the number of bytes written or -1 on failure
-static int write_byte( struct jtdev *p,
+ */
-		       address_t addr,
+static int write_words(device_t dev, const struct chipinfo_memory *m,
-		       uint8_t   value )
+		address_t addr, address_t len, const uint8_t *data)
 {
-  address_t aligned = addr & ~1;
+  struct pif_device *pif = (struct pif_device *)dev;
-  uint8_t data[2];
+  struct jtdev *p = &pif->jtag;
-  unsigned int word;
+  int r;
 
-  read_words(p, aligned, 2, data);
+  if (m->type != CHIPINFO_MEMTYPE_FLASH) {
-  data[addr & 1] = value;
+    len = 2;
-
+    r = write_ram_word(p, addr, r16le(data));
-  if ( ADDR_IN_FLASH(addr) ) {
-    /* program in FLASH */
-    write_flash_block(p, aligned, 2, data);
   } else {
-    /* write to RAM */
+    r = write_flash_block(p, addr, len, data);
-    word = (uint16_t)data[1] | ((uint16_t)data[0] << 8);
-    write_ram_word(p, aligned, word);
   }
 
-  return p->failed ? -1 : 0;
+  if (r < 0) {
+    printc_err("pif: write_words at address 0x%x failed\n", addr);
+    return -1;
+  }
+
+  return len;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -139,11 +144,6 @@ static int init_device(struct jtdev *p)
 
 /*===== MSPDebug Device interface ============================================*/
 
-struct pif_device {
-  struct device		base;
-  struct jtdev		jtag;
-};
-
 /*----------------------------------------------------------------------------*/
 static int refresh_bps(struct pif_device *dev)
 {
@@ -188,38 +188,8 @@ static int pif_readmem( device_t  dev_base,
 			address_t len )
 {
   struct pif_device *dev = (struct pif_device *)dev_base;
-  uint8_t  data[2];
-
   dev->jtag.failed = 0;
-
+  return readmem(dev_base, addr, mem, len, read_words);
-  if ( len > 0 ) {
-    /* Handle unaligned start */
-    if (addr & 1) {
-      if (read_words(&dev->jtag, addr & ~1, 2, data) < 0)
-	return -1;
-      mem[0] = data[1];
-      addr++;
-      mem++;
-      len--;
-    }
-
-    /* Read aligned blocks */
-    if (len >= 2) {
-      if (read_words(&dev->jtag, addr, len & ~1, mem) < 0)
-	return -1;
-      addr += len & ~1;
-      mem += len & ~1;
-      len &= 1;
-    }
-
-    /* Handle unaligned end */
-    if (len == 1) {
-      if (read_words(&dev->jtag, addr, 2, data) < 0)
-	return -1;
-      mem[0] = data[0];
-    }
-  }
-  return 0;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -229,45 +199,8 @@ static int pif_writemem( device_t       dev_base,
 			 address_t      len )
 {
   struct pif_device *dev = (struct pif_device *)dev_base;
-
   dev->jtag.failed = 0;
-
+  return writemem(dev_base, addr, mem, len, write_words, read_words);
-  if (len > 0)
-  {
-    /* Handle unaligned start */
-    if (addr & 1) {
-      if (write_byte(&dev->jtag, addr, mem[0]) < 0)
-	return -1;
-      addr++;
-      mem++;
-      len--;
-    }
-
-    /* Write aligned blocks */
-    while (len >= 2) {
-      if ( ADDR_IN_FLASH(addr) ) {
-	if (write_flash_block(&dev->jtag, addr, len & ~1, mem) < 0)
-	  return -1;
-	addr += len & ~1;
-	mem += len & ~1;
-	len &= 1;
-      } else {
-	if (write_ram_word(&dev->jtag, addr,
-		(uint16_t)mem[1] | ((uint16_t)mem[0] << 8)) < 0)
-	  return -1;
-	addr += 2;
-	mem  += 2;
-	len  -= 2;
-      }
-    }
-
-    /* Handle unaligned end */
-    if (len == 1) {
-      if (write_byte(&dev->jtag, addr, mem[0]) < 0)
-	return -1;
-    }
-  }
-  return 0;
 }
 
 /*----------------------------------------------------------------------------*/

drivers/pif.h

@@ -28,8 +28,6 @@
 
 #include "device.h"
 
-#define ADDR_IN_FLASH(a)        ( (a) >= 0x1000 )
-
 /* pif implementation */
 extern const struct device_class device_pif;
 /* share wiht gpio implementation */

drivers/v3hil.c

@@ -136,40 +136,6 @@ static hal_proto_fid_t map_fid(const struct v3hil *h, hal_proto_fid_t src)
 	return dst ? dst : src;
 }
 
-/* Given an address range, specified by a start and a size (in bytes),
- * return a size which is trimmed so as to not overrun a region boundary
- * in the chip's memory map.
- *
- * The single region occupied is optionally returned in m_ret. If the
- * range doesn't start in a valid region, it's trimmed to the start of
- * the next valid region, and m_ret is NULL.
- */
-static address_t check_range(const struct chipinfo *chip,
-			     address_t addr, address_t size,
-			     const struct chipinfo_memory **m_ret)
-{
-	const struct chipinfo_memory *m =
-		chipinfo_find_mem_by_addr(chip, addr);
-
-	if (m) {
-		if (m->offset > addr) {
-			address_t n = m->offset - addr;
-
-			if (size > n)
-				size = n;
-
-			m = NULL;
-		} else if (addr + size > m->offset + m->size) {
-			size = m->offset + m->size - addr;
-		}
-	}
-
-	if (m)
-		*m_ret = m;
-
-	return size;
-}
-
 void v3hil_init(struct v3hil *h, transport_t trans,
 		hal_proto_flags_t flags)
 {