altos: Move common storage code to ao_storage.c. Add M25P80 driver

This reworks the storage API so that you erase blocks and then store
data to them so that the M25P80 driver will work.

Signed-off-by: Keith Packard <keithp@keithp.com>

11 files changed, 662 insertions, 274 deletions

diff --git a/src/Makefile.proto b/src/Makefile.proto
index c79638ac..709cbca7 100644
--- a/src/Makefile.proto
+++ b/src/Makefile.proto
@@ -94,15 +94,25 @@ TM_DRIVER_SRC = \
 #
 # 25LC1024 driver source
 EE_DRIVER_SRC = \
+	ao_storage.c \
 	ao_ee.c
 
 #
 # AT45DB161D driver source
 
 FLASH_DRIVER_SRC = \
+	ao_storage.c \
 	ao_flash.c
 
 #
+# Numonyx M25P80 driver source
+#
+
+M25_DRIVER_SRC = \
+	ao_storage.c \
+	ao_m25.c
+
+#
 # SiRF driver source
 #
 SIRF_DRIVER_SRC = \
diff --git a/src/ao.h b/src/ao.h
index f23e4134..e1306c79 100644
--- a/src/ao.h
+++ b/src/ao.h
@@ -443,16 +443,15 @@ extern __xdata uint32_t	ao_storage_block;
 /* Byte offset of config block. Will be ao_storage_block bytes long */
 extern __xdata uint32_t	ao_storage_config;
 
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+extern __xdata uint16_t ao_storage_unit;
+
 #define AO_STORAGE_ERASE_LOG	(ao_storage_config + AO_CONFIG_MAX_SIZE)
 
 /* Initialize above values. Can only be called once the OS is running */
 void
 ao_storage_setup(void);
 
-/* Flush any pending write data */
-void
-ao_storage_flush(void) __reentrant;
-
 /* Write data. Returns 0 on failure, 1 on success */
 uint8_t
 ao_storage_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant;
@@ -463,13 +462,37 @@ ao_storage_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant;
 
 /* Erase a block of storage. This always clears ao_storage_block bytes */
 uint8_t
-ao_storage_erase(uint32_t pos);
+ao_storage_erase(uint32_t pos) __reentrant;
+
+/* Flush any pending writes to stable storage */
+void
+ao_storage_flush(void) __reentrant;
 
 /* Initialize the storage code */
 void
 ao_storage_init(void);
 
 /*
+ * Low-level functions wrapped by ao_storage.c
+ */
+
+/* Read data within a storage unit */
+uint8_t
+ao_storage_device_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Write data within a storage unit */
+uint8_t
+ao_storage_device_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant;
+
+/* Initialize low-level device bits */
+void
+ao_storage_device_init(void);
+
+/* Print out information about flash chips */
+void
+ao_storage_device_info(void) __reentrant;
+
+/*
  * ao_log.c
  */
 
@@ -581,14 +604,6 @@ ao_log_flush(void);
  */
 extern __xdata uint16_t ao_flight_number;
 
-/* Retrieve first log record for the current flight */
-uint8_t
-ao_log_dump_first(void);
-
-/* return next log record for the current flight */
-uint8_t
-ao_log_dump_next(void);
-
 /* Logging thread main routine */
 void
 ao_log(void);
@@ -605,6 +620,10 @@ ao_log_stop(void);
 void
 ao_log_init(void);
 
+/* Write out the current flight number to the erase log */
+void
+ao_log_write_erase(uint8_t pos);
+
 /*
  * ao_flight.c
  */
diff --git a/src/ao_config.c b/src/ao_config.c
index 2673e472..38b72798 100644
--- a/src/ao_config.c
+++ b/src/ao_config.c
@@ -33,7 +33,9 @@ static void
 _ao_config_put(void)
 {
 	ao_storage_setup();
+	ao_storage_erase(ao_storage_config);
 	ao_storage_write(ao_storage_config, &ao_config, sizeof (ao_config));
+	ao_log_write_erase(0);
 	ao_storage_flush();
 }
 
diff --git a/src/ao_ee.c b/src/ao_ee.c
index 575a7b30..7de05b7b 100644
--- a/src/ao_ee.c
+++ b/src/ao_ee.c
@@ -30,6 +30,9 @@ __xdata uint32_t	ao_storage_block;
 /* Byte offset of config block. Will be ao_storage_block bytes long */
 __xdata uint32_t	ao_storage_config;
 
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__xdata uint16_t	ao_storage_unit;
+
 /*
  * Using SPI on USART 0, with P1_2 as the chip select
  */
@@ -161,80 +164,34 @@ ao_ee_fill(uint16_t block)
 }
 
 uint8_t
