mtd/cfi_flash: fix write problems for Numonyx P33/30 32 MBit flashs

commit 54652991
Work around bug in Numonyx P33/P30 256-Mbit 65nm flash chips

fixes a problem for Numonyx P33/P30 flashes for 256-Mbit, but this leads
to problems for smaller versions of this chip e.g. the 32Mbit version
with deviceid 0x16 on mgcoge. So move the code for this work around to
an own function and check previously manufacturer id and device id to
not break other flashes which don't need this work around.

Signed-off-by: Holger Brunck <holger.brunck@keymile.com>
Signed-off-by: Heiko Schocher <hs@denx.de>
cc: Stefan Roese <sr@denx.de>
cc: Philippe De Muyter <phdm@macqel.be>
cc: Gerlando Falauto <gerlando.falauto@keymile.com>
Signed-off-by: Stefan Roese <sr@denx.de>
diff --git a/drivers/mtd/cfi_flash.c b/drivers/mtd/cfi_flash.c
index 50df9b0..f0f301a 100644
--- a/drivers/mtd/cfi_flash.c
+++ b/drivers/mtd/cfi_flash.c
@@ -1391,6 +1391,40 @@
  */
 #ifdef CONFIG_SYS_FLASH_PROTECTION
 
+static int cfi_protect_bugfix(flash_info_t *info, long sector, int prot)
+{
+	if ((info->manufacturer_id == (uchar)INTEL_MANUFACT) &&
+		(info->device_id == NUMONYX_256MBIT)) {
+		/*
+		 * see errata called
+		 * "Numonyx Axcell P33/P30 Specification Update" :)
+		 */
+		flash_write_cmd(info, sector, 0, FLASH_CMD_READ_ID);
+		if (!flash_isequal(info, sector, FLASH_OFFSET_PROTECT,
+				   prot)) {
+			/*
+			 * cmd must come before FLASH_CMD_PROTECT + 20us
+			 * Disable interrupts which might cause a timeout here.
+			 */
+			int flag = disable_interrupts();
+			unsigned short cmd;
+
+			if (prot)
+				cmd = FLASH_CMD_PROTECT_SET;
+			else
+				cmd = FLASH_CMD_PROTECT_CLEAR;
+				flash_write_cmd(info, sector, 0,
+					  FLASH_CMD_PROTECT);
+			flash_write_cmd(info, sector, 0, cmd);
+			/* re-enable interrupts if necessary */
+			if (flag)
+				enable_interrupts();
+		}
+		return 1;
+	}
+	return 0;
+}
+
 int flash_real_protect (flash_info_t * info, long sector, int prot)
 {
 	int retcode = 0;
@@ -1399,31 +1433,18 @@
 		case CFI_CMDSET_INTEL_PROG_REGIONS:
 		case CFI_CMDSET_INTEL_STANDARD:
 		case CFI_CMDSET_INTEL_EXTENDED:
-			/*
-			 * see errata called
-			 * "Numonyx Axcell P33/P30 Specification Update" :)
-			 */
-			flash_write_cmd (info, sector, 0, FLASH_CMD_READ_ID);
-			if (!flash_isequal (info, sector, FLASH_OFFSET_PROTECT,
-					    prot)) {
-				/*
-				 * cmd must come before FLASH_CMD_PROTECT + 20us
-				 * Disable interrupts which might cause a timeout here.
-				 */
-				int flag = disable_interrupts ();
-				unsigned short cmd;
-
+			if (!cfi_protect_bugfix(info, sector, prot)) {
+				flash_write_cmd(info, sector, 0,
+					 FLASH_CMD_CLEAR_STATUS);
+				flash_write_cmd(info, sector, 0,
+					FLASH_CMD_PROTECT);
 				if (prot)
-					cmd = FLASH_CMD_PROTECT_SET;
+					flash_write_cmd(info, sector, 0,
+						FLASH_CMD_PROTECT_SET);
 				else
-					cmd = FLASH_CMD_PROTECT_CLEAR;
+					flash_write_cmd(info, sector, 0,
+						FLASH_CMD_PROTECT_CLEAR);
 
-				flash_write_cmd (info, sector, 0,
-						  FLASH_CMD_PROTECT);
-				flash_write_cmd (info, sector, 0, cmd);
-				/* re-enable interrupts if necessary */
-				if (flag)
-					enable_interrupts ();
 			}
 			break;
 		case CFI_CMDSET_AMD_EXTENDED:
diff --git a/include/flash.h b/include/flash.h
index 0ca70d9..e614d07 100644
--- a/include/flash.h
+++ b/include/flash.h
@@ -348,6 +348,7 @@
 #define TOSH_ID_FVT160	0xC2		/* TC58FVT160 ID (16 M, top )		*/
 #define TOSH_ID_FVB160	0x43		/* TC58FVT160 ID (16 M, bottom )	*/
 #define PHILIPS_LPC2292 0x0401FF13  /* LPC2292 internal FLASH			*/
+#define NUMONYX_256MBIT	0x8922		/* Numonyx P33/30 256MBit 65nm	*/
 
 /*-----------------------------------------------------------------------
  * Internal FLASH identification codes