--- a/spiflash.c	Mon Mar 02 14:31:16 2015 +1030
+++ b/spiflash.c	Mon Mar 02 14:32:08 2015 +1030
@@ -27,108 +27,20 @@
 
 static int writestate = RW_IDLE;
 static int readstate = RW_IDLE;
-#if 0
-void
-spiflashcmd(int argc, char **argv) {
-    uint8_t status, tmp, len;
-    uint32_t addr;
-    
-    if (argc == 0) {
-	fputs("No command specified\n", stdout);
-	return;
-    }
-    
-    if (!strcmp(argv[0], "str")) {
-	status = spiflashreadstatus();
-	fputs("Status = ", stdout);
-	for (unsigned int i = 0; i < sizeof(flstattbl) / sizeof(flstattbl[0]); i++)
-	    if (status & 1 << i) {
-		fputs(flstattbl[i], stdout);
-		fputs(" ", stdout);
-	    }
-	printf("(0x%02x)\n", status);
-    } else if (!strcmp(argv[0], "stw")) {
-	if (argc != 2) {
-	    fputs("Incorrect number of arguments\n", stdout);
-	    return;
-	}
-	tmp = atoi(argv[1]);
-	spiflashwritestatus(tmp);
-	status = spiflashreadstatus();
-	printf("Wrote 0x%02x to status, now 0x%02x\n", tmp, status);
-    } else if (!strcmp(argv[0], "er")) {
-	if (argc != 2) {
-	    fputs("Incorrect number of arguments\n", stdout);
-	    return;
-	}
-	addr = atoi(argv[1]);
-	spiflash4kerase(addr);
-	printf("Erased 0x%x\n", (unsigned int)addr);
-    } else if (!strcmp(argv[0], "rd")) {
-	if (argc < 2) {
-	    fputs("Incorrect number of arguments\n", stdout);
-	    return;
-	}
-	
-	addr = atoi(argv[1]);
 
-	if (argc > 2)
-	    len = atoi(argv[2]);
-	else
-	    len = 16;
-
-	spiflashstartread(addr);
-	
-	for (uint32_t i = 0; i < len; i++)
-	    printf("Read 0x%02x from 0x%06x\n", spiflashreadbyte(), (unsigned int)(addr + i));
-	spiflashstopread();
-	
-	fputs("\n", stdout);
-    } else if (!strcmp(argv[0], "wr")) {
-	if (argc < 2) {
-	    fputs("Incorrect number of arguments\n", stdout);
-	    return;
-	}
-
-	addr = atoi(argv[1]);
-
-	if (argc > 2)
-	    len = atoi(argv[2]);
-	else
-	    len = 16;
-
-	for (int i = 0; i < 16; i += 2) {
-	    uint16_t data;
-	    data = ((i + 1) << 8) | i;
-	    printf("Writing 0x%04x to 0x%06x\n", data, (unsigned int)(addr + i));
-
-	    if (i == 0)
-		spiflashstartwrite(addr, data);
-	    else
-		spiflashwriteword(data);
-	}
-	spiflashstopwrite();
-    } else if (!strcmp(argv[0], "id")) {
-	printf("Flash ID = 0x%04hx (expect 0xbf41)\n", spiflashreadid());
-    } else {
-	fputs("Unknown sub command\n", stdout);
-	return;
-    }
-}
-#endif
 void
 spiflash4kerase(uint32_t addr) {
-    spiflashenablewrite();			/* Enable writing */
-    
+    spiflashenablewrite();		/* Enable writing */
+
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_4KERASE);		/* Send command */
     SPI_WriteByte(addr >> 16);	 	/* Send address */
     SPI_WriteByte(addr >> 8);
     SPI_WriteByte(addr);
-  
+
     FL_DESELECT();
-    
+
     //fputs("4k erase ", stdout);
     spiflashwait();
 }
