X-Git-Url: https://git.sur5r.net/?a=blobdiff_plain;f=drivers%2Fmtd%2Fcfi_flash.c;h=35294bc8c53ff37ffb059ac66765c733d71eacef;hb=d8bd820935bb9b8bf2717a259eeab4376e9ccc9a;hp=24eb33f0f6e053357b0baa7d82980c618a9217af;hpb=fa36ae790eb10ce70935f9a78f07703719dca422;p=u-boot

diff --git a/drivers/mtd/cfi_flash.c b/drivers/mtd/cfi_flash.c
index 24eb33f0f6..35294bc8c5 100644
--- a/drivers/mtd/cfi_flash.c
+++ b/drivers/mtd/cfi_flash.c
@@ -40,6 +40,7 @@
 #include <asm/byteorder.h>
 #include <environment.h>
 #include <mtd/cfi_flash.h>
+#include <watchdog.h>
 
 /*
  * This file implements a Common Flash Interface (CFI) driver for
@@ -62,18 +63,11 @@
  * reading and writing ... (yes there is such a Hardware).
  */
 
-#ifndef CONFIG_SYS_FLASH_BANKS_LIST
-#define CONFIG_SYS_FLASH_BANKS_LIST { CONFIG_SYS_FLASH_BASE }
-#endif
-
 static uint flash_offset_cfi[2] = { FLASH_OFFSET_CFI, FLASH_OFFSET_CFI_ALT };
+#ifdef CONFIG_FLASH_CFI_MTD
 static uint flash_verbose = 1;
-
-/* use CONFIG_SYS_MAX_FLASH_BANKS_DETECT if defined */
-#ifdef CONFIG_SYS_MAX_FLASH_BANKS_DETECT
-# define CFI_MAX_FLASH_BANKS	CONFIG_SYS_MAX_FLASH_BANKS_DETECT
 #else
-# define CFI_MAX_FLASH_BANKS	CONFIG_SYS_MAX_FLASH_BANKS
+#define flash_verbose 1
 #endif
 
 flash_info_t flash_info[CFI_MAX_FLASH_BANKS];	/* FLASH chips info */
@@ -85,6 +79,42 @@ flash_info_t flash_info[CFI_MAX_FLASH_BANKS];	/* FLASH chips info */
 #define CONFIG_SYS_FLASH_CFI_WIDTH	FLASH_CFI_8BIT
 #endif
 
