[CFI driver] add missing flash protection for new driver

3 files changed, 105 insertions, 43 deletions

diff --git a/drivers/cfi_flash_amd.c b/drivers/cfi_flash_amd.c
index 6d7130f53f..05274c3b8a 100644
--- a/drivers/cfi_flash_amd.c
+++ b/drivers/cfi_flash_amd.c
@@ -145,5 +145,6 @@ struct cfi_cmd_set cfi_cmd_set_amd = {
 	.flash_is_busy = amd_flash_is_busy,
 	.flash_read_jedec_ids = amd_read_jedec_ids,
 	.flash_prepare_write = amd_flash_prepare_write,
+	.flash_status_check = flash_generic_status_check,
 };
 
diff --git a/drivers/cfi_flash_intel.c b/drivers/cfi_flash_intel.c
index 18b377cc53..a0a88851b7 100644
--- a/drivers/cfi_flash_intel.c
+++ b/drivers/cfi_flash_intel.c
@@ -132,11 +132,44 @@ static int intel_flash_write_cfibuffer (flash_info_t * info, ulong dest, const u
 }
 #endif /* CONFIG_CFI_BUFFER_WRITE */
 
+static int intel_flash_status_check (flash_info_t * info, flash_sect_t sector,
+				    uint64_t tout, char *prompt)
+{
+	int retcode;
+
+	retcode = flash_generic_status_check (info, sector, tout, prompt);
+
+	if ((retcode == ERR_OK)
+	    && !flash_isequal (info, sector, 0, FLASH_STATUS_DONE)) {
+		retcode = ERR_INVAL;
+		printf ("Flash %s error at address %lx\n", prompt,
+			info->start[sector]);
+		if (flash_isset (info, sector, 0, FLASH_STATUS_ECLBS | FLASH_STATUS_PSLBS)) {
+			puts ("Command Sequence Error.\n");
+		} else if (flash_isset (info, sector, 0, FLASH_STATUS_ECLBS)) {
+			puts ("Block Erase Error.\n");
+			retcode = ERR_NOT_ERASED;
+		} else if (flash_isset (info, sector, 0, FLASH_STATUS_PSLBS)) {
+			puts ("Locking Error\n");
+		}
+		if (flash_isset (info, sector, 0, FLASH_STATUS_DPS)) {
+			puts ("Block locked.\n");
+			retcode = ERR_PROTECTED;
+		}
+		if (flash_isset (info, sector, 0, FLASH_STATUS_VPENS))
+			puts ("Vpp Low Error.\n");
+	}
+	flash_write_cmd (info, sector, 0, info->cmd_reset);
+
+	return retcode;
+}
+
 struct cfi_cmd_set cfi_cmd_set_intel = {
 	.flash_write_cfibuffer = intel_flash_write_cfibuffer,
 	.flash_erase_one = intel_flash_erase_one,
 	.flash_is_busy = intel_flash_is_busy,
 	.flash_read_jedec_ids = intel_read_jedec_ids,
 	.flash_prepare_write = intel_flash_prepare_write,
+	.flash_status_check = intel_flash_status_check,
 };
 
diff --git a/drivers/cfi_flash_new.c b/drivers/cfi_flash_new.c
index cd951d7bf1..b4dd29100d 100644
--- a/drivers/cfi_flash_new.c
+++ b/drivers/cfi_flash_new.c
@@ -647,6 +647,60 @@ static int write_buff (flash_info_t * info, const uchar * src, ulong addr, ulong
 	return flash_write_cfiword (info, wp, cword);
 }
 