-ao_storage_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+ao_storage_device_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
 {
-	uint16_t block;
-	uint16_t this_len;
-	uint8_t	this_off;
-
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
-		return 0;
-	while (len) {
-
-		/* Compute portion of transfer within
-		 * a single block
-		 */
-		this_off = pos;
-		this_len = EE_BLOCK_SIZE - (uint16_t) this_off;
-		block = (uint16_t) (pos >> 8);
-		if (this_len > len)
-			this_len = len;
-		if (this_len & 0xff00)
-			ao_panic(AO_PANIC_EE);
-
-		/* Transfer the data */
-		ao_mutex_get(&ao_ee_mutex); {
-			if (this_len != EE_BLOCK_SIZE)
-				ao_ee_fill(block);
-			else {
-				ao_ee_flush_internal();
-				ao_ee_block = block;
-			}
-			memcpy(ao_ee_data + this_off, buf, this_len);
-			ao_ee_block_dirty = 1;
-		} ao_mutex_put(&ao_ee_mutex);
-
-		/* See how much is left */
-		buf += this_len;
-		len -= this_len;
-		pos += this_len;
-	}
+	uint16_t block = (uint16_t) (pos >> 8);
+
+	/* Transfer the data */
+	ao_mutex_get(&ao_ee_mutex); {
+		if (len != EE_BLOCK_SIZE)
+			ao_ee_fill(block);
+		else {
+			ao_ee_flush_internal();
+			ao_ee_block = block;
+		}
+		memcpy(ao_ee_data + (uint16_t) (pos & 0xff), buf, len);
+		ao_ee_block_dirty = 1;
+	} ao_mutex_put(&ao_ee_mutex);
 	return 1;
 }
 
 uint8_t
-ao_storage_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+ao_storage_device_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
 {
-	uint16_t block;
-	uint16_t this_len;
-	uint8_t	this_off;
-
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
-		return 0;
-	while (len) {
-
-		/* Compute portion of transfer within
-		 * a single block
-		 */
-		this_off = pos;
-		this_len = EE_BLOCK_SIZE - (uint16_t) this_off;
-		block = (uint16_t) (pos >> 8);
-		if (this_len > len)
-			this_len = len;
-		if (this_len & 0xff00)
-			ao_panic(AO_PANIC_EE);
-
-		/* Transfer the data */
-		ao_mutex_get(&ao_ee_mutex); {
-			ao_ee_fill(block);
-			memcpy(buf, ao_ee_data + this_off, this_len);
-		} ao_mutex_put(&ao_ee_mutex);
+	uint16_t block = (uint16_t) (pos >> 8);
 
-		/* See how much is left */
-		buf += this_len;
-		len -= this_len;
-		pos += this_len;
-	}
+	/* Transfer the data */
+	ao_mutex_get(&ao_ee_mutex); {
+		ao_ee_fill(block);
+		memcpy(buf, ao_ee_data + (uint16_t) (pos & 0xff), len);
+	} ao_mutex_put(&ao_ee_mutex);
 	return 1;
 }
 
@@ -259,66 +216,10 @@ ao_storage_erase(uint32_t pos) __reentrant
 }
 
 static void
-ee_dump(void) __reentrant
-{
-	static __xdata uint8_t	b;
-	uint16_t block;
-	uint8_t i;
-
-	ao_cmd_hex();
-	block = ao_cmd_lex_i;
-	if (ao_cmd_status != ao_cmd_success)
-		return;
-	i = 0;
-	do {
-		if ((i & 7) == 0) {
-			if (i)
-				putchar('\n');
-			ao_cmd_put16((uint16_t) i);
-		}
-		putchar(' ');
-		ao_storage_read(((uint32_t) block << 8) | i, &b, 1);
-		ao_cmd_put8(b);
-		++i;
-	} while (i != 0);
-	putchar('\n');
-}
-
-static void
 ee_store(void) __reentrant
 {
-	uint16_t block;
-	uint8_t i;
-	uint16_t len;
-	static __xdata uint8_t b;
-	uint32_t addr;
-
-	ao_cmd_hex();
-	block = ao_cmd_lex_i;
-	ao_cmd_hex();
-	i = ao_cmd_lex_i;
-	addr = ((uint32_t) block << 8) | i;
-	ao_cmd_hex();
-	len = ao_cmd_lex_i;
-	if (ao_cmd_status != ao_cmd_success)
-		return;
-	while (len--) {
-		ao_cmd_hex();
-		if (ao_cmd_status != ao_cmd_success)
-			return;
-		b = ao_cmd_lex_i;
-		ao_storage_write(addr, &b, 1);
-		addr++;
-	}
-	ao_storage_flush();
 }
 