+/*
+ * 0xffff is an undefined value for the configuration register. When
+ * this value is returned, the configuration register shall not be
+ * written at all (default mode).
+ */
+static u16 cfi_flash_config_reg(int i)
+{
+#ifdef CONFIG_SYS_CFI_FLASH_CONFIG_REGS
+	return ((u16 [])CONFIG_SYS_CFI_FLASH_CONFIG_REGS)[i];
+#else
+	return 0xffff;
+#endif
+}
+
+#if defined(CONFIG_SYS_MAX_FLASH_BANKS_DETECT)
+int cfi_flash_num_flash_banks = CONFIG_SYS_MAX_FLASH_BANKS_DETECT;
+#endif
+
+static phys_addr_t __cfi_flash_bank_addr(int i)
+{
+	return ((phys_addr_t [])CONFIG_SYS_FLASH_BANKS_LIST)[i];
+}
+phys_addr_t cfi_flash_bank_addr(int i)
+	__attribute__((weak, alias("__cfi_flash_bank_addr")));
+
+static unsigned long __cfi_flash_bank_size(int i)
+{
+#ifdef CONFIG_SYS_FLASH_BANKS_SIZES
+	return ((unsigned long [])CONFIG_SYS_FLASH_BANKS_SIZES)[i];
+#else
+	return 0;
+#endif
+}
+unsigned long cfi_flash_bank_size(int i)
+	__attribute__((weak, alias("__cfi_flash_bank_size")));
+
 static void __flash_write8(u8 value, void *addr)
 {
 	__raw_writeb(value, addr);
@@ -153,7 +183,7 @@ u64 flash_read64(void *addr)__attribute__((weak, alias("__flash_read64")));
 flash_info_t *flash_get_info(ulong base)
 {
 	int i;
-	flash_info_t * info = 0;
+	flash_info_t *info = NULL;
 
 	for (i = 0; i < CONFIG_SYS_MAX_FLASH_BANKS; i++) {
 		info = & flash_info[i];
@@ -162,7 +192,7 @@ flash_info_t *flash_get_info(ulong base)
 			break;
 	}
 
-	return i == CONFIG_SYS_MAX_FLASH_BANKS ? 0 : info;
+	return info;
 }
 #endif
 
@@ -537,17 +567,25 @@ static int flash_status_check (flash_info_t * info, flash_sect_t sector,
 	ulong start;
 
 #if CONFIG_SYS_HZ != 1000
-	tout *= CONFIG_SYS_HZ/1000;
+	if ((ulong)CONFIG_SYS_HZ > 100000)
+		tout *= (ulong)CONFIG_SYS_HZ / 1000;  /* for a big HZ, avoid overflow */
+	else
+		tout = DIV_ROUND_UP(tout * (ulong)CONFIG_SYS_HZ, 1000);
 #endif
 
 	/* Wait for command completion */
+#ifdef CONFIG_SYS_LOW_RES_TIMER
+	reset_timer();
+#endif
 	start = get_timer (0);
+	WATCHDOG_RESET();
 	while (flash_is_busy (info, sector)) {
 		if (get_timer (start) > tout) {
 			printf ("Flash %s timeout at address %lx data %lx\n",
 				prompt, info->start[sector],
 				flash_read_long (info, sector, 0));
 			flash_write_cmd (info, sector, 0, info->cmd_reset);
+			udelay(1);
 			return ERR_TIMOUT;
 		}
 		udelay (1);		/* also triggers watchdog */
@@ -595,6 +633,7 @@ static int flash_full_status_check (flash_info_t * info, flash_sect_t sector,
 				puts ("Vpp Low Error.\n");
 		}
 		flash_write_cmd (info, sector, 0, info->cmd_reset);
+		udelay(1);
 		break;
 	default:
 		break;
@@ -602,6 +641,67 @@ static int flash_full_status_check (flash_info_t * info, flash_sect_t sector,
 	return retcode;
 }
 
+static int use_flash_status_poll(flash_info_t *info)
+{
+#ifdef CONFIG_SYS_CFI_FLASH_STATUS_POLL
+	if (info->vendor == CFI_CMDSET_AMD_EXTENDED ||
+	    info->vendor == CFI_CMDSET_AMD_STANDARD)
+		return 1;
+#endif
+	return 0;
+}
+
+static int flash_status_poll(flash_info_t *info, void *src, void *dst,
+			     ulong tout, char *prompt)
+{
+#ifdef CONFIG_SYS_CFI_FLASH_STATUS_POLL
+	ulong start;
+	int ready;
+
+#if CONFIG_SYS_HZ != 1000
+	if ((ulong)CONFIG_SYS_HZ > 100000)
+		tout *= (ulong)CONFIG_SYS_HZ / 1000;  /* for a big HZ, avoid overflow */
+	else
+		tout = DIV_ROUND_UP(tout * (ulong)CONFIG_SYS_HZ, 1000);
+#endif
+
+	/* Wait for command completion */
+#ifdef CONFIG_SYS_LOW_RES_TIMER
+	reset_timer();
+#endif
+	start = get_timer(0);
+	WATCHDOG_RESET();
+	while (1) {
+		switch (info->portwidth) {
+		case FLASH_CFI_8BIT:
+			ready = flash_read8(dst) == flash_read8(src);
+			break;
+		case FLASH_CFI_16BIT:
+			ready = flash_read16(dst) == flash_read16(src);
+			break;
+		case FLASH_CFI_32BIT:
+			ready = flash_read32(dst) == flash_read32(src);
+			break;
+		case FLASH_CFI_64BIT:
+			ready = flash_read64(dst) == flash_read64(src);
+			break;
+		default:
+			ready = 0;
+			break;
+		}
+		if (ready)
+			break;
+		if (get_timer(start) > tout) {
+			printf("Flash %s timeout at address %lx data %lx\n",
+			       prompt, (ulong)dst, (ulong)flash_read8(dst));
+			return ERR_TIMOUT;
+		}
+		udelay(1);		/* also triggers watchdog */
+	}
+#endif /* CONFIG_SYS_CFI_FLASH_STATUS_POLL */
+	return ERR_OK;
+}
+
 /*-----------------------------------------------------------------------
  */
 static void flash_add_byte (flash_info_t * info, cfiword_t * cword, uchar c)
@@ -653,8 +753,12 @@ static void flash_add_byte (flash_info_t * info, cfiword_t * cword, uchar c)
 static flash_sect_t find_sector (flash_info_t * info, ulong addr)
 {
 	static flash_sect_t saved_sector = 0; /* previously found sector */
+	static flash_info_t *saved_info = 0; /* previously used flash bank */
 	flash_sect_t sector = saved_sector;
 
+	if ((info != saved_info) || (sector >= info->sector_count))
+		sector = 0;
+
 	while ((info->start[sector] < addr)
 			&& (sector < info->sector_count - 1))
 		sector++;
@@ -666,6 +770,7 @@ static flash_sect_t find_sector (flash_info_t * info, ulong addr)
 		sector--;
 
 	saved_sector = sector;
+	saved_info = info;
 	return sector;
 }
 
@@ -749,7 +854,12 @@ static int flash_write_cfiword (flash_info_t * info, ulong dest,
 	if (!sect_found)
 		sect = find_sector (info, dest);
 
-	return flash_full_status_check (info, sect, info->write_tout, "write");
+	if (use_flash_status_poll(info))
+		return flash_status_poll(info, &cword, dstaddr,
+					 info->write_tout, "write");
+	else
+		return flash_full_status_check(info, sect,
+					       info->write_tout, "write");
 }
 
 #ifdef CONFIG_SYS_FLASH_USE_BUFFER_WRITE
@@ -763,7 +873,7 @@ static int flash_write_cfibuffer (flash_info_t * info, ulong dest, uchar * cp,
 	void *src = cp;
 	void *dst = (void *)dest;
 	void *dst2 = dst;
-	int flag = 0;
+	int flag = 1;
 	uint offset = 0;
 	unsigned int shift;
 	uchar write_cmd;
@@ -788,7 +898,7 @@ static int flash_write_cfibuffer (flash_info_t * info, ulong dest, uchar * cp,
 
 	cnt = len >> shift;
 
-	while ((cnt-- > 0) && (flag == 0)) {
+	while ((cnt-- > 0) && (flag == 1)) {
 		switch (info->portwidth) {
 		case FLASH_CFI_8BIT:
 			flag = ((flash_read8(dst2) & flash_read8(src)) ==
@@ -911,9 +1021,15 @@ static int flash_write_cfibuffer (flash_info_t * info, ulong dest, uchar * cp,
 		}
 
 		flash_write_cmd (info, sector, 0, AMD_CMD_WRITE_BUFFER_CONFIRM);
-		retcode = flash_full_status_check (info, sector,
-						   info->buffer_write_tout,
-						   "buffer write");
+		if (use_flash_status_poll(info))
+			retcode = flash_status_poll(info, src - (1 << shift),
+						    dst - (1 << shift),
+						    info->buffer_write_tout,
+						    "buffer write");
+		else
+			retcode = flash_full_status_check(info, sector,
+							  info->buffer_write_tout,
+							  "buffer write");
 		break;
 
 	default:
@@ -935,6 +1051,7 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 	int rcode = 0;
 	int prot;
 	flash_sect_t sect;
+	int st;
 
 	if (info->flash_id != FLASH_MAN_CFI) {
 		puts ("Can't erase unknown flash type - aborted\n");
@@ -998,10 +1115,20 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 				break;
 			}
 
-			if (flash_full_status_check
-			    (info, sect, info->erase_blk_tout, "erase")) {
+			if (use_flash_status_poll(info)) {
+				cfiword_t cword = (cfiword_t)0xffffffffffffffffULL;
+				void *dest;
+				dest = flash_map(info, sect, 0);
+				st = flash_status_poll(info, &cword, dest,
+						       info->erase_blk_tout, "erase");
+				flash_unmap(info, sect, 0, dest);
+			} else
+				st = flash_full_status_check(info, sect,
+							     info->erase_blk_tout,
+							     "erase");
+			if (st)
 				rcode = 1;
-			} else if (flash_verbose)
+			else if (flash_verbose)
 				putc ('.');
 		}
 	}
@@ -1012,8 +1139,30 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 	return rcode;
 }
 
-/*-----------------------------------------------------------------------
- */
+#ifdef CONFIG_SYS_FLASH_EMPTY_INFO
+static int sector_erased(flash_info_t *info, int i)
+{
+	int k;
+	int size;
+	u32 *flash;
+
+	/*
+	 * Check if whole sector is erased
+	 */
+	size = flash_sector_size(info, i);
+	flash = (u32 *)info->start[i];
+	/* divide by 4 for longword access */
+	size = size >> 2;
+
+	for (k = 0; k < size; k++) {
+		if (flash_read32(flash++) != 0xffffffff)
+			return 0;	/* not erased */
+	}
+
+	return 1;			/* erased */
+}
+#endif /* CONFIG_SYS_FLASH_EMPTY_INFO */
+
 void flash_print_info (flash_info_t * info)
 {
 	int i;
@@ -1023,7 +1172,7 @@ void flash_print_info (flash_info_t * info)
 		return;
 	}
 
-	printf ("%s FLASH (%d x %d)",
+	printf ("%s flash (%d x %d)",
 		info->name,
 		(info->portwidth << 3), (info->chipwidth << 3));
 	if (info->size < 1024*1024)
@@ -1058,10 +1207,13 @@ void flash_print_info (flash_info_t * info)
 			printf ("Unknown (%d)", info->vendor);
 			break;
 	}
-	printf (" command set, Manufacturer ID: 0x%02X, Device ID: 0x%02X",
-		info->manufacturer_id, info->device_id);
-	if (info->device_id == 0x7E) {
-		printf("%04X", info->device_id2);
+	printf (" command set, Manufacturer ID: 0x%02X, Device ID: 0x",
+		info->manufacturer_id);
+	printf (info->chipwidth == FLASH_CFI_16BIT ? "%04X" : "%02X",
+		info->device_id);
+	if ((info->device_id & 0xff) == 0x7E) {
+		printf(info->chipwidth == FLASH_CFI_16BIT ? "%04X" : "%02X",
+		info->device_id2);
 	}
 	printf ("\n  Erase timeout: %ld ms, write timeout: %ld ms\n",
 		info->erase_blk_tout,
@@ -1075,32 +1227,15 @@ void flash_print_info (flash_info_t * info)
 
 	puts ("\n  Sector Start Addresses:");
 	for (i = 0; i < info->sector_count; ++i) {
+		if (ctrlc())
+			break;
 		if ((i % 5) == 0)
-			printf ("\n");
+			putc('\n');
 #ifdef CONFIG_SYS_FLASH_EMPTY_INFO
-		int k;
-		int size;
-		int erased;
-		volatile unsigned long *flash;
-
-		/*
-		 * Check if whole sector is erased
-		 */
-		size = flash_sector_size(info, i);
-		erased = 1;
-		flash = (volatile unsigned long *) info->start[i];
-		size = size >> 2;	/* divide by 4 for longword access */
-		for (k = 0; k < size; k++) {
-			if (*flash++ != 0xffffffff) {
-				erased = 0;
-				break;
-			}
-		}
-
 		/* print empty and read-only info */
 		printf ("  %08lX %c %s ",
 			info->start[i],
-			erased ? 'E' : ' ',
+			sector_erased(info, i) ? 'E' : ' ',
 			info->protect[i] ? "RO" : "  ");
 #else	/* ! CONFIG_SYS_FLASH_EMPTY_INFO */
 		printf ("  %08lX   %s ",
@@ -1264,15 +1399,32 @@ int flash_real_protect (flash_info_t * info, long sector, int prot)
 		case CFI_CMDSET_INTEL_PROG_REGIONS:
 		case CFI_CMDSET_INTEL_STANDARD:
 		case CFI_CMDSET_INTEL_EXTENDED:
-			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
+			/*
+			 * 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_CLEAR);
+						  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:
 		case CFI_CMDSET_AMD_STANDARD:
@@ -1307,6 +1459,11 @@ int flash_real_protect (flash_info_t * info, long sector, int prot)
 #endif
 	};
 
+	/*
+	 * Flash needs to be in status register read mode for
+	 * flash_full_status_check() to work correctly
+	 */
+	flash_write_cmd(info, sector, 0, FLASH_CMD_READ_STATUS);
 	if ((retcode =
 	     flash_full_status_check (info, sector, info->erase_blk_tout,
 				      prot ? "protect" : "unprotect")) == 0) {
@@ -1343,6 +1500,7 @@ void flash_read_user_serial (flash_info_t * info, void *buffer, int offset,
 	flash_write_cmd (info, 0, 0, FLASH_CMD_READ_ID);
 	memcpy (dst, src + offset, len);
 	flash_write_cmd (info, 0, 0, info->cmd_reset);
+	udelay(1);
 	flash_unmap(info, 0, FLASH_OFFSET_USER_PROTECTION, src);
 }
 
@@ -1358,6 +1516,7 @@ void flash_read_factory_serial (flash_info_t * info, void *buffer, int offset,
 	flash_write_cmd (info, 0, 0, FLASH_CMD_READ_ID);
 	memcpy (buffer, src + offset, len);
 	flash_write_cmd (info, 0, 0, info->cmd_reset);
+	udelay(1);
 	flash_unmap(info, 0, FLASH_OFFSET_INTEL_PROTECTION, src);
 }
 
@@ -1389,12 +1548,14 @@ static void cfi_reverse_geometry(struct cfi_qry *qry)
 static void cmdset_intel_read_jedec_ids(flash_info_t *info)
 {
 	flash_write_cmd(info, 0, 0, FLASH_CMD_RESET);
+	udelay(1);
 	flash_write_cmd(info, 0, 0, FLASH_CMD_READ_ID);
 	udelay(1000); /* some flash are slow to respond */
 	info->manufacturer_id = flash_read_uchar (info,
 					FLASH_OFFSET_MANUFACTURER_ID);
-	info->device_id = flash_read_uchar (info,
-					FLASH_OFFSET_DEVICE_ID);
+	info->device_id = (info->chipwidth == FLASH_CFI_16BIT) ?
+			flash_read_word (info, FLASH_OFFSET_DEVICE_ID) :
+			flash_read_uchar (info, FLASH_OFFSET_DEVICE_ID);
 	flash_write_cmd(info, 0, 0, FLASH_CMD_RESET);
 }
 
@@ -1451,11 +1612,20 @@ static void cmdset_amd_read_jedec_ids(flash_info_t *info)
 	case FLASH_CFI_16BIT:
 		info->device_id = flash_read_word (info,
 						FLASH_OFFSET_DEVICE_ID);
+		if ((info->device_id & 0xff) == 0x7E) {
+			/* AMD 3-byte (expanded) device ids */
+			info->device_id2 = flash_read_uchar (info,
+						FLASH_OFFSET_DEVICE_ID2);
+			info->device_id2 <<= 8;
+			info->device_id2 |= flash_read_uchar (info,
+						FLASH_OFFSET_DEVICE_ID3);
+		}
 		break;
 	default:
 		break;
 	}
 	flash_write_cmd(info, 0, 0, AMD_CMD_RESET);
+	udelay(1);
 }
 
 static int cmdset_amd_init(flash_info_t *info, struct cfi_qry *qry)
@@ -1582,6 +1752,7 @@ void __flash_cmd_reset(flash_info_t *info)
 	 * that AMD flash roms ignore the Intel command.
 	 */
 	flash_write_cmd(info, 0, 0, AMD_CMD_RESET);
+	udelay(1);
 	flash_write_cmd(info, 0, 0, FLASH_CMD_RESET);
 }
 void flash_cmd_reset(flash_info_t *info)
@@ -1704,11 +1875,16 @@ static void flash_fixup_stm(flash_info_t *info, struct cfi_qry *qry)
 	if (qry->num_erase_regions > 1) {
 		/* reverse geometry if top boot part */
 		if (info->cfi_version < 0x3131) {
-			/* CFI < 1.1, guess by device id (M29W320{DT,ET} only) */
-			if (info->device_id == 0x22CA ||
-			    info->device_id == 0x2256) {
+			/* CFI < 1.1, guess by device id */
+			if (info->device_id == 0x22CA || /* M29W320DT */
+			    info->device_id == 0x2256 || /* M29W320ET */
+			    info->device_id == 0x22D7) { /* M29W800DT */
 				cfi_reverse_geometry(qry);
 			}
+		} else if (flash_read_uchar(info, info->ext_addr + 0xf) == 3) {
+			/* CFI >= 1.1, deduct from top/bottom flag */
+			/* note: ext_addr is valid since cfi_version > 0 */
+			cfi_reverse_geometry(qry);
 		}
 	}
 }
@@ -1729,6 +1905,7 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 	int erase_region_size;
 	int erase_region_count;
 	struct cfi_qry qry;
+	unsigned long max_size;
 
 	memset(&qry, 0, sizeof(qry));
 
@@ -1780,7 +1957,8 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 
 		/* Do manufacturer-specific fixups */
 		switch (info->manufacturer_id) {
-		case 0x0001:
+		case 0x0001: /* AMD */
+		case 0x0037: /* AMIC */
 			flash_fixup_amd(info, &qry);
 			break;
 		case 0x001f:
@@ -1806,6 +1984,14 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 		debug ("size_ratio %d port %d bits chip %d bits\n",
 		       size_ratio, info->portwidth << CFI_FLASH_SHIFT_WIDTH,
 		       info->chipwidth << CFI_FLASH_SHIFT_WIDTH);
+		info->size = 1 << qry.dev_size;
+		/* multiply the size by the number of chips */
+		info->size *= size_ratio;
+		max_size = cfi_flash_bank_size(banknum);
+		if (max_size && (info->size > max_size)) {
+			debug("[truncated from %ldMiB]", info->size >> 20);
+			info->size = max_size;
+		}
 		debug ("found %d erase regions\n", num_erase_regions);
 		sect_cnt = 0;
 		sector = base;
@@ -1826,6 +2012,8 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 			debug ("erase_region_count = %d erase_region_size = %d\n",
 				erase_region_count, erase_region_size);
 			for (j = 0; j < erase_region_count; j++) {
+				if (sector - base >= info->size)
+					break;
 				if (sect_cnt >= CONFIG_SYS_MAX_FLASH_SECT) {
 					printf("ERROR: too many flash sectors\n");
 					break;
@@ -1844,6 +2032,13 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 				case CFI_CMDSET_INTEL_PROG_REGIONS:
 				case CFI_CMDSET_INTEL_EXTENDED:
 				case CFI_CMDSET_INTEL_STANDARD:
+					/*
+					 * Set flash to read-id mode. Otherwise
+					 * reading protected status is not
+					 * guaranteed.
+					 */
+					flash_write_cmd(info, sect_cnt, 0,
+							FLASH_CMD_READ_ID);
 					info->protect[sect_cnt] =
 						flash_isset (info, sect_cnt,
 							     FLASH_OFFSET_PROTECT,
@@ -1859,9 +2054,6 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 		}
 
 		info->sector_count = sect_cnt;
-		info->size = 1 << qry.dev_size;
-		/* multiply the size by the number of chips */
-		info->size *= size_ratio;
 		info->buffer_size = 1 << le16_to_cpu(qry.max_buf_write_size);
 		tmp = 1 << qry.block_erase_timeout_typ;
 		info->erase_blk_tout = tmp *
@@ -1888,45 +2080,116 @@ ulong flash_get_size (phys_addr_t base, int banknum)
 	return (info->size);
 }
 
+#ifdef CONFIG_FLASH_CFI_MTD
 void flash_set_verbose(uint v)
 {
 	flash_verbose = v;
 }
+#endif
+
+static void cfi_flash_set_config_reg(u32 base, u16 val)
+{
+#ifdef CONFIG_SYS_CFI_FLASH_CONFIG_REGS
+	/*
+	 * Only set this config register if really defined
+	 * to a valid value (0xffff is invalid)
+	 */
+	if (val == 0xffff)
+		return;
+
+	/*
+	 * Set configuration register. Data is "encrypted" in the 16 lower
+	 * address bits.
+	 */
+	flash_write16(FLASH_CMD_SETUP, (void *)(base + (val << 1)));
+	flash_write16(FLASH_CMD_SET_CR_CONFIRM, (void *)(base + (val << 1)));
+
+	/*
+	 * Finally issue reset-command to bring device back to
+	 * read-array mode
+	 */
+	flash_write16(FLASH_CMD_RESET, (void *)base);
+#endif
+}
 
 /*-----------------------------------------------------------------------
  */
-unsigned long flash_init (void)
+
+void flash_protect_default(void)
 {
-	unsigned long size = 0;
-	int i;
 #if defined(CONFIG_SYS_FLASH_AUTOPROTECT_LIST)
+	int i;
 	struct apl_s {
 		ulong start;
 		ulong size;
 	} apl[] = CONFIG_SYS_FLASH_AUTOPROTECT_LIST;
 #endif
 
+	/* Monitor protection ON by default */
+#if (CONFIG_SYS_MONITOR_BASE >= CONFIG_SYS_FLASH_BASE) && \
+	(!defined(CONFIG_MONITOR_IS_IN_RAM))
+	flash_protect(FLAG_PROTECT_SET,
+		       CONFIG_SYS_MONITOR_BASE,
+		       CONFIG_SYS_MONITOR_BASE + monitor_flash_len  - 1,
+		       flash_get_info(CONFIG_SYS_MONITOR_BASE));
+#endif
+
+	/* Environment protection ON by default */
+#ifdef CONFIG_ENV_IS_IN_FLASH
+	flash_protect(FLAG_PROTECT_SET,
+		       CONFIG_ENV_ADDR,
+		       CONFIG_ENV_ADDR + CONFIG_ENV_SECT_SIZE - 1,
+		       flash_get_info(CONFIG_ENV_ADDR));
+#endif
+
+	/* Redundant environment protection ON by default */
+#ifdef CONFIG_ENV_ADDR_REDUND
+	flash_protect(FLAG_PROTECT_SET,
+		       CONFIG_ENV_ADDR_REDUND,
+		       CONFIG_ENV_ADDR_REDUND + CONFIG_ENV_SECT_SIZE - 1,
+		       flash_get_info(CONFIG_ENV_ADDR_REDUND));
+#endif
+
+#if defined(CONFIG_SYS_FLASH_AUTOPROTECT_LIST)
+	for (i = 0; i < (sizeof(apl) / sizeof(struct apl_s)); i++) {
+		debug("autoprotecting from %08lx to %08lx\n",
+		      apl[i].start, apl[i].start + apl[i].size - 1);
+		flash_protect(FLAG_PROTECT_SET,
+			       apl[i].start,
+			       apl[i].start + apl[i].size - 1,
+			       flash_get_info(apl[i].start));
+	}
+#endif
+}
+
+unsigned long flash_init (void)
+{
+	unsigned long size = 0;
+	int i;
+
 #ifdef CONFIG_SYS_FLASH_PROTECTION
 	/* read environment from EEPROM */
 	char s[64];
-	getenv_r ("unlock", s, sizeof(s));
+	getenv_f("unlock", s, sizeof(s));
 #endif
 
-#define BANK_BASE(i)	(((phys_addr_t [CFI_MAX_FLASH_BANKS])CONFIG_SYS_FLASH_BANKS_LIST)[i])
-
 	/* Init: no FLASHes known */
 	for (i = 0; i < CONFIG_SYS_MAX_FLASH_BANKS; ++i) {
 		flash_info[i].flash_id = FLASH_UNKNOWN;
 
-		if (!flash_detect_legacy (BANK_BASE(i), i))
-			flash_get_size (BANK_BASE(i), i);
+		/* Optionally write flash configuration register */
+		cfi_flash_set_config_reg(cfi_flash_bank_addr(i),
+					 cfi_flash_config_reg(i));
+
+		if (!flash_detect_legacy(cfi_flash_bank_addr(i), i))
+			flash_get_size(cfi_flash_bank_addr(i), i);
 		size += flash_info[i].size;
 		if (flash_info[i].flash_id == FLASH_UNKNOWN) {
 #ifndef CONFIG_SYS_FLASH_QUIET_TEST
-			printf ("## Unknown FLASH on Bank %d "
+			printf ("## Unknown flash on Bank %d "
 				"- Size = 0x%08lx = %ld MB\n",
 				i+1, flash_info[i].size,
-				flash_info[i].size << 20);
+				flash_info[i].size >> 20);
 #endif /* CONFIG_SYS_FLASH_QUIET_TEST */
 		}
 #ifdef CONFIG_SYS_FLASH_PROTECTION
@@ -1979,41 +2242,7 @@ unsigned long flash_init (void)
 #endif /* CONFIG_SYS_FLASH_PROTECTION */
 	}
 
-	/* Monitor protection ON by default */
-#if (CONFIG_SYS_MONITOR_BASE >= CONFIG_SYS_FLASH_BASE)
-	flash_protect (FLAG_PROTECT_SET,
-		       CONFIG_SYS_MONITOR_BASE,
-		       CONFIG_SYS_MONITOR_BASE + monitor_flash_len  - 1,
-		       flash_get_info(CONFIG_SYS_MONITOR_BASE));
-#endif
-
-	/* Environment protection ON by default */
-#ifdef CONFIG_ENV_IS_IN_FLASH
-	flash_protect (FLAG_PROTECT_SET,
-		       CONFIG_ENV_ADDR,
-		       CONFIG_ENV_ADDR + CONFIG_ENV_SECT_SIZE - 1,
-		       flash_get_info(CONFIG_ENV_ADDR));
-#endif
-
-	/* Redundant environment protection ON by default */
-#ifdef CONFIG_ENV_ADDR_REDUND
-	flash_protect (FLAG_PROTECT_SET,
-		       CONFIG_ENV_ADDR_REDUND,
-		       CONFIG_ENV_ADDR_REDUND + CONFIG_ENV_SECT_SIZE - 1,
-		       flash_get_info(CONFIG_ENV_ADDR_REDUND));
-#endif
-
-#if defined(CONFIG_SYS_FLASH_AUTOPROTECT_LIST)
-	for (i = 0; i < (sizeof(apl) / sizeof(struct apl_s)); i++) {
-		debug("autoprotecting from %08x to %08x\n",
-		      apl[i].start, apl[i].start + apl[i].size - 1);
-		flash_protect (FLAG_PROTECT_SET,
-			       apl[i].start,
-			       apl[i].start + apl[i].size - 1,
-			       flash_get_info(apl[i].start));
-	}
-#endif
-
+	flash_protect_default();
 #ifdef CONFIG_FLASH_CFI_MTD
 	cfi_mtd_init();
 #endif