1 files changed, 61 insertions, 3 deletions

diff --git a/sst_fwhub.c b/sst_fwhub.c
index ab541e41..0bf2d4d0 100644
--- a/sst_fwhub.c
+++ b/sst_fwhub.c
@@ -35,23 +35,75 @@ void print_sst_fwhub_status(uint8_t status)
 	printf("%s", status & 0x2 ? "WP|TBL#|WP#,ABORT:" : "UNLOCK:");
 }
 
+int check_sst_fwhub_block_lock(struct flashchip *flash, int offset)
+{
+	volatile uint8_t *registers = flash->virtual_registers;
+	uint8_t blockstatus;
+
+	blockstatus = chip_readb(registers + offset + 2);
+	printf_debug("Lock status for 0x%06x (size 0x%06x) is %02x, ",
+		     offset, flash->page_size, blockstatus);
+	switch (blockstatus & 0x3) {
+	case 0x0:
+		printf_debug("full access\n");
+		break;
+	case 0x1:
+		printf_debug("write locked\n");
+		break;
+	case 0x2:
+		printf_debug("locked open\n");
+		break;
+	case 0x3:
+		printf_debug("write locked down\n");
+		break;
+	}
+	/* Return content of the write_locked bit */
+	return blockstatus & 0x1;
+}
+
+int clear_sst_fwhub_block_lock(struct flashchip *flash, int offset)
+{
+	volatile uint8_t *registers = flash->virtual_registers;
+	uint8_t blockstatus;
+
+	blockstatus = check_sst_fwhub_block_lock(flash, offset);
+
+	if (blockstatus) {
+		printf_debug("Trying to clear lock for 0x%06x... ", offset)
+		chip_writeb(0, registers + offset + 2);
+
+		blockstatus = check_sst_fwhub_block_lock(flash, offset);
+		printf_debug("%s\n", (blockstatus) ? "failed" : "OK");
+	}
+
+	return blockstatus;
+}
+
 /* probe_jedec works fine for probing */
 int probe_sst_fwhub(struct flashchip *flash)
 {
+	int i;
+
 	if (probe_jedec(flash) == 0)
 		return 0;
 
 	map_flash_registers(flash);
 
+	for (i = 0; i < flash->total_size * 1024; i += flash->page_size)
+		check_sst_fwhub_block_lock(flash, i);
+
 	return 1;
 }
 
 int erase_sst_fwhub_block(struct flashchip *flash, int offset)
 {
-	volatile uint8_t *wrprotect = flash->virtual_registers + offset + 2;
+	uint8_t blockstatus = clear_sst_fwhub_block_lock(flash, offset);
 
-	// clear write protect
-	chip_writeb(0, wrprotect);
+	if (blockstatus) {
+		printf("Block lock clearing failed, not erasing block "
+			"at 0x%06x\n", offset);
+		return 1;
+	}
 
 	erase_block_jedec(flash->virtual_memory, offset);
 	toggle_ready_jedec(flash->virtual_memory);
@@ -85,6 +137,7 @@ int write_sst_fwhub(struct flashchip *flash, uint8_t *buf)
 	int total_size = flash->total_size * 1024;
 	int page_size = flash->page_size;
 	volatile uint8_t *bios = flash->virtual_memory;
+	uint8_t blockstatus;
 
 	// FIXME: We want block wide erase instead of ironing the whole chip
 	if (erase_sst_fwhub(flash))
@@ -93,6 +146,11 @@ int write_sst_fwhub(struct flashchip *flash, uint8_t *buf)
 	printf("Programming page: ");
 	for (i = 0; i < total_size / page_size; i++) {
 		printf("%04d at address: 0x%08x", i, i * page_size);
+		blockstatus = clear_sst_fwhub_block_lock(flash, i * page_size);
+		if (blockstatus) {
+			printf(" is locked down permanently, aborting\n");
+			return 1;
+		}
 		write_sector_jedec(bios, buf + i * page_size,
 				   bios + i * page_size, page_size);
 		printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b");