-__code struct ao_cmds ao_ee_cmds[] = {
-	{ 'e', ee_dump, 	"e <block>                          Dump a block of EEPROM data" },
-	{ 'w', ee_store,	"w <block> <start> <len> <data> ... Write data to EEPROM" },
-	{ 0,   ee_store, NULL },
-};
-
 void
 ao_storage_setup(void)
 {
@@ -326,20 +227,24 @@ ao_storage_setup(void)
 		ao_storage_total = EE_DEVICE_SIZE;
 		ao_storage_block = EE_BLOCK_SIZE;
 		ao_storage_config = EE_DEVICE_SIZE - EE_BLOCK_SIZE;
+		ao_storage_unit = EE_BLOCK_SIZE;
 	}
 }
 
+void
+ao_storage_device_info(void) __reentrant
+{
+}
+
 /*
  * To initialize the chip, set up the CS line and
  * the SPI interface
  */
 void
-ao_storage_init(void)
+ao_storage_device_init(void)
 {
 	/* set up CS */
 	EE_CS = 1;
 	P1DIR |= (1 << EE_CS_INDEX);
 	P1SEL &= ~(1 << EE_CS_INDEX);
-
-	ao_cmd_register(&ao_ee_cmds[0]);
 }
diff --git a/src/ao_flash.c b/src/ao_flash.c
index bc8b56ad..1201a0e5 100644
--- a/src/ao_flash.c
+++ b/src/ao_flash.c
@@ -27,6 +27,9 @@ __xdata uint32_t	ao_storage_block;
 /* Byte offset of config block. Will be ao_storage_block bytes long */
 __xdata uint32_t	ao_storage_config;
 
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__xdata uint16_t	ao_storage_unit;
+
 #define FLASH_CS		P1_1
 #define FLASH_CS_INDEX		1
 
@@ -162,9 +165,11 @@ ao_storage_setup(void)
 		ao_panic(AO_PANIC_FLASH);
 	}
 	ao_flash_block_size = 1 << ao_flash_block_shift;
+	ao_flash_block_mask = ao_flash_block_size - 1;
 
 	ao_storage_block = ao_flash_block_size;
 	ao_storage_config = ao_storage_total - ao_storage_block;
+	ao_storage_unit = ao_flash_block_size;
 
 	ao_flash_setup_done = 1;
 	ao_mutex_put(&ao_flash_mutex);
@@ -238,79 +243,38 @@ ao_flash_fill(uint16_t block)
 }
 
 uint8_t
-ao_storage_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+ao_storage_device_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
 {
-	uint16_t block;
-	uint16_t this_len;
-	uint16_t	this_off;
+	uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
 
-	ao_storage_setup();
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
-		return 0;
-	while (len) {
-
-		/* Compute portion of transfer within
-		 * a single block
-		 */
-		this_off = (uint16_t) pos & ao_flash_block_mask;
-		this_len = ao_flash_block_size - this_off;
-		block = (uint16_t) (pos >> ao_flash_block_shift);
-		if (this_len > len)
-			this_len = len;
-
-		/* Transfer the data */
-		ao_mutex_get(&ao_flash_mutex); {
-			if (this_len != ao_flash_block_size)
-				ao_flash_fill(block);
-			else {
-				ao_flash_flush_internal();
-				ao_flash_block = block;
-			}
-			memcpy(ao_flash_data + this_off, buf, this_len);
-			ao_flash_block_dirty = 1;
-		} ao_mutex_put(&ao_flash_mutex);
-
-		/* See how much is left */
-		buf += this_len;
-		len -= this_len;
-		pos += this_len;
-	}
+	/* Transfer the data */
+	ao_mutex_get(&ao_flash_mutex); {
+		if (len != ao_flash_block_size)
+			ao_flash_fill(block);
+		else {
+			ao_flash_flush_internal();
+			ao_flash_block = block;
+		}
+		memcpy(ao_flash_data + (uint16_t) (pos & ao_flash_block_mask),
+		       buf,
+		       len);
+		ao_flash_block_dirty = 1;
+	} ao_mutex_put(&ao_flash_mutex);
 	return 1;
 }
 
 uint8_t
-ao_storage_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+ao_storage_device_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
 {
-	uint16_t block;
-	uint16_t this_len;
-	uint16_t this_off;
+	uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
 
-	ao_storage_setup();
-	if (pos >= ao_storage_total || pos + len > ao_storage_total)
-		return 0;
-	while (len) {
-
-
-		/* Compute portion of transfer within
-		 * a single block
-		 */
-		this_off = (uint16_t) pos & ao_flash_block_mask;
-		this_len = ao_flash_block_size - this_off;
-		block = (uint16_t) (pos >> ao_flash_block_shift);
-		if (this_len > len)
-			this_len = len;
-
-		/* Transfer the data */
-		ao_mutex_get(&ao_flash_mutex); {
-			ao_flash_fill(block);
-			memcpy(buf, ao_flash_data + this_off, this_len);
-		} ao_mutex_put(&ao_flash_mutex);
-
-		/* See how much is left */
-		buf += this_len;
-		len -= this_len;
-		pos += this_len;
-	}
+	/* Transfer the data */
+	ao_mutex_get(&ao_flash_mutex); {
+		ao_flash_fill(block);
+		memcpy(buf,
+		       ao_flash_data + (uint16_t) (pos & ao_flash_block_mask),
+		       len);
+	} ao_mutex_put(&ao_flash_mutex);
 	return 1;
 }
 