@@ -136,7 +48,7 @@
 void
 spiflashwait(void) {
     uint8_t	cnt;
-    
+
     /* Wait for not BUSY */
     for (cnt = 0; (spiflashreadstatus() & FL_BUSY) != 0; cnt++)
 	;
@@ -147,7 +59,7 @@
 uint16_t
 spiflashreadid(void) {
     uint8_t	fac, dev;
-    
+
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_RDID);		/* Send command */
@@ -156,7 +68,7 @@
     SPI_WriteByte(0x00);
     fac = SPI_WriteByte(0x00);		/* Read ID */
     dev = SPI_WriteByte(0x00);
-  
+
     FL_DESELECT();			/* De-select device */
 
     return fac << 8 | dev;
@@ -167,20 +79,20 @@
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_WREN);		/* Send command */
-  
+
     FL_DESELECT();			/* De-select device */
 }
 
 uint8_t
 spiflashreadstatus(void) {
     uint8_t	status;
-    
+
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_RDSR);		/* Send command */
     SPI_WriteByte(0x00);		/* Send dummy byte for address cycle */
     status = SPI_WriteByte(0x00);	/* Read status */
-  
+
     FL_DESELECT();			/* De-select device */
 
     return status;
@@ -195,7 +107,7 @@
     FL_DESELECT();
 
     /* Actually write status */
-    FL_SELECT();			/* Re-select device for new command */			
+    FL_SELECT();			/* Re-select device for new command */
     SPI_WriteByte(FL_WRSR);		/* Send command */
     SPI_WriteByte(status);		/* Send data byte */
     FL_DESELECT();			/* De-select device */
@@ -204,7 +116,7 @@
 uint8_t
 spiflashread(uint32_t addr) {
     uint8_t	data;
-    
+
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_READ);		/* Send command */
@@ -212,7 +124,7 @@
     SPI_WriteByte(addr >> 8);
     SPI_WriteByte(addr);
     data = SPI_WriteByte(0x00);		/* Read data */
-  
+
     FL_DESELECT();			/* De-select device */
 
     return data;
@@ -230,7 +142,7 @@
     SPI_WriteByte(addr >> 8);
     SPI_WriteByte(addr);
     SPI_WriteByte(data);		/* Write data */
-  
+
     FL_DESELECT();			/* De-select device */
 
 }
@@ -243,7 +155,7 @@
 void
 spiflashstartread(uint32_t addr) {
     assert(readstate == RW_IDLE);
-    
+
     FL_SELECT();			/* Select device */
 
     SPI_WriteByte(FL_READ);		/* Send command */
@@ -269,7 +181,7 @@
     readstate = RW_IDLE;
 }
 
-/* 
+/*
  * Auto increment writing looks like so
  *
  * Enable writing  				CS, WREN, nCS
@@ -284,8 +196,8 @@
 void
 spiflashstartwrite(uint32_t addr, uint16_t data) {
     assert(writestate == RW_IDLE);
-    
-    spiflashenablewrite();			/* Enable writes */
+
+    spiflashenablewrite();		/* Enable writes */
 
     FL_SELECT();			/* Select device */
 