+static int flash_real_protect (flash_info_t * info, long sector, int prot)
+{
+	int retcode = 0;
+
+	flash_write_cmd (info, sector, 0, FLASH_CMD_CLEAR_STATUS);
+	flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT);
+	if (prot)
+		flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT_SET);
+	else
+		flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT_CLEAR);
+
+	if ((retcode =
+	     flash_status_check (info, sector, info->erase_blk_tout,
+				      prot ? "protect" : "unprotect")) == 0) {
+
+		info->protect[sector] = prot;
+
+		/*
+		 * On some of Intel's flash chips (marked via legacy_unlock)
+		 * unprotect unprotects all locking.
+		 */
+		if ((prot == 0) && (info->legacy_unlock)) {
+			flash_sect_t i;
+
+			for (i = 0; i < info->sector_count; i++) {
+				if (info->protect[i])
+					flash_real_protect (info, i, 1);
+			}
+		}
+	}
+	return retcode;
+}
+
+static int cfi_protect(struct device_d *dev, size_t count, unsigned long offset, int prot)
+{
+	flash_info_t *finfo = (flash_info_t *)dev->priv;
+	unsigned long start, end;
+	int i, ret = 0;
+
+	debug("%s: protect 0x%08x (size %d)\n", __FUNCTION__, offset, count);
+
+	start = find_sector(finfo, dev->map_base + offset);
+	end   = find_sector(finfo, dev->map_base + offset + count - 1);
+
+	for (i = start; i <= end; i++) {
+		ret = flash_real_protect (finfo, i, prot);
+		if (ret)
+			goto out;
+	}
+out:
+	putchar('\n');
+	return ret;
+}
+
 static ssize_t cfi_write(struct device_d* dev, const void* buf, size_t count, unsigned long offset, ulong flags)
 {
         flash_info_t *finfo = (flash_info_t *)dev->priv;
@@ -749,12 +803,14 @@ static void cfi_info (struct device_d* dev)
 }
 
 static struct driver_d cfi_driver = {
-        .name  = "cfi_flash",
-        .probe = cfi_probe,
-        .read  = mem_read,
-        .write = cfi_write,
-        .erase = cfi_erase,
-        .info  = cfi_info,
+        .name    = "cfi_flash",
+        .probe   = cfi_probe,
+        .read    = mem_read,
+        .write   = cfi_write,
+        .erase   = cfi_erase,
+        .info    = cfi_info,
+	.protect = cfi_protect,
+	.memmap  = generic_memmap_ro,
 };
 
 static int cfi_init(void)
@@ -764,41 +820,7 @@ static int cfi_init(void)
 
 device_initcall(cfi_init);
 
-#ifdef CFG_FLASH_PROTECTION
-
-int flash_real_protect (flash_info_t * info, long sector, int prot)
-{
-	int retcode = 0;
-
-	flash_write_cmd (info, sector, 0, FLASH_CMD_CLEAR_STATUS);
-	flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT);
-	if (prot)
-		flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT_SET);
-	else
-		flash_write_cmd (info, sector, 0, FLASH_CMD_PROTECT_CLEAR);
-
-	if ((retcode =
-	     flash_full_status_check (info, sector, info->erase_blk_tout,
-				      prot ? "protect" : "unprotect")) == 0) {
-
-		info->protect[sector] = prot;
-
-		/*
-		 * On some of Intel's flash chips (marked via legacy_unlock)
-		 * unprotect unprotects all locking.
-		 */
-		if ((prot == 0) && (info->legacy_unlock)) {
-			flash_sect_t i;
-
-			for (i = 0; i < info->sector_count; i++) {
-				if (info->protect[i])
-					flash_real_protect (info, i, 1);
-			}
-		}
-	}
-	return retcode;
-}
-
+#if 0
 /*
  * flash_read_user_serial - read the OneTimeProgramming cells
  */
@@ -829,13 +851,19 @@ static void flash_read_factory_serial (flash_info_t * info, void *buffer, int of
 	flash_write_cmd (info, 0, 0, info->cmd_reset);
 }
 
-#endif /* CFG_FLASH_PROTECTION */
+#endif 
+
+int flash_status_check (flash_info_t * info, flash_sect_t sector,
+			       uint64_t tout, char *prompt)
+{
+	return info->cfi_cmd_set->flash_status_check(info, sector, tout, prompt);
+}
 
 /*
  *  wait for XSR.7 to be set. Time out with an error if it does not.
  *  This routine does not set the flash to read-array mode.
  */
-int flash_status_check (flash_info_t * info, flash_sect_t sector,
+int flash_generic_status_check (flash_info_t * info, flash_sect_t sector,
 			       uint64_t tout, char *prompt)
 {
 	uint64_t start;