@@ -325,8 +289,9 @@ ao_storage_flush(void) __reentrant
 uint8_t
 ao_storage_erase(uint32_t pos) __reentrant
 {
+	uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
+
 	ao_mutex_get(&ao_flash_mutex); {
-		uint16_t block = (uint16_t) (pos >> ao_flash_block_shift);
 		ao_flash_fill(block);
 		memset(ao_flash_data, 0xff, ao_flash_block_size);
 		ao_flash_block_dirty = 1;
@@ -334,63 +299,8 @@ ao_storage_erase(uint32_t pos) __reentrant
 	return 1;
 }
 
-static void
-flash_dump(void) __reentrant
-{
-	static __xdata uint8_t	b;
-	uint16_t block;
-	uint8_t i;
-
-	ao_cmd_hex();
-	block = ao_cmd_lex_i;
-	if (ao_cmd_status != ao_cmd_success)
-		return;
-	i = 0;
-	do {
-		if ((i & 7) == 0) {
-			if (i)
-				putchar('\n');
-			ao_cmd_put16((uint16_t) i);
-		}
-		putchar(' ');
-		ao_storage_read(((uint32_t) block << 8) | i, &b, 1);
-		ao_cmd_put8(b);
-		++i;
-	} while (i != 0);
-	putchar('\n');
-}
-
-static void
-flash_store(void) __reentrant
-{
-	uint16_t block;
-	uint8_t i;
-	uint16_t len;
-	static __xdata uint8_t b;
-	uint32_t addr;
-
-	ao_cmd_hex();
-	block = ao_cmd_lex_i;
-	ao_cmd_hex();
-	i = ao_cmd_lex_i;
-	addr = ((uint32_t) block << 8) | i;
-	ao_cmd_hex();
-	len = ao_cmd_lex_i;
-	if (ao_cmd_status != ao_cmd_success)
-		return;
-	while (len--) {
-		ao_cmd_hex();
-		if (ao_cmd_status != ao_cmd_success)
-			return;
-		b = ao_cmd_lex_i;
-		ao_storage_write(addr, &b, 1);
-		addr++;
-	}
-	ao_storage_flush();
-}
-
-static void
-flash_status(void) __reentrant
+void
+ao_storage_device_info(void) __reentrant
 {
 	uint8_t	status;
 
@@ -405,23 +315,15 @@ flash_status(void) __reentrant
 	} ao_mutex_put(&ao_flash_mutex);
 }
 
-__code struct ao_cmds ao_flash_cmds[] = {
-	{ 'e', flash_dump, 	"e <block>                          Dump a block of flash data" },
-	{ 'w', flash_store,	"w <block> <start> <len> <data> ... Write data to flash" },
-	{ 'f', flash_status,	"f                                  Show flash status register" },
-	{ 0,   flash_store, NULL },
-};
-
 /*
  * To initialize the chip, set up the CS line and
  * the SPI interface
  */
 void
-ao_storage_init(void)
+ao_storage_device_init(void)
 {
 	/* set up CS */
 	FLASH_CS = 1;
 	P1DIR |= (1 << FLASH_CS_INDEX);
 	P1SEL &= ~(1 << FLASH_CS_INDEX);
-	ao_cmd_register(&ao_flash_cmds[0]);
 }
diff --git a/src/ao_log.c b/src/ao_log.c
index 132512e6..9ca033e1 100644
--- a/src/ao_log.c
+++ b/src/ao_log.c
@@ -168,7 +168,7 @@ ao_log_erase_pos(uint8_t i)
 	return i * sizeof (struct ao_log_erase) + AO_STORAGE_ERASE_LOG;
 }
 
-static void
+void
 ao_log_write_erase(uint8_t pos)
 {
 	erase.unused = 0x00;
@@ -364,13 +364,16 @@ ao_log_delete(void) __reentrant
 				ao_storage_erase(ao_log_current_pos);
 				ao_log_current_pos += ao_storage_block;
 			}
-			puts("Erased\n");
+			puts("Erased");
 			return;
 		}
 	}
+	ao_log_erase_mark();
 	printf("No such flight: %d\n", ao_cmd_lex_i);
 }
 