@@ -305,7 +217,7 @@
 void
 spiflashwriteword(uint16_t data) {
     assert(writestate == RW_RUNNING);
-    
+
     //fputs("write word ", stdout);
     spiflashwait();			/* Wait until not busy */
 
@@ -339,17 +251,22 @@
 
 int
 spiflashreadblock(uint32_t addr, uint32_t len, void *_data) {
-    uint8_t	*data = _data;
-    uint32_t	flashcrc, ramcrc;
+    uint32_t	*data = _data;
+    uint32_t	flashcrc, ramcrc, tmp;
 
     /* Must be a multiple of 4 due to CRC check */
     assert(len % 4 == 0);
 
     spiflashstartread(addr);
-   CRC_ResetDR();
-    for (int i = len; i > 0; i--) {
-	*data = spiflashreadbyte();
-	CRC_CalcCRC(*data);
+    CRC_ResetDR();
+    for (int i = len; i > 0; i -= 4) {
+	tmp = spiflashreadbyte() |
+	  spiflashreadbyte() << 8 |
+	  spiflashreadbyte() << 16 |
+	  spiflashreadbyte() << 24;
+
+	*data = tmp;
+	CRC_CalcCRC(tmp);
 	data++;
     }
 
@@ -361,8 +278,8 @@
     spiflashstopread();
 
     ramcrc = CRC_GetCRC();
-    
-    /* printf("RAM CRC 0x%08x Flash CRC 0x%08x\n", (uint)ramcrc, (uint)flashcrc); */
+
+    //printf("RAM CRC 0x%08x Flash CRC 0x%08x\n", (uint)ramcrc, (uint)flashcrc);
     if (ramcrc == flashcrc)
 	return 1;
     else
@@ -371,14 +288,20 @@
 
 uint32_t
 spiflashcrcblock(uint32_t addr, uint32_t len) {
+    uint32_t tmp;
+
     assert(len % 4 == 0);
 
-   CRC_ResetDR();
+    CRC_ResetDR();
 
     spiflashstartread(addr);
-    for (int i = len; i > 0; i--)
-	CRC_CalcCRC(spiflashreadbyte());
-
+    for (; len > 0; len -= 4) {
+	tmp = spiflashreadbyte() |
+	  spiflashreadbyte() << 8 |
+	  spiflashreadbyte() << 16 |
+	  spiflashreadbyte() << 24;
+	CRC_CalcCRC(tmp);
+    }
     spiflashstopread();
 
     return CRC_GetCRC();
@@ -387,54 +310,60 @@
 int
 spiflashwriteblock(uint32_t addr, uint32_t len, void *_data) {
     uint16_t	*data = _data;
-    uint32_t	crc, vcrc;
-    
+    uint32_t	crc, vcrc, tmp;
+
     //printf("Writing %u bytes to 0x%06x\n", (uint)len, (uint)addr);
-    
+
     /* Ensure data is
      * - 16 bit aligned
      * - a multiple of 32 bits in length (for CRCs, the flash only need 16 bits)
-     * - not longer than a sector 
+     * - not longer than a sector
      */
     assert(addr % 2 == 0);
     assert(len % 4 == 0);
     assert(len <= 4096);
-    
+
     /* Disable write protect */
     spiflashwritestatus(0x00);
 
     /* Erase sector */
     spiflash4kerase(addr);
- 
-   CRC_ResetDR();
 
     /* Write data */
-   for (uint i = 0; i < len / 2; i++) {
+    CRC_ResetDR();
+    for (uint i = 0; i < len / 2; i++) {
+	//printf("0x%04x: %04x\n", i, *data);
 	if (i == 0)
 	    spiflashstartwrite(addr, *data);
 	else
 	    spiflashwriteword(*data);
-	CRC_CalcCRC(*data);
+
+	if (i % 2 == 0)
+	    tmp = *data;
+	else {
+	    tmp |= *data << 16;
+	    CRC_CalcCRC(tmp);
+	}
 	data++;
     }
 
-   /* Calculate CRC */
-   crc = CRC_GetCRC();
-   
-   //printf("CRC is 0x%08x\n", (uint)crc);
-   
-   /* Write CRC */
-   spiflashwriteword(crc);
-   spiflashwriteword(crc >> 16);
-   
-   spiflashstopwrite();
+    /* Calculate CRC */
+    crc = CRC_GetCRC();
+    //printf("CRC is 0x%08x\n", (uint)crc);
+
+    /* Write CRC */
+    spiflashwriteword(crc);
+    spiflashwriteword(crc >> 16);
 
-   /* Read back and check CRC */
-   vcrc = spiflashcrcblock(addr, len);
-   if (vcrc != crc)
-       return 1;
-   else
-       return 0;
+    spiflashstopwrite();
+
+    /* Read back and check CRC */
+    vcrc = spiflashcrcblock(addr, len);
+    //printf("CRC read back as 0x%08x\n", (uint)vcrc);
+    if (vcrc != crc)
+	return 1;
+    else
+	return 0;
 }
 
 void