+
+
 __code struct ao_cmds ao_log_cmds[] = {
 	{ 'l',	ao_log_list,	"l                                  List stored flight logs" },
 	{ 'd',	ao_log_delete,	"d <flight-number>                  Delete stored flight" },
diff --git a/src/ao_m25.c b/src/ao_m25.c
new file mode 100644
index 00000000..afd5df76
--- /dev/null
+++ b/src/ao_m25.c
@@ -0,0 +1,380 @@
+/*
+ * Copyright © 2010 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+/* Total bytes of available storage */
+__xdata uint32_t	ao_storage_total;
+
+/* Block size - device is erased in these units. At least 256 bytes */
+__xdata uint32_t	ao_storage_block;
+
+/* Byte offset of config block. Will be ao_storage_block bytes long */
+__xdata uint32_t	ao_storage_config;
+
+/* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */
+__xdata uint16_t	ao_storage_unit;
+
+/*
+ * Each flash chip is arranged in 64kB sectors; the
+ * chip cannot erase in units smaller than that.
+ *
+ * Writing happens in units of 256 byte pages and
+ * can only change bits from 1 to 0. So, you can rewrite
+ * the same contents, or append to an existing page easily enough
+ */
+
+#define M25_WREN	0x06	/* Write Enable */
+#define M25_WRDI	0x04	/* Write Disable */
+#define M25_RDID	0x9f	/* Read Identification */
+#define M25_RDSR	0x05	/* Read Status Register */
+#define M25_WRSR	0x01	/* Write Status Register */
+#define M25_READ	0x03	/* Read Data Bytes */
+#define M25_FAST_READ	0x0b	/* Read Data Bytes at Higher Speed */
+#define M25_PP		0x02	/* Page Program */
+#define M25_SE		0xd8	/* Sector Erase */
+#define M25_BE		0xc7	/* Bulk Erase */
+#define M25_DP		0xb9	/* Deep Power-down */
+
+/* RDID response */
+#define M25_MANUF_OFFSET	0
+#define M25_MEMORY_TYPE_OFFSET	1
+#define M25_CAPACITY_OFFSET	2
+#define M25_UID_OFFSET		3
+#define M25_CFI_OFFSET		4
+#define M25_RDID_LEN		4	/* that's all we need */
+
+#define M25_CAPACITY_128KB	0x11
+#define M25_CAPACITY_256KB	0x12
+#define M25_CAPACITY_512KB	0x13
+#define M25_CAPACITY_1MB	0x14
+#define M25_CAPACITY_2MB	0x15
+
+/*
+ * Status register bits
+ */
+
+#define M25_STATUS_SRWD		(1 << 7)	/* Status register write disable */
+#define M25_STATUS_BP_MASK	(7 << 2)	/* Block protect bits */
+#define M25_STATUS_BP_SHIFT	(2)
+#define M25_STATUS_WEL		(1 << 1)	/* Write enable latch */
+#define M25_STATUS_WIP		(1 << 0)	/* Write in progress */
+
+/*
+ * On teleterra, the m25 chip select pins are
+ * wired on P0_0 through P0_3.
+ */
+
+#if M25_MAX_CHIPS > 1
+static uint8_t ao_m25_size[M25_MAX_CHIPS];	/* number of sectors in each chip */
+static uint8_t ao_m25_pin[M25_MAX_CHIPS];	/* chip select pin for each chip */
+static uint8_t ao_m25_numchips;			/* number of chips detected */
+#endif
+static uint8_t ao_m25_total;			/* total sectors available */
+static uint8_t ao_m25_wip;			/* write in progress */
+
+static __xdata uint8_t ao_m25_mutex;
+
+/*
+ * This little array is abused to send and receive data. A particular
+ * caution -- the read and write addresses are written into the last
+ * three bytes of the array by ao_m25_set_page_address and then the
+ * first byte is used by ao_m25_wait_wip and ao_m25_write_enable, neither
+ * of which touch those last three bytes.
+ */
+
+static __xdata uint8_t	ao_m25_instruction[4];
+
+#define M25_SELECT(cs)			(SPI_CS_PORT &= ~(cs))
+#define M25_DESELECT(cs)		(SPI_CS_PORT |= (cs))
+
+#define M25_BLOCK_SHIFT			16
+#define M25_BLOCK			65536L
+#define M25_POS_TO_SECTOR(pos)		((uint8_t) ((pos) >> M25_BLOCK_SHIFT))
+#define M25_SECTOR_TO_POS(sector)	(((uint32_t) (sector)) << M25_BLOCK_SHIFT)
+
+/*
+ * Block until the specified chip is done writing
+ */
+static void
+ao_m25_wait_wip(uint8_t cs)
+{
+	if (ao_m25_wip & cs) {
+		M25_SELECT(cs);
+		ao_m25_instruction[0] = M25_RDSR;
+		ao_spi_send(ao_m25_instruction, 1);
+		do {
+			ao_spi_recv(ao_m25_instruction, 1);
+		} while (ao_m25_instruction[0] & M25_STATUS_WIP);
+		M25_DESELECT(cs);
+		ao_m25_wip &= ~cs;
+	}
+}
+
+/*
+ * Set the write enable latch so that page program and sector
+ * erase commands will work. Also mark the chip as busy writing
+ * so that future operations will block until the WIP bit goes off
+ */
+static void
+ao_m25_write_enable(uint8_t cs)
+{
+	M25_SELECT(cs);
+	ao_m25_instruction[0] = M25_WREN;
+	ao_spi_send(&ao_m25_instruction, 1);
+	M25_DESELECT(cs);
+	ao_m25_wip |= cs;
+}
+
+
+/*
+ * Returns the number of 64kB sectors
+ */
+static uint8_t
+ao_m25_read_capacity(uint8_t cs)
+{
+	uint8_t	capacity;
+	M25_SELECT(cs);
+	ao_m25_instruction[0] = M25_RDID;
+	ao_spi_send(ao_m25_instruction, 1);
+	ao_spi_recv(ao_m25_instruction, M25_RDID_LEN);
+	M25_DESELECT(cs);
+
+	/* Check to see if the chip is present */
+	if (ao_m25_instruction[0] == 0xff)
+		return 0;
+	capacity = ao_m25_instruction[M25_CAPACITY_OFFSET];
+
+	/* Sanity check capacity number */
+	if (capacity < 0x11 || 0x1f < capacity)
+		return 0;
+	return 1 << (capacity - 0x10);
+}
+
+static uint8_t
+ao_m25_set_address(uint32_t pos)
+{
+	uint8_t	chip;
+#if M25_MAX_CHIPS > 1
+	uint8_t	size;
+
+	for (chip = 0; chip < ao_m25_numchips; chip++) {
+		size = ao_m25_size[chip];
+		if (M25_POS_TO_SECTOR(pos) < size)
+			break;
+		pos -= M25_SECTOR_TO_POS(size);
+	}
+	if (chip == ao_m25_numchips)
+		return 0xff;
+
+	chip = ao_m25_pin[chip];
+#else
+	chip = M25_CS_MASK;
+#endif
+	ao_m25_wait_wip(chip);
+
+	ao_m25_instruction[1] = pos >> 16;
+	ao_m25_instruction[2] = pos >> 8;
+	ao_m25_instruction[3] = pos;
+	return chip;
+}
+
+/*
+ * Scan the possible chip select lines
+ * to see which flash chips are connected
+ */
+static uint8_t
+ao_m25_scan(void)
+{
+#if M25_MAX_CHIPS > 1
+	uint8_t	pin, size;
+#endif
+
+	if (ao_m25_total)
+		return 1;
+
+#if M25_MAX_CHIPS > 1
+	ao_m25_numchips = 0;
+	for (pin = 1; pin != 0; pin <<= 1) {
+		if (M25_CS_MASK & pin) {
+			size = ao_m25_read_capacity(pin);
+			if (size != 0) {
+				ao_m25_size[ao_m25_numchips] = size;
+				ao_m25_pin[ao_m25_numchips] = pin;
+				ao_m25_total += size;
+				ao_m25_numchips++;
+			}
+		}
+	}
+#else
+	ao_m25_total = ao_m25_read_capacity(M25_CS_MASK);
+#endif
+	if (!ao_m25_total)
+		return 0;
+	ao_storage_total = M25_SECTOR_TO_POS(ao_m25_total);
+	ao_storage_block = M25_BLOCK;
+	ao_storage_config = ao_storage_total - M25_BLOCK;
+	ao_storage_unit = 256;
+	return 1;
+}
+
+/*
+ * Erase the specified sector
+ */
+uint8_t
+ao_storage_erase(uint32_t pos) __reentrant
+{
+	uint8_t	cs;
+
+	if (pos >= ao_storage_total || pos + ao_storage_block > ao_storage_total)
+		return 0;
+
+	ao_mutex_get(&ao_m25_mutex);
+	ao_m25_scan();
+
+	cs = ao_m25_set_address(pos);
+
+	ao_m25_wait_wip(cs);
+	ao_m25_write_enable(cs);
+
+	ao_m25_instruction[0] = M25_SE;
+	M25_SELECT(cs);
+	ao_spi_send(ao_m25_instruction, 4);
+	M25_DESELECT(cs);
+	ao_m25_wip |= cs;
+
+	ao_mutex_put(&ao_m25_mutex);
+	return 1;
+}
+
+/*
+ * Write to flash
+ */
+uint8_t
+ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
+{
+	uint8_t	cs;
+
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+
+	ao_mutex_get(&ao_m25_mutex);
+	ao_m25_scan();
+
+	cs = ao_m25_set_address(pos);
+	ao_m25_write_enable(cs);
+
+	ao_m25_instruction[0] = M25_PP;
+	M25_SELECT(cs);
+	ao_spi_send(ao_m25_instruction, 4);
+	ao_spi_send(d, len);
+	M25_DESELECT(cs);
+
+	ao_mutex_put(&ao_m25_mutex);
+	return 1;
+}
+
+/*
+ * Read from flash
+ */
+uint8_t
+ao_storage_device_read(uint32_t pos, __xdata void *d, uint16_t len) __reentrant
+{
+	uint8_t	cs;
+
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	ao_mutex_get(&ao_m25_mutex);
+	ao_m25_scan();
+
+	cs = ao_m25_set_address(pos);
+
+	/* No need to use the FAST_READ as we're running at only 8MHz */
+	ao_m25_instruction[0] = M25_READ;
+	M25_SELECT(cs);
+	ao_spi_send(ao_m25_instruction, 4);
+	ao_spi_recv(d, len);
+	M25_DESELECT(cs);
+
+	ao_mutex_put(&ao_m25_mutex);
+	return 1;
+}
+
+void
+ao_storage_flush(void) __reentrant
+{
+}
+
+void
+ao_storage_setup(void)
+{
+	ao_mutex_get(&ao_m25_mutex);
+	ao_m25_scan();
+	ao_mutex_put(&ao_m25_mutex);
+}
+
+void
+ao_storage_device_info(void) __reentrant
+{
+	uint8_t	cs;
+#if M25_MAX_CHIPS > 1
+	uint8_t chip;
+#endif
+
+	ao_mutex_get(&ao_m25_mutex);
+	ao_m25_scan();
+	ao_mutex_put(&ao_m25_mutex);
+
+#if M25_MAX_CHIPS > 1
+	printf ("Detected chips %d size %d\n", ao_m25_numchips, ao_m25_total);
+	for (chip = 0; chip < ao_m25_numchips; chip++)
+		printf ("Flash chip %d select %02x size %d\n",
+			chip, ao_m25_pin[chip], ao_m25_size[chip]);
+#else
+	printf ("Detected chips 1 size %d\n", ao_m25_total);
+#endif
+
+	printf ("Available chips:\n");
+	for (cs = 1; cs != 0; cs <<= 1) {
+		if ((M25_CS_MASK & cs) == 0)
+			continue;
+
+		ao_mutex_get(&ao_m25_mutex);
+		M25_SELECT(cs);
+		ao_m25_instruction[0] = M25_RDID;
+		ao_spi_send(ao_m25_instruction, 1);
+		ao_spi_recv(ao_m25_instruction, M25_RDID_LEN);
+		M25_DESELECT(cs);
+
+		printf ("Select %02x manf %02x type %02x cap %02x uid %02x\n",
+			cs,
+			ao_m25_instruction[M25_MANUF_OFFSET],
+			ao_m25_instruction[M25_MEMORY_TYPE_OFFSET],
+			ao_m25_instruction[M25_CAPACITY_OFFSET],
+			ao_m25_instruction[M25_UID_OFFSET]);
+		ao_mutex_put(&ao_m25_mutex);
+	}
+}
+
+void
+ao_storage_device_init(void)
+{
+	/* Set up chip select wires */
+	SPI_CS_PORT |= M25_CS_MASK;	/* raise all CS pins */
+	SPI_CS_DIR |= M25_CS_MASK;	/* set CS pins as outputs */
+	SPI_CS_SEL &= ~M25_CS_MASK;	/* set CS pins as GPIO */
+}
diff --git a/src/ao_pins.h b/src/ao_pins.h
index b5089b85..9446964e 100644
--- a/src/ao_pins.h
+++ b/src/ao_pins.h
@@ -48,6 +48,8 @@
 	#define HAS_EXTERNAL_TEMP	0
 	#define SPI_CS_ON_P1		1
 	#define SPI_CS_ON_P0		0
+	#define M25_CS_MASK		0x02	/* CS0 is P1_1 */
+	#define M25_MAX_CHIPS		1
 #endif
 
 #if defined(TELEDONGLE_V_0_2)
diff --git a/src/ao_storage.c b/src/ao_storage.c
new file mode 100644
index 00000000..25a7c8b9
--- /dev/null
+++ b/src/ao_storage.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao.h"
+
+uint8_t
+ao_storage_read(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+	uint16_t this_len;
+	uint16_t this_off;
+
+	ao_storage_setup();
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	while (len) {
+
+		/* Compute portion of transfer within
+		 * a single block
+		 */
+		this_off = (uint16_t) pos & (ao_storage_unit - 1);
+		this_len = ao_storage_unit - this_off;
+		if (this_len > len)
+			this_len = len;
+
+		if (!ao_storage_device_read(pos, buf, this_len))
+			return 0;
+
+		/* See how much is left */
+		buf += this_len;
+		len -= this_len;
+		pos += this_len;
+	}
+	return 1;
+}
+
+uint8_t
+ao_storage_write(uint32_t pos, __xdata void *buf, uint16_t len) __reentrant
+{
+	uint16_t this_len;
+	uint16_t this_off;
+
+	ao_storage_setup();
+	if (pos >= ao_storage_total || pos + len > ao_storage_total)
+		return 0;
+	while (len) {
+
+		/* Compute portion of transfer within
+		 * a single block
+		 */
+		this_off = (uint16_t) pos & (ao_storage_unit - 1);
+		this_len = ao_storage_unit - this_off;
+		if (this_len > len)
+			this_len = len;
+
+		if (!ao_storage_device_write(pos, buf, this_len))
+			return 0;
+
+		/* See how much is left */
+		buf += this_len;
+		len -= this_len;
+		pos += this_len;
+	}
+	return 1;
+}
+
+static __xdata uint8_t storage_data[8];
+
+static void
+ao_storage_dump(void) __reentrant
+{
+	uint8_t i, j;
+
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	for (i = 0; ; i += 8) {
+		if (ao_storage_read(((uint32_t) (ao_cmd_lex_i) << 8) + i,
+				  storage_data,
+				  8)) {
+			ao_cmd_put16((uint16_t) i);
+			for (j = 0; j < 7; j++) {
+				putchar(' ');
+				ao_cmd_put8(storage_data[j]);
+			}
+			putchar ('\n');
+		}
+		if (i == 248)
+			break;
+	}
+}
+
+static void
+ao_storage_store(void) __reentrant
+{
+	uint16_t block;
+	uint8_t i;
+	uint16_t len;
+	static __xdata uint8_t b;
+	uint32_t addr;
+
+	ao_cmd_hex();
+	block = ao_cmd_lex_i;
+	ao_cmd_hex();
+	i = ao_cmd_lex_i;
+	addr = ((uint32_t) block << 8) | i;
+	ao_cmd_hex();
+	len = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	while (len--) {
+		ao_cmd_hex();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		b = ao_cmd_lex_i;
+		ao_storage_write(addr, &b, 1);
+		addr++;
+	}
+}
+
+void
+ao_storage_zap(void) __reentrant
+{
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	ao_storage_erase((uint32_t) ao_cmd_lex_i << 8);
+}
+
+void
+ao_storage_info(void) __reentrant
+{
+	printf("Storage size: %ld\n", ao_storage_total);
+	printf("Storage erase unit: %ld\n", ao_storage_block);
+	ao_storage_device_info();
+}
+
+__code struct ao_cmds ao_storage_cmds[] = {
+	{ 'f', ao_storage_info,	"f                                  Show storage info" },
+	{ 'e', ao_storage_dump, "e <block>                          Dump a block of flash data" },
+	{ 'w', ao_storage_store,"w <block> <start> <len> <data> ... Write data to flash" },
+	{ 'z', ao_storage_zap,	"z <block>                          Erase flash containing <block>" },
+	{ 0,   ao_storage_zap, NULL },
+};
+
+void
+ao_storage_init(void)
+{
+	ao_storage_device_init();
+	ao_cmd_register(&ao_storage_cmds[0]);
+}
diff --git a/src/at45db161d.h b/src/at45db161d.h
index 96042459..9ee6f1b6 100644
--- a/src/at45db161d.h
+++ b/src/at45db161d.h
@@ -29,6 +29,7 @@
 
 #define FLASH_READ		0x03
 #define FLASH_WRITE		0x82
+#define FLASH_PAGE_ERASE	0x81
 #define FLASH_READ_STATUS	0xd7
 #define FLASH_SET_CONFIG	0x3d
 
diff --git a/src/telemetrum-v1.1/Makefile.defs b/src/telemetrum-v1.1/Makefile.defs
index a230203a..f38226c6 100644
--- a/src/telemetrum-v1.1/Makefile.defs
+++ b/src/telemetrum-v1.1/Makefile.defs
@@ -3,7 +3,7 @@ PROG = telemetrum-v1.1-$(VERSION).ihx
 SRC = \
 	$(TM_BASE_SRC) \
 	$(SPI_DRIVER_SRC) \
-	$(FLASH_DRIVER_SRC) \
+	$(M25_DRIVER_SRC) \
 	$(SKY_DRIVER_SRC) \
 	$(DBG_SRC)