* Patch by Marc Singer, 29 May 2003:LABEL_2003_05_31_2115

  Fixed rarp boot method for IA32 and other little-endian CPUs.

* Patch by Marc Singer, 28 May 2003:
  Added port I/O commands.

* Patch by Matthew McClintock, 28 May 2003
  - cpu/mpc824x/start.S: fix relocation code when booting from RAM
  - minor patches for utx8245

* Patch by Daniel Engström, 28 May 2003:
  x86 update

* Patch by Dave Ellis, 9 May 2003 + 27 May 2003:
  add nand flash support to SXNI855T configuration
  fix/extend nand flash support:
  - fix 'nand erase' command so does not erase bad blocks
  - fix 'nand write' command so does not write to bad blocks
  - fix nand_probe() so handles no flash detected properly
  - add doc/README.nand
  - add .jffs2 and .oob options to nand read/write
  - add 'nand bad' command to list bad blocks
  - add 'clean' option to 'nand erase' to write JFFS2 clean markers
  - make NAND read/write faster

* Patch by Rune Torgersen, 23 May 2003:
  Update for MPC8266ADS board

6 files changed, 1426 insertions, 428 deletions

diff --git a/board/sc520_cdp/config.mk b/board/sc520_cdp/config.mk
index d4cd9986d63..2253815647a 100644
--- a/board/sc520_cdp/config.mk
+++ b/board/sc520_cdp/config.mk
@@ -22,4 +22,4 @@
 #
 
 
-TEXT_BASE = 0x387e0000
+TEXT_BASE = 0x387c0000
diff --git a/board/sc520_cdp/flash.c b/board/sc520_cdp/flash.c
index 973cde8b99a..1ebb32e2e98 100644
--- a/board/sc520_cdp/flash.c
+++ b/board/sc520_cdp/flash.c
@@ -1,4 +1,7 @@
 /*
+ * (C) Copyright 2002, 2003
+ * Daniel Engström, Omicron Ceti AB, daniel@omicron.se
+ * 
  * (C) Copyright 2002
  * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
  * Alex Zuepke <azu@sysgo.de>
@@ -23,31 +26,23 @@
  */
 
 #include <common.h>
+#include <asm/io.h>
+#include <pci.h>
+#include <asm/ic/sc520.h>
 
-ulong myflush(void);
+#define PROBE_BUFFER_SIZE 1024
+static unsigned char buffer[PROBE_BUFFER_SIZE];
 
+#define SC520_MAX_FLASH_BANKS  3
+#define SC520_FLASH_BANK0_BASE 0x38000000  /* BOOTCS */
+#define SC520_FLASH_BANK1_BASE 0x30000000  /* ROMCS0 */
+#define SC520_FLASH_BANK2_BASE 0x28000000  /* ROMCS1 */
+#define SC520_FLASH_BANKSIZE   0x8000000
 
-#define FLASH_BANK_SIZE 0x400000	/* 4 MB */
-#define MAIN_SECT_SIZE  0x20000		/* 128 KB */
+#define AMD29LV016B_SIZE        0x200000
+#define AMD29LV016B_SECTORS     32
 
-flash_info_t    flash_info[CFG_MAX_FLASH_BANKS];
-
-
-#define CMD_READ_ARRAY		0x00F000F0
-#define CMD_UNLOCK1		0x00AA00AA
-#define CMD_UNLOCK2		0x00550055
-#define CMD_ERASE_SETUP		0x00800080
-#define CMD_ERASE_CONFIRM	0x00300030
-#define CMD_PROGRAM		0x00A000A0
-#define CMD_UNLOCK_BYPASS	0x00200020
-
-#define MEM_FLASH_ADDR1		(*(volatile u32 *)(CFG_FLASH_BASE + (0x00000555 << 2)))
-#define MEM_FLASH_ADDR2		(*(volatile u32 *)(CFG_FLASH_BASE + (0x000002AA << 2)))
-
-#define BIT_ERASE_DONE		0x00800080
-#define BIT_RDY_MASK		0x00800080
-#define BIT_PROGRAM_ERROR	0x00200020
-#define BIT_TIMEOUT		0x80000000 /* our flag */
+flash_info_t    flash_info[SC520_MAX_FLASH_BANKS];
 
 #define READY 1
 #define ERR   2
@@ -56,394 +51,592 @@ flash_info_t    flash_info[CFG_MAX_FLASH_BANKS];
 /*-----------------------------------------------------------------------
  */
 
-ulong flash_init(void)
+
+static u32 _probe_flash(u32 addr, u32 bw, int il)
 {
-    int i, j;
-    ulong size = 0;
-
-    for (i = 0; i < CFG_MAX_FLASH_BANKS; i++)
-    {
-	ulong flashbase = 0;
-	flash_info[i].flash_id =
-	  (AMD_MANUFACT & FLASH_VENDMASK) |
-	  (AMD_ID_LV160B & FLASH_TYPEMASK);
-	flash_info[i].size = FLASH_BANK_SIZE;
-	flash_info[i].sector_count = CFG_MAX_FLASH_SECT;
-	memset(flash_info[i].protect, 0, CFG_MAX_FLASH_SECT);
-	if (i == 0)
-	  flashbase = PHYS_FLASH_1;
-	else
-	  panic("configured to many flash banks!\n");
-	for (j = 0; j < flash_info[i].sector_count; j++)
-	{
-
-	    if (j <= 3)
-	    {
-		/* 1st one is 32 KB */
-		if (j == 0)
-		{
-			flash_info[i].start[j] = flashbase + 0;
+	u32 result=0;
+	
+	/* First do an unlock cycle for the benefit of
+	 * devices that need it */
+	
+	switch (bw) {
+		
+	case 1:
+		*(volatile u8*)(addr+0x5555) = 0xaa;
+		*(volatile u8*)(addr+0x2aaa) = 0x55;
+		*(volatile u8*)(addr+0x5555) = 0x90;
+		
+		/* Read vendor */
+		result = *(volatile u8*)addr;
+		result <<= 16;
+		
+		/* Read device */
+		result |= *(volatile u8*)(addr+2);
+		
+		/* Return device to data mode */
+		*(volatile u8*)addr = 0xff;
+		*(volatile u8*)(addr+0x5555), 0xf0;  
+		break;
+		
+	case 2:
+		*(volatile u16*)(addr+0xaaaa) = 0xaaaa;
+		*(volatile u16*)(addr+0x5554) = 0x5555;
+		
+		/* Issue identification command */
+		if (il == 2) {
+			*(volatile u16*)(addr+0xaaaa) = 0x9090;
+			
+			/* Read vendor */
+			result = *(volatile u8*)addr;
+			result <<= 16;
+			
+			/* Read device */
+			result |= *(volatile u8*)(addr+2);
+			
+			/* Return device to data mode */
+			*(volatile u16*)addr =  0xffff;
+			*(volatile u16*)(addr+0xaaaa), 0xf0f0;  
+			
+		} else {
+			*(volatile u8*)(addr+0xaaaa) = 0x90;
+			/* Read vendor */
+			result = *(volatile u16*)addr;
+			result <<= 16;
+			
+			/* Read device */
+			result |= *(volatile u16*)(addr+2);
+			
+			/* Return device to data mode */
+			*(volatile u8*)addr = 0xff;
+			*(volatile u8*)(addr+0xaaaa), 0xf0;  			
 		}
-
-		/* 2nd and 3rd are both 16 KB */
-		if ((j == 1) || (j == 2))
-		{
-			flash_info[i].start[j] = flashbase + 0x8000 + (j-1)*0x4000;
+		
+		break;
+		
+	 case 4:
+		*(volatile u32*)(addr+0x5554) = 0xaaaaaaaa;
+		*(volatile u32*)(addr+0xaaa8) = 0x55555555;
+		
+		switch (il) {
+		case 1:
+			/* Issue identification command */
+			*(volatile u8*)(addr+0x5554) = 0x90;
+			
+			/* Read vendor */
+			result = *(volatile u16*)addr;
+			result <<= 16;
+		
+			/* Read device */
+			result |= *(volatile u16*)(addr+4);
+			
+			/* Return device to data mode */
+			*(volatile u8*)addr =  0xff;
+			*(volatile u8*)(addr+0x5554), 0xf0;  
+			break;
+			
+		case 2:
+			/* Issue identification command */
+			*(volatile u32*)(addr + 0x5554) = 0x00900090;
+			
+			/* Read vendor */
+			result = *(volatile u16*)addr;
+			result <<= 16;
+			
+			/* Read device */
+			result |= *(volatile u16*)(addr+4);
+			
+			/* Return device to data mode */
+			*(volatile u32*)addr =  0x00ff00ff;
+			*(volatile u32*)(addr+0x5554), 0x00f000f0;  
+			break;
+			
+		case 4:
+			/* Issue identification command */
+			*(volatile u32*)(addr+0x5554) = 0x90909090;
+			
+			/* Read vendor */
+			result = *(volatile u8*)addr;
+			result <<= 16;
+			
+			/* Read device */
+			result |= *(volatile u8*)(addr+4);
+			
+			/* Return device to data mode */
+			*(volatile u32*)addr =  0xffffffff;
+			*(volatile u32*)(addr+0x5554), 0xf0f0f0f0; 
+			break;
 		}
+		break;
+	}
+	
+	
+	
+	return result;
+}
+
+extern int _probe_flash_end;
+asm ("_probe_flash_end:\n"
+     ".long 0\n");
 
-		/* 4th 64 KB */
-		if (j == 3)
-		{
-			flash_info[i].start[j] = flashbase + 0x10000;
+static int identify_flash(unsigned address, int width)
+{
+	int is;	
+	int device;
+	int vendor;	
+	int size;
+	unsigned res;
+	
+	u32 (*_probe_flash_ptr)(u32 a, u32 bw, int il);
+	
+	size = (unsigned)&_probe_flash_end - (unsigned)_probe_flash; 
+	
+	if (size > PROBE_BUFFER_SIZE) {
+		printf("_probe_flash() routine too large (%d) %p - %p\n",
+		       size, &_probe_flash_end, _probe_flash);
+		return 0;
+	}
+	
+	memcpy(buffer, _probe_flash, size);
+	_probe_flash_ptr = (void*)buffer;
+	
+	is = disable_interrupts();
+	res = _probe_flash_ptr(address, width, 1);
+	if (is) {
+		enable_interrupts();
+	}
+	
+	
+        vendor = res >> 16;
+	device = res & 0xffff;
+	
+		
+	return res;
+}
+
+ulong flash_init(void)
+{
+	int i, j;
+	ulong size = 0;
+	
+	for (i = 0; i < SC520_MAX_FLASH_BANKS; i++) {
+		unsigned id;
+		ulong flashbase = 0;
+		int sectsize = 0; 
+		
+		memset(flash_info[i].protect, 0, CFG_MAX_FLASH_SECT);
+		switch (i) {
+		case 0:
+			flashbase = SC520_FLASH_BANK0_BASE;
+			break;
+		case 1:
+			flashbase = SC520_FLASH_BANK1_BASE;
+			break;
+		case 2:
+			flashbase = SC520_FLASH_BANK2_BASE;
+			break;
+		default:
+			panic("configured to many flash banks!\n");
+		}
+		
+		id = identify_flash(flashbase, 4);
+		switch (id & 0x00ff00ff) {
+		case 0x000100c8:
+			/* 29LV016B/29LV017B */
+			flash_info[i].flash_id =
+				(AMD_MANUFACT & FLASH_VENDMASK) |
+				(AMD_ID_LV016B & FLASH_TYPEMASK);
+			
+			flash_info[i].size = AMD29LV016B_SIZE*4;
+			flash_info[i].sector_count = AMD29LV016B_SECTORS;
+			sectsize = (AMD29LV016B_SIZE*4)/AMD29LV016B_SECTORS;
+			printf("Bank %d: 4 x AMD 29LV017B\n", i);
+			break;
+			
+			
+		default:
+			printf("Bank %d have unknown flash %08x\n", i, id);
+			flash_info[i].flash_id = FLASH_UNKNOWN;
+			continue;
+		}
+		
+		for (j = 0; j < flash_info[i].sector_count; j++) {
+			flash_info[i].start[j] = flashbase + j * sectsize;
 		}
-	    }
-	    else
-	    {
-		flash_info[i].start[j] = flashbase + (j - 3)*MAIN_SECT_SIZE;
-	    }
+		size += flash_info[i].size;
+		
+		flash_protect(FLAG_PROTECT_CLEAR,
+			      flash_info[i].start[0],
+			       flash_info[i].start[0] + flash_info[i].size - 1,
+			      &flash_info[i]);
 	}
-	size += flash_info[i].size;
-    }
-
-    /*
-     * Protect monitor and environment sectors
-     */
-    flash_protect(FLAG_PROTECT_SET,
-		  i386boot_start-CFG_FLASH_BASE,
-		  i386boot_end-CFG_FLASH_BASE,
-		  &flash_info[0]);
-
-    flash_protect(FLAG_PROTECT_SET,
-		  CFG_ENV_ADDR,
-		  CFG_ENV_ADDR + CFG_ENV_SIZE - 1,
-		  &flash_info[0]);
-    return size;
+	
+	/*
+	 * Protect monitor and environment sectors
+	 */
+	flash_protect(FLAG_PROTECT_SET,
+		      i386boot_start,
+		      i386boot_end,
+		      &flash_info[0]);
+#ifdef CFG_ENV_ADDR
+	flash_protect(FLAG_PROTECT_SET,
+		      CFG_ENV_ADDR,
+		      CFG_ENV_ADDR + CFG_ENV_SIZE - 1,
+		      &flash_info[0]);
+#endif	
+	return size;
 }
 
 /*-----------------------------------------------------------------------
  */
-void flash_print_info  (flash_info_t *info)
+void flash_print_info(flash_info_t *info)
 {
-    int i;
-
-    switch (info->flash_id & FLASH_VENDMASK)
-    {
-    case (AMD_MANUFACT & FLASH_VENDMASK):
-	printf("AMD: ");
-	break;
-    default:
-	printf("Unknown Vendor ");
-	break;
-    }
-
-    switch (info->flash_id & FLASH_TYPEMASK)
-    {
-    case (AMD_ID_LV160B & FLASH_TYPEMASK):
-	printf("2x Amd29F160BB (16Mbit)\n");
-	break;
-    default:
-	printf("Unknown Chip Type\n");
-	goto Done;
-	break;
-    }
-
-    printf("  Size: %ld MB in %d Sectors\n",
-	   info->size >> 20, info->sector_count);
-
-    printf("  Sector Start Addresses:");
-    for (i = 0; i < info->sector_count; i++)
-    {
-	if ((i % 5) == 0)
-	{
-	    printf ("\n   ");
+	int i;
+	
+	switch (info->flash_id & FLASH_VENDMASK) {
+		
+	case (AMD_MANUFACT & FLASH_VENDMASK):
+		printf("AMD:   ");
+		switch (info->flash_id & FLASH_TYPEMASK) {
+		case (AMD_ID_LV016B & FLASH_TYPEMASK):
+			printf("4x AMD29LV017B (4x16Mbit)\n");
+			break;
+		default:
+			printf("Unknown Chip Type\n");
+			goto done;
+			break;
+		}
+		
+		break;
+	default:
+		printf("Unknown Vendor ");
+		break;
 	}
-	printf (" %08lX%s", info->start[i],
-		info->protect[i] ? " (RO)" : "     ");
-    }
-    printf ("\n");
-
-Done:
+	
+	
+	printf("  Size: %ld MB in %d Sectors\n",
+	       info->size >> 20, info->sector_count);
+	
+	printf("  Sector Start Addresses:");
+	for (i = 0; i < info->sector_count; i++) {
+		if ((i % 5) == 0) {
+			printf ("\n   ");
+		}
+		printf (" %08lX%s", info->start[i],
+			info->protect[i] ? " (RO)" : "     ");
+	}
+	printf ("\n");
+	
+	done:
 }
 
 /*-----------------------------------------------------------------------
  */
 
-int	flash_erase (flash_info_t *info, int s_first, int s_last)
+/* this needs to be inlined, the SWTMRMMILLI register is reset by each read */
+#define __udelay(delay) \
+{	\
+	unsigned micro; \
+	unsigned milli=0; \
+	\
+	micro = *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI); \
+         \
+	for (;;) { \
+		\
+		milli += *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI); \
+		micro = *(volatile u16*)(0xfffef000+SC520_SWTMRMICRO); \
+		\
+		if ((delay) <= (micro + (milli * 1000))) { \
+			break; \
+		} \
+	} \
+} while (0) 
+
+static u32 _amd_erase_flash(u32 addr, u32 sector)
 {
-    ulong result;
-    int iflag, prot, sect;
-    int rc = ERR_OK;
-    int chip1, chip2;
-
-    /* first look for protection bits */
-
-    if (info->flash_id == FLASH_UNKNOWN)
-	return ERR_UNKNOWN_FLASH_TYPE;
-
-    if ((s_first < 0) || (s_first > s_last)) {
-	return ERR_INVAL;
-    }
+	unsigned elapsed;
+	
+	/* Issue erase */
+	*(volatile u32*)(addr + 0x5554) = 0xAAAAAAAA;
+	*(volatile u32*)(addr + 0xaaa8) = 0x55555555;
+	*(volatile u32*)(addr + 0x5554) = 0x80808080;
+	/* And one unlock */
+	*(volatile u32*)(addr + 0x5554) = 0xAAAAAAAA;
+	*(volatile u32*)(addr + 0xaaa8) = 0x55555555;
+	/* Sector erase command comes last */
+	*(volatile u32*)(addr + sector) = 0x30303030;
+	
+	elapsed = *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI); /* dummy read */
+	elapsed = 0;
+	__udelay(50);
+	while (((*(volatile u32*)(addr + sector)) & 0x80808080) != 0x80808080) {
+		
+		elapsed += *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI);
+		if (elapsed > ((CFG_FLASH_ERASE_TOUT/CFG_HZ) * 1000)) {
+			*(volatile u32*)(addr) = 0xf0f0f0f0;
+			return 1;			
+		}
+	}
+	
+	*(volatile u32*)(addr) = 0xf0f0f0f0;
+	
+	return 0;
+}
 
-    if ((info->flash_id & FLASH_VENDMASK) !=
-	(AMD_MANUFACT & FLASH_VENDMASK)) {
-	return ERR_UNKNOWN_FLASH_VENDOR;
-    }
+extern int _amd_erase_flash_end;
+asm ("_amd_erase_flash_end:\n"
+     ".long 0\n");
 
-    prot = 0;
-    for (sect=s_first; sect<=s_last; ++sect) {
-	if (info->protect[sect]) {
-	    prot++;
+int flash_erase(flash_info_t *info, int s_first, int s_last)
+{
+	u32 (*_erase_flash_ptr)(u32 a, u32 so);
+	int prot;
+	int sect;
+	unsigned size;
+	
+	if ((s_first < 0) || (s_first > s_last)) {
+		if (info->flash_id == FLASH_UNKNOWN) {
+			printf("- missing\n");
+		} else {
+			printf("- no sectors to erase\n");
+		}
+		return 1;
 	}
-    }
-    if (prot)
-	return ERR_PROTECTED;
-
-    /*
-     * Disable interrupts which might cause a timeout
-     * here. Remember that our exception vectors are
-     * at address 0 in the flash, and we don't want a
-     * (ticker) exception to happen while the flash
-     * chip is in programming mode.
-     */
-    iflag = disable_interrupts();
-
-    /* Start erase on unprotected sectors */
-    for (sect = s_first; sect<=s_last && !ctrlc(); sect++)
-    {
-	printf("Erasing sector %2d ... ", sect);
-
-	/* arm simple, non interrupt dependent timer */
-	reset_timer();
-
-	if (info->protect[sect] == 0)
-	{	/* not protected */
-	    vu_long *addr = (vu_long *)(info->start[sect]);
-
-	    MEM_FLASH_ADDR1 = CMD_UNLOCK1;
-	    MEM_FLASH_ADDR2 = CMD_UNLOCK2;
-	    MEM_FLASH_ADDR1 = CMD_ERASE_SETUP;
-
-	    MEM_FLASH_ADDR1 = CMD_UNLOCK1;
-	    MEM_FLASH_ADDR2 = CMD_UNLOCK2;
-	    *addr = CMD_ERASE_CONFIRM;
-
-	    /* wait until flash is ready */
-	    chip1 = chip2 = 0;
-
-	    do
-	    {
-		result = *addr;
-
-		/* check timeout */
-		if (get_timer(0) > CFG_FLASH_ERASE_TOUT)
-		{
-		    MEM_FLASH_ADDR1 = CMD_READ_ARRAY;
-		    chip1 = TMO;
-		    break;
+	
+	if ((info->flash_id & FLASH_VENDMASK) == (AMD_MANUFACT & FLASH_VENDMASK)) {
+		size = (unsigned)&_amd_erase_flash_end - (unsigned)_amd_erase_flash; 
+		
+		if (size > PROBE_BUFFER_SIZE) {
+			printf("_amd_erase_flash() routine too large (%d) %p - %p\n",
+			       size, &_amd_erase_flash_end, _amd_erase_flash);
+			return 0;
 		}
-
-		if (!chip1 && (result & 0xFFFF) & BIT_ERASE_DONE)
-			chip1 = READY;
-
-		if (!chip1 && (result & 0xFFFF) & BIT_PROGRAM_ERROR)
-			chip1 = ERR;
-
-		if (!chip2 && (result >> 16) & BIT_ERASE_DONE)
-			chip2 = READY;
-
-		if (!chip2 && (result >> 16) & BIT_PROGRAM_ERROR)
-			chip2 = ERR;
-
-	    }  while (!chip1 || !chip2);
-
-	    MEM_FLASH_ADDR1 = CMD_READ_ARRAY;
-
-	    if (chip1 == ERR || chip2 == ERR)
-	    {
-		rc = ERR_PROG_ERROR;
-		goto outahere;
-	    }
-	    if (chip1 == TMO)
-	    {
-		rc = ERR_TIMOUT;
-		goto outahere;
-	    }
-
-	    printf("ok.\n");
+		
+		memcpy(buffer, _amd_erase_flash, size);
+		_erase_flash_ptr = (void*)buffer;
+	
+	}  else {
+		printf ("Can't erase unknown flash type - aborted\n");
+		return 1;
 	}
-	else /* it was protected */
-	{
-	    printf("protected!\n");
+	
+	prot = 0;
+	for (sect=s_first; sect<=s_last; ++sect) {
+		if (info->protect[sect]) {
+			prot++;
+		}
+	}
+	
+	if (prot) {
+		printf ("- Warning: %d protected sectors will not be erased!\n", prot);
+	} else {
+		printf ("\n");
+	}
+		
+	
+	/* Start erase on unprotected sectors */
+	for (sect = s_first; sect<=s_last; sect++) {
+		
+		if (info->protect[sect] == 0) { /* not protected */
+			int res;
+			int flag;
+			
+			/* Disable interrupts which might cause a timeout here */
+			flag = disable_interrupts();
+			
+			res = _erase_flash_ptr(info->start[0], info->start[sect]-info->start[0]);
+			
+			/* re-enable interrupts if necessary */
+			if (flag) {
+				enable_interrupts();
+			}
+			
+			
+			if (res) {
+				printf("Erase timed out, sector %d\n", sect);
+				return res;
+			}
+			
+			putc('.');			
+		}		
 	}
-    }
-
-    if (ctrlc())
-      printf("User Interrupt!\n");
-
-outahere:
-    /* allow flash to settle - wait 10 ms */
-    udelay(10000);
-
-    if (iflag)
-      enable_interrupts();
-
 
-    return rc;
+	
+	return 0;
 }
 
 /*-----------------------------------------------------------------------
- * Copy memory to flash
+ * Write a word to Flash, returns:
+ * 0 - OK
+ * 1 - write timeout
+ * 2 - Flash not erased
  */
-
-volatile static int write_word (flash_info_t *info, ulong dest, ulong data)
+static int _amd_write_word(unsigned start, unsigned dest, unsigned data)
 {
-    vu_long *addr = (vu_long *)dest;
-    ulong result;
-    int rc = ERR_OK;
-    int iflag;
-    int chip1, chip2;
-
-    /*
-     * Check if Flash is (sufficiently) erased
-     */
-    result = *addr;
-    if ((result & data) != data)
-        return ERR_NOT_ERASED;
-
-
-    /*
-     * Disable interrupts which might cause a timeout
-     * here. Remember that our exception vectors are
-     * at address 0 in the flash, and we don't want a
-     * (ticker) exception to happen while the flash
-     * chip is in programming mode.
-     */
-    iflag = disable_interrupts();
-
-    MEM_FLASH_ADDR1 = CMD_UNLOCK1;
-    MEM_FLASH_ADDR2 = CMD_UNLOCK2;
-    MEM_FLASH_ADDR1 = CMD_UNLOCK_BYPASS;
-    *addr = CMD_PROGRAM;
-    *addr = data;
-
-    /* arm simple, non interrupt dependent timer */
-    reset_timer();
-
-    /* wait until flash is ready */
-    chip1 = chip2 = 0;
-    do
-    {
-	result = *addr;
-
-	/* check timeout */
-	if (get_timer(0) > CFG_FLASH_ERASE_TOUT)
-	{
-	    chip1 = ERR | TMO;
-	    break;
+	volatile u32 *addr2 = (u32*)start;
+	volatile u32 *dest2 = (u32*)dest;
+	volatile u32 *data2 = (u32*)&data;
+	unsigned elapsed;
+	
+	/* Check if Flash is (sufficiently) erased */
+	if ((*((volatile u32*)dest) & (u32)data) != (u32)data) {
+		return 2;
 	}
-	if (!chip1 && ((result & 0x80) == (data & 0x80)))
-		chip1 = READY;
-
-	if (!chip1 && ((result & 0xFFFF) & BIT_PROGRAM_ERROR))
-	{
-		result = *addr;
-
-		if ((result & 0x80) == (data & 0x80))
-			chip1 = READY;
-		else
-			chip1 = ERR;
-	}
-
-	if (!chip2 && ((result & (0x80 << 16)) == (data & (0x80 << 16))))
-		chip2 = READY;
-
-	if (!chip2 && ((result >> 16) & BIT_PROGRAM_ERROR))
-	{
-		result = *addr;
-
-		if ((result & (0x80 << 16)) == (data & (0x80 << 16)))
-			chip2 = READY;
-		else
-			chip2 = ERR;
+		
+	addr2[0x5554] = 0xAAAAAAAA;
+	addr2[0xaaa8] = 0x55555555;
+	addr2[0x5554] = 0xA0A0A0A0;
+	
+	dest2[0] = data;
+	
+	elapsed = *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI); /* dummy read */
+	elapsed = 0;
+	
+	/* data polling for D7 */
+	while ((dest2[0] & 0x80808080) != (data2[0] & 0x80808080)) {
+		elapsed += *(volatile u16*)(0xfffef000+SC520_SWTMRMILLI);
+		if (elapsed > ((CFG_FLASH_WRITE_TOUT/CFG_HZ) * 1000)) {
+			addr2[0] = 0xf0f0f0f0;
+			return 1;			
+		}
 	}
 
-    }  while (!chip1 || !chip2);
-
-    *addr = CMD_READ_ARRAY;
+	
+	addr2[0] = 0xf0f0f0f0;
+	
+	return 0;
+}
 
-    if (chip1 == ERR || chip2 == ERR || *addr != data)
-        rc = ERR_PROG_ERROR;
+extern int _amd_write_word_end;
+asm ("_amd_write_word_end:\n"
+     ".long 0\n");
 
-    if (iflag)
-      enable_interrupts();
 
 
-    return rc;
-}
 
 /*-----------------------------------------------------------------------
- * Copy memory to flash.
+ * Copy memory to flash, returns:
+ * 0 - OK
+ * 1 - write timeout
+ * 2 - Flash not erased
+ * 3 - Unsupported flash type
  */
 
-int write_buff (flash_info_t *info, uchar *src, ulong addr, ulong cnt)
+int write_buff(flash_info_t *info, uchar *src, ulong addr, ulong cnt)
 {
-    ulong cp, wp, data;
-    int l;
-    int i, rc;
+	ulong cp, wp, data;
+	int i, l, rc;
+	int flag;
+	u32 (*_write_word_ptr)(unsigned start, unsigned dest, unsigned data);
+	unsigned size;
+	
+	if ((info->flash_id & FLASH_VENDMASK) == (AMD_MANUFACT & FLASH_VENDMASK)) {
+		size = (unsigned)&_amd_write_word_end - (unsigned)_amd_write_word; 
+		
+		if (size > PROBE_BUFFER_SIZE) {
+			printf("_amd_write_word() routine too large (%d) %p - %p\n",
+			       size, &_amd_write_word_end, _amd_write_word);
+			return 0;
+		}
+		
+		memcpy(buffer, _amd_write_word, size);
+		_write_word_ptr = (void*)buffer;
+	
+	} else {
+		printf ("Can't program unknown flash type - aborted\n");
+		return 3;
+	}
 
-    wp = (addr & ~3);	/* get lower word aligned address */
 
-    /*
-     * handle unaligned start bytes
-     */
-    if ((l = addr - wp) != 0) {
-	data = 0;
-	for (i=0, cp=wp; i<l; ++i, ++cp) {
-	    data = (data >> 8) | (*(uchar *)cp << 24);
+	wp = (addr & ~3);	/* get lower word aligned address */
+	
+
+	/*
+	 * handle unaligned start bytes
+	 */
+	if ((l = addr - wp) != 0) {
+		data = 0;
+		for (i=0, cp=wp; i<l; ++i, ++cp) {
+			data |= (*(uchar *)cp) << (8*i);
+		}
+		for (; i<4 && cnt>0; ++i) {
+			data |= *src++ << (8*i);
+			--cnt;
+			++cp;
+		}
+		for (; cnt==0 && i<4; ++i, ++cp) {
+			data |= (*(uchar *)cp)  << (8*i);
+		}
+		
+		/* Disable interrupts which might cause a timeout here */
+		flag = disable_interrupts();
+		
+		rc = _write_word_ptr(info->start[0], wp, data);
+		
+		/* re-enable interrupts if necessary */
+		if (flag) {
+			enable_interrupts();
+		}
+		if (rc != 0) {
+			return rc;
+		}
+		wp += 4;
 	}
-	for (; i<4 && cnt>0; ++i) {
-	    data = (data >> 8) | (*src++ << 24);
-	    --cnt;
-	    ++cp;
+	
+	/*
+	 * handle word aligned part
+	 */
+	while (cnt >= 4) {
+		data = 0;
+			       
+		for (i=0; i<4; ++i) {
+			data |= *src++ << (8*i);
+		}
+		
+		/* Disable interrupts which might cause a timeout here */
+		flag = disable_interrupts();
+
+		rc = _write_word_ptr(info->start[0], wp, data);
+		
+		/* re-enable interrupts if necessary */
+		if (flag) {
+			enable_interrupts();
+		}
+		if (rc != 0) {
+			return rc;
+		}
+		wp  += 4;
+		cnt -= 4;
 	}
-	for (; cnt==0 && i<4; ++i, ++cp) {
-	    data = (data >> 8) | (*(uchar *)cp << 24);
+	
+	if (cnt == 0) {
+		return 0;
 	}
-
-	if ((rc = write_word(info, wp, data)) != 0) {
-	    return (rc);
+	
+	/*
+	 * handle unaligned tail bytes
+	 */
+	data = 0;
+	for (i=0, cp=wp; i<4 && cnt>0; ++i, ++cp) {
+		data |= *src++ << (8*i);
+		--cnt;
 	}
-	wp += 4;
-    }
-
-    /*
-     * handle word aligned part
-     */
-    while (cnt >= 4) {
-	data = *((vu_long*)src);
-	if ((rc = write_word(info, wp, data)) != 0) {
-	    return (rc);
+	
+	for (; i<4; ++i, ++cp) {
+		data |= (*(uchar *)cp) << (8*i);
 	}
-	src += 4;
-	wp  += 4;
-	cnt -= 4;
-    }
-
-    if (cnt == 0) {
-	return ERR_OK;
-    }
-
-    /*
-     * handle unaligned tail bytes
-     */
-    data = 0;
-    for (i=0, cp=wp; i<4 && cnt>0; ++i, ++cp) {
-	data = (data >> 8) | (*src++ << 24);
-	--cnt;
-    }
-    for (; i<4; ++i, ++cp) {
-	data = (data >> 8) | (*(uchar *)cp << 24);
-    }
-
-    return write_word(info, wp, data);
+
+	/* Disable interrupts which might cause a timeout here */
+	flag = disable_interrupts();
+
+	rc = _write_word_ptr(info->start[0], wp, data);
+	
+	/* re-enable interrupts if necessary */
+	if (flag) {
+		enable_interrupts();
+	}
+	
+	return rc;
+	
 }
+
+
diff --git a/board/sc520_cdp/flash_old.c b/board/sc520_cdp/flash_old.c
new file mode 100644
index 00000000000..d4204a8ced9
--- /dev/null
+++ b/board/sc520_cdp/flash_old.c
@@ -0,0 +1,458 @@
+/*
+ * (C) Copyright 2002
+ * Daniel Engström, Omicron Ceti AB, daniel@omicron.se
+ * 
+ * (C) Copyright 2002
+ * Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Alex Zuepke <azu@sysgo.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+
+ulong myflush(void);
+
+
+#define SC520_MAX_FLASH_BANKS  3
+#define SC520_FLASH_BANK0_BASE 0x38000000  /* BOOTCS */
+#define SC520_FLASH_BANK1_BASE 0x30000000  /* ROMCS0 */
+#define SC520_FLASH_BANK2_BASE 0x28000000  /* ROMCS1 */
+#define SC520_FLASH_BANKSIZE   0x8000000
+
+#define AMD29LV016_SIZE        0x200000
+#define AMD29LV016_SECTORS     32
+
+flash_info_t    flash_info[SC520_MAX_FLASH_BANKS];
+
+#define CMD_READ_ARRAY		0x00F000F0
+#define CMD_UNLOCK1		0x00AA00AA
+#define CMD_UNLOCK2		0x00550055
+#define CMD_ERASE_SETUP		0x00800080
+#define CMD_ERASE_CONFIRM	0x00300030
+#define CMD_PROGRAM		0x00A000A0
+#define CMD_UNLOCK_BYPASS	0x00200020
+
+
+#define BIT_ERASE_DONE		0x00800080
+#define BIT_RDY_MASK		0x00800080
+#define BIT_PROGRAM_ERROR	0x00200020
+#define BIT_TIMEOUT		0x80000000 /* our flag */
+
+#define READY 1
+#define ERR   2
+#define TMO   4
+
+/*-----------------------------------------------------------------------
+ */
+
+ulong flash_init(void)
+{
+	int i, j;
+	ulong size = 0;
+	
+	for (i = 0; i < SC520_MAX_FLASH_BANKS; i++) {
+		ulong flashbase = 0;
+		int sectsize = 0; 
+		if (i==0 || i==2) {
+			/* FixMe: this assumes that bank 0 and 2 
+			 * are mapped to the two 8Mb banks */
+			flash_info[i].flash_id =
+				(AMD_MANUFACT & FLASH_VENDMASK) |
+				(AMD_ID_LV016B & FLASH_TYPEMASK);
+			
+			flash_info[i].size = AMD29LV016_SIZE*4;
+			flash_info[i].sector_count = AMD29LV016_SECTORS;
+			sectsize = (AMD29LV016_SIZE*4)/AMD29LV016_SECTORS;
+		} else {
+			/* FixMe: this assumes that bank1 is unmapped 
+			 * (or mapped to the same flash bank as BOOTCS) */
+			flash_info[i].flash_id = 0;
+			flash_info[i].size = 0;
+			flash_info[i].sector_count = 0;
+			sectsize=0;
+		}
+		memset(flash_info[i].protect, 0, CFG_MAX_FLASH_SECT);
+		switch (i) {
+		case 0:
+			flashbase = SC520_FLASH_BANK0_BASE;
+			break;
+		case 1:
+			flashbase = SC520_FLASH_BANK1_BASE;
+			break;
+		case 2:
+			flashbase = SC520_FLASH_BANK0_BASE;
+			break;
+		default:
+			panic("configured to many flash banks!\n");
+		}
+		
+		for (j = 0; j < flash_info[i].sector_count; j++) {
+			flash_info[i].start[j] = sectsize;
+			flash_info[i].start[j] = flashbase + j * sectsize;
+		}
+		size += flash_info[i].size;
+	}
+	
+	/*
+	 * Protect monitor and environment sectors
+	 */
+	flash_protect(FLAG_PROTECT_SET,
+		      i386boot_start-SC520_FLASH_BANK0_BASE,
+		      i386boot_end-SC520_FLASH_BANK0_BASE,
+		      &flash_info[0]);
+	
+#ifdef CFG_ENV_ADDR
+	flash_protect(FLAG_PROTECT_SET,
+		      CFG_ENV_ADDR,
+		      CFG_ENV_ADDR + CFG_ENV_SIZE - 1,
+		      &flash_info[0]);
+#endif
+	return size;
+}
+
+/*-----------------------------------------------------------------------
+ */
+void flash_print_info(flash_info_t *info)
+{
+	int i;
+
+	switch (info->flash_id & FLASH_VENDMASK) {
+	case (AMD_MANUFACT & FLASH_VENDMASK):
+		printf("AMD: ");
+		break;
+	default:
+		printf("Unknown Vendor ");
+		break;
+	}
+	
+	switch (info->flash_id & FLASH_TYPEMASK) {
+	case (AMD_ID_LV016B & FLASH_TYPEMASK):
+		printf("4x Amd29LV016B (16Mbit)\n");
+		break;
+	default:
+		printf("Unknown Chip Type\n");
+		goto done;
+		break;
+	}
+	
+	printf("  Size: %ld MB in %d Sectors\n",
+	       info->size >> 20, info->sector_count);
+	
+	printf("  Sector Start Addresses:");
+	for (i = 0; i < info->sector_count; i++) {
+		if ((i % 5) == 0) {
+			printf ("\n   ");
+		}
+		printf (" %08lX%s", info->start[i],
+			info->protect[i] ? " (RO)" : "     ");
+	}
+	printf ("\n");
+	
+	done:
+}
+
+/*-----------------------------------------------------------------------
+ */
+
+int flash_erase(flash_info_t *info, int s_first, int s_last)
+{
+	ulong result;
+	int iflag, prot, sect;
+	int rc = ERR_OK;
+	int chip1, chip2;
+
+	/* first look for protection bits */
+	
+	if (info->flash_id == FLASH_UNKNOWN) {
+		return ERR_UNKNOWN_FLASH_TYPE;
+	}
+	
+	if ((s_first < 0) || (s_first > s_last)) {
+		return ERR_INVAL;
+	}
+	
+	if ((info->flash_id & FLASH_VENDMASK) !=
+	    (AMD_MANUFACT & FLASH_VENDMASK)) {
+		return ERR_UNKNOWN_FLASH_VENDOR;
+	}
+	
+	prot = 0;
+	for (sect=s_first; sect<=s_last; ++sect) {
+		if (info->protect[sect]) {
+			prot++;
+		}
+	}
+	if (prot) {
+		return ERR_PROTECTED;
+	}
+	
+	/*
+	 * Disable interrupts which might cause a timeout
+	 * here. Remember that our exception vectors are
+	 * at address 0 in the flash, and we don't want a
+	 * (ticker) exception to happen while the flash
+	 * chip is in programming mode.
+	 */
+	iflag = disable_interrupts();
+	
+	/* Start erase on unprotected sectors */
+	for (sect = s_first; sect<=s_last && !ctrlc(); sect++) {
+		printf("Erasing sector %2d ... ", sect);
+		
+		/* arm simple, non interrupt dependent timer */
+		reset_timer();
+		
+		if (info->protect[sect] == 0) {	
+			/* not protected */
+			ulong addr = info->start[sect];
+			
+			writel(CMD_UNLOCK1, addr + 1);
+			writel(CMD_UNLOCK2, addr + 2);
+			writel(CMD_ERASE_SETUP, addr + 1);
+			
+			writel(CMD_UNLOCK1, addr + 1);
+			writel(CMD_UNLOCK2, addr + 2);
+			writel(CMD_ERASE_CONFIRM, addr);
+
+			
+			/* wait until flash is ready */
+			chip1 = chip2 = 0;
+			
+			do {
+				result = readl(addr);
+				
+				/* check timeout */
+				if (get_timer(0) > CFG_FLASH_ERASE_TOUT) {
+					writel(CMD_READ_ARRAY, addr + 1);
+					chip1 = TMO;
+					break;
+				}
+				
+				if (!chip1 && (result & 0xFFFF) & BIT_ERASE_DONE) {
+					chip1 = READY;
+				}
+				
+				if (!chip1 && (result & 0xFFFF) & BIT_PROGRAM_ERROR) {
+					chip1 = ERR;
+				}
+				
+				if (!chip2 && (result >> 16) & BIT_ERASE_DONE) {
+					chip2 = READY;
+				}
+				
+				if (!chip2 && (result >> 16) & BIT_PROGRAM_ERROR) {
+					chip2 = ERR;
+				}
+				
+			}  while (!chip1 || !chip2);
+			
+			writel(CMD_READ_ARRAY, addr + 1);
+			
+			if (chip1 == ERR || chip2 == ERR) {
+				rc = ERR_PROG_ERROR;
+				goto outahere;
+			}
+			
+			if (chip1 == TMO) {
+				rc = ERR_TIMOUT;
+				goto outahere;
+			}
+			
+			printf("ok.\n");
+		} else { /* it was protected */
+			
+			printf("protected!\n");
+		}
+	}
+	
+	if (ctrlc()) {
+		printf("User Interrupt!\n");
+	}
+
+outahere:
+	/* allow flash to settle - wait 10 ms */
+	udelay(10000);
+	
+	if (iflag) {
+		enable_interrupts();
+	}
+	
+	return rc;
+}
+
+/*-----------------------------------------------------------------------
+ * Copy memory to flash
+ */
+
+volatile static int write_word(flash_info_t *info, ulong dest, ulong data)
+{
+	ulong addr = dest;
+	ulong result;
+	int rc = ERR_OK;
+	int iflag;
+	int chip1, chip2;
+	
+	/*
+	 * Check if Flash is (sufficiently) erased
+	 */
+	result = readl(addr);
+	if ((result & data) != data) {
+		return ERR_NOT_ERASED;
+	}
+	
+	/*
+	 * Disable interrupts which might cause a timeout
+	 * here. Remember that our exception vectors are
+	 * at address 0 in the flash, and we don't want a
+	 * (ticker) exception to happen while the flash
+	 * chip is in programming mode.
+	 */
+	iflag = disable_interrupts();
+
+	writel(CMD_UNLOCK1, addr + 1);
+	writel(CMD_UNLOCK2, addr + 2);
+	writel(CMD_UNLOCK_BYPASS, addr + 1);
+	writel(addr, CMD_PROGRAM);
+	writel(addr, data);
+	
+	/* arm simple, non interrupt dependent timer */
+	reset_timer();
+	
+	/* wait until flash is ready */
+	chip1 = chip2 = 0;
+	do {
+		result = readl(addr);
+		
+		/* check timeout */
+		if (get_timer(0) > CFG_FLASH_ERASE_TOUT) {
+			chip1 = ERR | TMO;
+			break;
+		}
+		
+		if (!chip1 && ((result & 0x80) == (data & 0x80))) {
+			chip1 = READY;
+		}
+		
+		if (!chip1 && ((result & 0xFFFF) & BIT_PROGRAM_ERROR)) {
+			result = readl(addr);
+			
+			if ((result & 0x80) == (data & 0x80)) {
+				chip1 = READY;
+			} else {
+				chip1 = ERR;
+			}
+		}
+		
+		if (!chip2 && ((result & (0x80 << 16)) == (data & (0x80 << 16)))) {
+			chip2 = READY;
+		}
+		
+		if (!chip2 && ((result >> 16) & BIT_PROGRAM_ERROR)) {
+			result = readl(addr);
+			
+			if ((result & (0x80 << 16)) == (data & (0x80 << 16))) {
+				chip2 = READY;
+			} else {
+				chip2 = ERR;
+			}
+		}
+		
+	}  while (!chip1 || !chip2);
+	
+	writel(CMD_READ_ARRAY, addr);
+	
+	if (chip1 == ERR || chip2 == ERR || readl(addr) != data) {
+		rc = ERR_PROG_ERROR;
+	}
+	
+	if (iflag) {
+		enable_interrupts();
+	}
+	
+	return rc;
+}
+
+/*-----------------------------------------------------------------------
+ * Copy memory to flash.
+ */
+
+int write_buff(flash_info_t *info, uchar *src, ulong addr, ulong cnt)
+{
+	ulong cp, wp, data;
+	int l;
+	int i, rc;
+	
+	wp = (addr & ~3);	/* get lower word aligned address */
+	
+	/*
+	 * handle unaligned start bytes
+	 */
+	if ((l = addr - wp) != 0) {
+		data = 0;
+		for (i=0, cp=wp; i<l; ++i, ++cp) {
+			data = (data >> 8) | (*(uchar *)cp << 24);
+		}
+		for (; i<4 && cnt>0; ++i) {
+			data = (data >> 8) | (*src++ << 24);
+			--cnt;
+			++cp;
+		}
+		for (; cnt==0 && i<4; ++i, ++cp) {
+			data = (data >> 8) | (*(uchar *)cp << 24);
+		}
+		
+		if ((rc = write_word(info, wp, data)) != 0) {
+			return rc;
+		}
+		wp += 4;
+	}
+	
+	/*
+	 * handle word aligned part
+	 */
+	while (cnt >= 4) {
+		data = *((vu_long*)src);
+		if ((rc = write_word(info, wp, data)) != 0) {
+			return rc;
+		}
+		src += 4;
+		wp  += 4;
+		cnt -= 4;
+	}
+	
+	if (cnt == 0) {
+		return ERR_OK;
+	}
+	
+	/*
+	 * handle unaligned tail bytes
+	 */
+	data = 0;
+	for (i=0, cp=wp; i<4 && cnt>0; ++i, ++cp) {
+		data = (data >> 8) | (*src++ << 24);
+		--cnt;
+	}
+	for (; i<4; ++i, ++cp) {
+		data = (data >> 8) | (*(uchar *)cp << 24);
+	}
+	
+	return write_word(info, wp, data);
+}
diff --git a/board/sc520_cdp/sc520_cdp.c b/board/sc520_cdp/sc520_cdp.c
index 2809ff72af1..50a49e601a5 100644
--- a/board/sc520_cdp/sc520_cdp.c
+++ b/board/sc520_cdp/sc520_cdp.c
@@ -1,4 +1,5 @@
 /*
+ * 
  * (C) Copyright 2002
  * Daniel Engström, Omicron Ceti AB <daniel@omicron.se>.
  *
@@ -24,109 +25,162 @@
 #include <common.h>
 #include <pci.h>
 #include <asm/io.h>
+#include <asm/pci.h>
 #include <asm/ic/sc520.h>
 #include <asm/ic/ali512x.h>
+#include <ssi.h>
+
+#undef SC520_CDP_DEBUG
+
+#ifdef	SC520_CDP_DEBUG
+#define	PRINTF(fmt,args...)	printf (fmt ,##args)
+#else
+#define PRINTF(fmt,args...)
+#endif
 
 /* ------------------------------------------------------------------------- */
 
+	
+/* 
+ * Theory:
+ * We first set up all IRQs to be non-pci, edge triggered,
+ * when we later enumerate the pci bus and pci_sc520_fixup_irq() gets 
+ * called we reallocate irqs to the pci bus with sc520_pci_set_irq()
+ * as needed. Whe choose the irqs to gram from a configurable list
+ * inside pci_sc520_fixup_irq() (If this list contains stupid irq's
+ * such as 0 thngas will not work)
+ */
+
 static void irq_init(void)
 {
-	
 	/* disable global interrupt mode */
 	write_mmcr_byte(SC520_PICICR, 0x40); 
 	
-	/* set irq0-7 to edge */
+	/* set all irqs to edge */
 	write_mmcr_byte(SC520_MPICMODE, 0x00);
-	
-	/* set irq9-12 to level, all the other (8, 13-15) are edge */
-	write_mmcr_byte(SC520_SL1PICMODE, 0x1e);
-	
-	/* set irq16-24 (unused slave pic2) to level */
-	write_mmcr_byte(SC520_SL2PICMODE, 0xff);
+	write_mmcr_byte(SC520_SL1PICMODE, 0x00);
+	write_mmcr_byte(SC520_SL2PICMODE, 0x00);
 	
 	/* active low polarity on PIC interrupt pins, 
-	   active high polarity on all other irq pins */
-	write_mmcr_word(SC520_INTPINPOL, 0);
+	 *  active high polarity on all other irq pins */
+	write_mmcr_word(SC520_INTPINPOL, 0x0000);
 
 	/* set irq number mapping */
-	write_mmcr_byte(SC520_GPTMR0MAP,0);            /* disable GP timer 0 INT */       
-	write_mmcr_byte(SC520_GPTMR1MAP,0);            /* disable GP timer 1 INT */
-	write_mmcr_byte(SC520_GPTMR2MAP,0);            /* disable GP timer 2 INT */
-	write_mmcr_byte(SC520_PIT0MAP,0x1);            /* Set PIT timer 0 INT to IRQ0 */ 
-	write_mmcr_byte(SC520_PIT1MAP,0);              /* diable PIT timer 1 INT */
-	write_mmcr_byte(SC520_PIT2MAP,0);              /* diable PIT timer 2 INT */
-	write_mmcr_byte(SC520_PCIINTAMAP,0x4);         /* Set PCI INT A to IRQ9 */
-	write_mmcr_byte(SC520_PCIINTBMAP,0x5);         /* Set PCI INT B to IRQ10 */
-	write_mmcr_byte(SC520_PCIINTCMAP,0x6);         /* Set PCI INT C to IRQ11 */
-	write_mmcr_byte(SC520_PCIINTDMAP,0x7);         /* Set PCI INT D to IRQ12 */
-	write_mmcr_byte(SC520_DMABCINTMAP,0);          /* disable DMA INT */ 
-	write_mmcr_byte(SC520_SSIMAP,0);               /* disable Synchronius serial INT */
-	write_mmcr_byte(SC520_WDTMAP,0);               /* disable Watchdor INT */
-	write_mmcr_byte(SC520_RTCMAP,0x3);             /* Set RTC int to 8 */
-	write_mmcr_byte(SC520_WPVMAP,0);               /* disable write protect INT */
-	write_mmcr_byte(SC520_ICEMAP,0x2);             /* Set ICE Debug Serielport INT to IRQ1 */
-	write_mmcr_byte(SC520_FERRMAP,0x8);            /* Set FP error INT to IRQ13 */
-	write_mmcr_byte(SC520_GP0IMAP,6);              /* Set GPIRQ0 (ISA IRQ2) to IRQ9 */
-	write_mmcr_byte(SC520_GP1IMAP,2);              /* Set GPIRQ1 (SIO IRQ1) to IRQ1 */
-	write_mmcr_byte(SC520_GP2IMAP,7);              /* Set GPIRQ2 (ISA IRQ12) to IRQ12 */
+	write_mmcr_byte(SC520_GPTMR0MAP, SC520_IRQ_DISABLED);   /* disable GP timer 0 INT */       
+	write_mmcr_byte(SC520_GPTMR1MAP, SC520_IRQ_DISABLED);   /* disable GP timer 1 INT */
+	write_mmcr_byte(SC520_GPTMR2MAP, SC520_IRQ_DISABLED);   /* disable GP timer 2 INT */
+	write_mmcr_byte(SC520_PIT0MAP, SC520_IRQ0);             /* Set PIT timer 0 INT to IRQ0 */ 
+	write_mmcr_byte(SC520_PIT1MAP, SC520_IRQ_DISABLED);     /* disable PIT timer 1 INT */
+	write_mmcr_byte(SC520_PIT2MAP, SC520_IRQ_DISABLED);     /* disable PIT timer 2 INT */
+	write_mmcr_byte(SC520_PCIINTAMAP, SC520_IRQ_DISABLED);  /* disable PCI INT A */
+	write_mmcr_byte(SC520_PCIINTBMAP, SC520_IRQ_DISABLED);  /* disable PCI INT B */
+	write_mmcr_byte(SC520_PCIINTCMAP, SC520_IRQ_DISABLED);  /* disable PCI INT C */
+	write_mmcr_byte(SC520_PCIINTDMAP, SC520_IRQ_DISABLED);  /* disable PCI INT D */
+	write_mmcr_byte(SC520_DMABCINTMAP, SC520_IRQ_DISABLED); /* disable DMA INT */ 
+	write_mmcr_byte(SC520_SSIMAP, SC520_IRQ_DISABLED);      /* disable Synchronius serial INT */
+	write_mmcr_byte(SC520_WDTMAP, SC520_IRQ_DISABLED);      /* disable Watchdog INT */
+	write_mmcr_byte(SC520_RTCMAP, SC520_IRQ8);              /* Set RTC int to 8 */
+	write_mmcr_byte(SC520_WPVMAP, SC520_IRQ_DISABLED);      /* disable write protect INT */
+	write_mmcr_byte(SC520_ICEMAP, SC520_IRQ1);              /* Set ICE Debug Serielport INT to IRQ1 */
+	write_mmcr_byte(SC520_FERRMAP,SC520_IRQ13);             /* Set FP error INT to IRQ13 */
 	
 	if (CFG_USE_SIO_UART) {
-		write_mmcr_byte(SC520_UART1MAP,0);     /* disable internal UART1 INT */
-		write_mmcr_byte(SC520_UART2MAP,0);     /* disable internal UART2 INT */
-		write_mmcr_byte(SC520_GP3IMAP,11);     /* Set GPIRQ3 (ISA IRQ3) to IRQ3 */ 
-		write_mmcr_byte(SC520_GP4IMAP,12);     /* Set GPIRQ4 (ISA IRQ4) to IRQ4 */
+		write_mmcr_byte(SC520_UART1MAP, SC520_IRQ_DISABLED); /* disable internal UART1 INT */
+		write_mmcr_byte(SC520_UART2MAP, SC520_IRQ_DISABLED); /* disable internal UART2 INT */
+		write_mmcr_byte(SC520_GP3IMAP, SC520_IRQ3);          /* Set GPIRQ3 (ISA IRQ3) to IRQ3 */ 
+		write_mmcr_byte(SC520_GP4IMAP, SC520_IRQ4);          /* Set GPIRQ4 (ISA IRQ4) to IRQ4 */
 	} else {
-		write_mmcr_byte(SC520_UART1MAP,12);    /* Set internal UART2 INT to IRQ4 */
-		write_mmcr_byte(SC520_UART2MAP,11);    /* Set internal UART2 INT to IRQ3 */
-		write_mmcr_byte(SC520_GP3IMAP,0);      /* disable GPIRQ3 (ISA IRQ3) */ 
-		write_mmcr_byte(SC520_GP4IMAP,0);      /* disable GPIRQ4 (ISA IRQ4) */
+		write_mmcr_byte(SC520_UART1MAP, SC520_IRQ4);         /* Set internal UART2 INT to IRQ4 */
+		write_mmcr_byte(SC520_UART2MAP, SC520_IRQ3);         /* Set internal UART2 INT to IRQ3 */
+		write_mmcr_byte(SC520_GP3IMAP, SC520_IRQ_DISABLED);  /* disable GPIRQ3 (ISA IRQ3) */ 
+		write_mmcr_byte(SC520_GP4IMAP, SC520_IRQ_DISABLED);  /* disable GPIRQ4 (ISA IRQ4) */
 	}
 	
-	write_mmcr_byte(SC520_GP5IMAP,13);             /* Set GPIRQ5 (ISA IRQ5) to IRQ5 */
-	write_mmcr_byte(SC520_GP6IMAP,21);             /* Set GPIRQ6 (ISA IRQ6) to IRQ6 */
-	write_mmcr_byte(SC520_GP7IMAP,22);             /* Set GPIRQ7 (ISA IRQ7) to IRQ7 */
-	write_mmcr_byte(SC520_GP8IMAP,3);              /* Set GPIRQ8 (SIO IRQ8) to IRQ8 */
-	write_mmcr_byte(SC520_GP9IMAP,4);              /* Set GPIRQ9 (ISA IRQ9) to IRQ9 */
-	write_mmcr_byte(SC520_GP10IMAP,9);             /* Set GPIRQ10 (ISA IRQ10) to IRQ10 */          
-	write_mmcr_word(SC520_PCIHOSTMAP,0x11f);       /* Map PCI hostbridge INT to NMI */
-	write_mmcr_word(SC520_ECCMAP,0x100);           /* Map SDRAM ECC failure INT to NMI */
+	write_mmcr_byte(SC520_GP1IMAP, SC520_IRQ1);             /* Set GPIRQ1 (SIO IRQ1) to IRQ1 */
+	write_mmcr_byte(SC520_GP5IMAP, SC520_IRQ5);             /* Set GPIRQ5 (ISA IRQ5) to IRQ5 */
+	write_mmcr_byte(SC520_GP6IMAP, SC520_IRQ6);             /* Set GPIRQ6 (ISA IRQ6) to IRQ6 */
+	write_mmcr_byte(SC520_GP7IMAP, SC520_IRQ7);             /* Set GPIRQ7 (ISA IRQ7) to IRQ7 */
+	write_mmcr_byte(SC520_GP8IMAP, SC520_IRQ8);             /* Set GPIRQ8 (SIO IRQ8) to IRQ8 */
+	write_mmcr_byte(SC520_GP9IMAP, SC520_IRQ9);             /* Set GPIRQ9 (ISA IRQ2) to IRQ9 */
+	write_mmcr_byte(SC520_GP0IMAP, SC520_IRQ11);            /* Set GPIRQ0 (ISA IRQ11) to IRQ10 */
+	write_mmcr_byte(SC520_GP2IMAP, SC520_IRQ12);            /* Set GPIRQ2 (ISA IRQ12) to IRQ12 */
+	write_mmcr_byte(SC520_GP10IMAP,SC520_IRQ14);            /* Set GPIRQ10 (ISA IRQ14) to IRQ14 */          
+	
+	write_mmcr_word(SC520_PCIHOSTMAP, 0x11f);                /* Map PCI hostbridge INT to NMI */
+	write_mmcr_word(SC520_ECCMAP, 0x100);                    /* Map SDRAM ECC failure INT to NMI */
  
 }
 
+	
 /* PCI stuff */
 static void pci_sc520_cdp_fixup_irq(struct pci_controller *hose, pci_dev_t dev)
 {
-	char pin;
-	int irq;
+	/* a configurable lists of irqs to steal
+	 * when we need one (a board with more pci interrupt pins
+	 * would use a larger table */
+	static int irq_list[] = {
+		CFG_FIRST_PCI_IRQ,
+		CFG_SECOND_PCI_IRQ,
+		CFG_THIRD_PCI_IRQ,
+		CFG_FORTH_PCI_IRQ
+	};
+	static int next_irq_index=0;
 	
+	char tmp_pin;	
+	int pin;
 	
-	pci_hose_read_config_byte(hose, dev, PCI_INTERRUPT_PIN, &pin);
-	irq = pin-1;
+	pci_hose_read_config_byte(hose, dev, PCI_INTERRUPT_PIN, &tmp_pin);
+	pin = tmp_pin;
 	
+	pin-=1; /* pci config space use 1-based numbering */
+	if (-1 == pin) {
+		return; /* device use no irq */
+	}
+	
+	
+       
+	/* map device number +  pin to a pin on the sc520 */
 	switch (PCI_DEV(dev)) {
 	case 20:
+		pin+=SC520_PCI_INTA;
 		break;
+		
 	case 19:
-		irq+=1;
+		pin+=SC520_PCI_INTB;
 		break;
+		
 	case 18:
-		irq+=2;
+		pin+=SC520_PCI_INTC;
 		break;
+		
 	case 17:
-		irq+=3;
+		pin+=SC520_PCI_INTD;
 		break;
+		
 	default: 
 		return;
 	}
 	
-	irq&=3; /* wrap around */
-	irq+=9; /* lowest IRQ is 9 */
+	pin&=3; /* wrap around */
 	
-	pci_hose_write_config_byte(hose, dev, PCI_INTERRUPT_LINE, irq);
-#if 0	
-	printf("fixup_irq: device %d pin %c irq %d\n", 
-	       PCI_DEV(dev), 'A' + pin -1, irq);
-#endif
+	if (sc520_pci_ints[pin] == -1) {
+		/* re-route one interrupt for us */		
+		if (next_irq_index > 3) {
+			return;
+		}
+		if (pci_sc520_set_irq(pin, irq_list[next_irq_index])) { 
+			return;
+		}
+		next_irq_index++;
+	}
+
+	
+	if (-1 != sc520_pci_ints[pin]) {
+		pci_hose_write_config_byte(hose, dev, PCI_INTERRUPT_LINE, 
+					   sc520_pci_ints[pin]);
+	}
+	PRINTF("fixup_irq: device %d pin %c irq %d\n", 
+	       PCI_DEV(dev), 'A' + pin, sc520_pci_ints[pin]);
 }
  
 static struct pci_controller sc520_cdp_hose = {
@@ -162,8 +216,8 @@ void setup_ali_sio(int uart_primary)
 	
 	/* SSI chip select pins */
 	ali512x_cio_function(14, 0, 0, 0);  /* SSI_CS */
-	ali512x_cio_function(15, 0, 0, 0);  /* SSI_MV */					     
-	ali512x_cio_function(16, 0, 1, 0);  /* SSI_SPI# (inverted) */
+	ali512x_cio_function(15, 0, 0, 0);  /* SSI_MV */ 
+	ali512x_cio_function(16, 0, 0, 0);  /* SSI_SPI# */
 
 	/* Board REV pins */
 	ali512x_cio_function(20, 0, 0, 1);
@@ -202,6 +256,7 @@ static void bus_init(void)
 	 * we need to map 1G-128k - 1G (0x38000000 - 0x3fffffff) to CS1 */ 
 	
 		
+	
 	/* SRAM = GPCS3 128k @ d0000-effff*/
 	write_mmcr_long(SC520_PAR2,  0x4e00400d);		
 	
@@ -219,13 +274,15 @@ static void bus_init(void)
 	/* 680 LEDS */
 	write_mmcr_long(SC520_PAR15, 0x30000640);		
 	
+	write_mmcr_byte(SC520_ADDDECCTL, 0);
+	
 	asm ("wbinvd\n"); /* Flush cache, req. after setting the unchached attribute ona PAR */	
 
 	if (CFG_USE_SIO_UART) {
 		write_mmcr_byte(SC520_ADDDECCTL, read_mmcr_byte(SC520_ADDDECCTL) | UART2_DIS|UART1_DIS);	
 		setup_ali_sio(1);
 	} else {
-		write_mmcr_byte(SC520_ADDDECCTL, read_mmcr_byte(SC520_ADDDECCTL) & ~(UART2_DIS|UART1_DIS));	
+		write_mmcr_byte(SC520_ADDDECCTL, read_mmcr_byte(SC520_ADDDECCTL) & ~(UART2_DIS|UART1_DIS));
 		setup_ali_sio(0);
 		silence_uart(0x3e8);
 		silence_uart(0x2e8);
@@ -233,7 +290,192 @@ static void bus_init(void)
 
 }
 
+/* GPCS usage
+ * GPCS0       PIO27 (NMI)
+ * GPCS1       ROMCS1
+ * GPCS2       ROMCS2
+ * GPCS3       SRAMCS       PAR2
+ * GPCS4       unused       PAR3
+ * GPCS5       unused       PAR4
+ * GPCS6       IDE
+ * GPCS7       IDE
+ */
+
+
+/* par usage:
+ * PAR0   legacy_video
+ * PAR1   PCI ROM mapping
+ * PAR2   SRAM
+ * PAR3   IDE
+ * PAR4   IDE
+ * PAR5   legacy_video
+ * PAR6   legacy_video
+ * PAR7   legacy_video
+ * PAR8   legacy_video
+ * PAR9   legacy_video
+ * PAR10  legacy_video
+ * PAR11  ISAROM
+ * PAR12  BOOTCS
+ * PAR13  ROMCS1
+ * PAR14  ROMCS2
+ * PAR15  Port 0x680 LED display
+ */
+
+/* 
+ * This function should map a chunk of size bytes
+ * of the system address space to the ISA bus
+ * 
+ * The function will return the memory address
+ * as seen by the host (which may very will be the
+ * same as the bus address)
+ */
+u32 isa_map_rom(u32 bus_addr, int size) 
+{
+	u32 par;
+	
+	PRINTF("isa_map_rom asked to map %d bytes at %x\n", 
+	       size, bus_addr);
+	
+	par = size;
+	if (par < 0x80000) {
+		par = 0x80000;
+	}
+	par >>= 12;
+	par--;
+	par&=0x7f;
+	par <<= 18;
+	par |= (bus_addr>>12);
+	par |= 0x50000000;
+	
+	PRINTF ("setting PAR11 to %x\n", par);
+	
+	/* Map rom 0x10000 with PAR1 */
+	write_mmcr_long(SC520_PAR11,  par);
+	
+	return bus_addr;
+}
+
+/*
+ * this function removed any mapping created
+ * with pci_get_rom_window()
+ */
+void isa_unmap_rom(u32 addr)
+{
+	PRINTF("isa_unmap_rom asked to unmap %x", addr);
+	if ((addr>>12) == (read_mmcr_long(SC520_PAR11)&0x3ffff)) {
+		write_mmcr_long(SC520_PAR11, 0);
+		PRINTF(" done\n");
+		return;
+	}
+	PRINTF(" not ours\n");
+}
+
+#ifdef CONFIG_PCI
+#define PCI_ROM_TEMP_SPACE 0x10000
+/* 
+ * This function should map a chunk of size bytes
+ * of the system address space to the PCI bus,
+ * suitable to map PCI ROMS (bus address < 16M)
+ * the function will return the host memory address
+ * which should be converted into a bus address
+ * before used to configure the PCI rom address 
+ * decoder
+ */
+u32 pci_get_rom_window(struct pci_controller *hose, int size) 
+{
+	u32 par;
+	
+	par = size;
+	if (par < 0x80000) {
+		par = 0x80000;
+	}
+	par >>= 16;
+	par--;
+	par&=0x7ff;
+	par <<= 14;
+	par |= (PCI_ROM_TEMP_SPACE>>16);
+	par |= 0x72000000;
+	
+	PRINTF ("setting PAR1 to %x\n", par);
+	
+	/* Map rom 0x10000 with PAR1 */
+	write_mmcr_long(SC520_PAR1,  par);
+	
+	return PCI_ROM_TEMP_SPACE;
+}
+
+/*
+ * this function removed any mapping created
+ * with pci_get_rom_window()
+ */
+void pci_remove_rom_window(struct pci_controller *hose, u32 addr)
+{
+	PRINTF("pci_remove_rom_window: %x", addr);
+	if (addr == PCI_ROM_TEMP_SPACE) {
+		write_mmcr_long(SC520_PAR1, 0);
+		PRINTF(" done\n");
+		return;
+	}
+	PRINTF(" not ours\n");
+	
+}
+
+/*
+ * This function is called in order to provide acces to the
+ * legacy video I/O ports on the PCI bus. 
+ * After this function accesses to I/O ports 0x3b0-0x3bb and 
+ * 0x3c0-0x3df shuld result in transactions on the PCI bus.
+ * 
+ */
+int pci_enable_legacy_video_ports(struct pci_controller *hose)
+{
+	/* Map video memory to 0xa0000*/
+	write_mmcr_long(SC520_PAR0,  0x7200400a);
+	
+	/* forward all I/O accesses to PCI */
+	write_mmcr_byte(SC520_ADDDECCTL, 
+			read_mmcr_byte(SC520_ADDDECCTL) | IO_HOLE_DEST_PCI);	
+	
+	
+	/* so we map away all io ports to pci (only way to access pci io
+	 * below 0x400. But then we have to map back the portions that we dont
+	 * use so that the generate cycles on the GPIO bus where the sio and
+	 * ISA slots are connected, this requre the use of several PAR registers 
+	 */
+	
+	/* bring 0x100 - 0x1ef back to ISA using PAR5 */
+	write_mmcr_long(SC520_PAR5, 0x30ef0100);		
+	
+	/* IDE use 1f0-1f7 */
+	
+	/* bring 0x1f8 - 0x2f7 back to ISA using PAR6 */
+	write_mmcr_long(SC520_PAR6, 0x30ff01f8);		
+	
+	/* com2 use 2f8-2ff */
+	
+	/* bring 0x300 - 0x3af back to ISA using PAR7 */
+	write_mmcr_long(SC520_PAR7, 0x30af0300);		
+	
+	/* vga use 3b0-3bb */
+	
+	/* bring 0x3bc - 0x3bf back to ISA using PAR8 */
+	write_mmcr_long(SC520_PAR8, 0x300303bc);		
+	
+	/* vga use 3c0-3df */
+	
+	/* bring 0x3e0 - 0x3f5 back to ISA using PAR9 */
+	write_mmcr_long(SC520_PAR9, 0x301503e0);		
+	
+	/* ide use 3f6 */
+	
+	/* bring 0x3f7  back to ISA using PAR10 */
+	write_mmcr_long(SC520_PAR10, 0x300003f7);		
+	
+	/* com1 use 3f8-3ff */		
 
+	return 0;
+}
+#endif
 
 /*
  * Miscelaneous platform dependent initialisations
@@ -252,7 +494,6 @@ int board_init(void)
 		
 	/* enter debug mode after next reset (only if jumper is also set) */
 	write_mmcr_byte(SC520_RESCFG, 0x08);
-	
 	/* configure the software timer to 33.333MHz */
 	write_mmcr_byte(SC520_SWTMRCFG, 0);
 	gd->bus_clk = 33333000;
@@ -288,3 +529,87 @@ int last_stage_init(void)
 	
 	return 0;
 }
+
+
+
+void ssi_chip_select(int dev) 
+{
+	
+	/* Spunk board: SPI EEPROM is active-low, MW EEPROM and AUX are active high */
+	switch (dev) {
+	case 1: /* SPI EEPROM */
+		ali512x_cio_out(16, 0);
+		break;
+		
+	case 2: /* MW EEPROM */
+		ali512x_cio_out(15, 1);
+		break;
+		
+	case 3: /* AUX */				
+		ali512x_cio_out(14, 1);
+		break;
+		
+	case 0:
+		ali512x_cio_out(16, 1);
+		ali512x_cio_out(15, 0);
+		ali512x_cio_out(14, 0);
+		break;
+		
+	default:
+		printf("Illegal SSI device requested: %d\n", dev);
+	}
+}
+
+
+void spi_init_f(void) 
+{
+#ifdef CONFIG_SC520_CDP_USE_SPI
+	spi_eeprom_probe(1); 
+#endif	
+#ifdef CONFIG_SC520_CDP_USE_MW
+	mw_eeprom_probe(2);
+#endif		
+}
+
+ssize_t spi_read(uchar *addr, int alen, uchar *buffer, int len) 
+{
+	int offset;
+	int i;
+	ssize_t res;
+	
+	offset = 0;
+	for (i=0;i<alen;i++) {
+		offset <<= 8;
+		offset |= addr[i];
+	}
+ 
+#ifdef CONFIG_SC520_CDP_USE_SPI
+	res = spi_eeprom_read(1, offset, buffer, len); 
+#endif	
+#ifdef CONFIG_SC520_CDP_USE_MW
+	res = mw_eeprom_read(2, offset, buffer, len);
+#endif	
+	return res;
+}
+
+ssize_t spi_write(uchar *addr, int alen, uchar *buffer, int len) 
+{
+	int offset;
+	int i;
+	ssize_t res;
+	
+	offset = 0;
+	for (i=0;i<alen;i++) {
+		offset <<= 8;
+		offset |= addr[i];
+	}
+		
+#ifdef CONFIG_SC520_CDP_USE_SPI
+	res = spi_eeprom_write(1, offset, buffer, len);
+#endif	
+#ifdef CONFIG_SC520_CDP_USE_MW
+	res = mw_eeprom_write(2, offset, buffer, len);
+#endif	
+	return res;
+}
+
diff --git a/board/sc520_cdp/sc520_cdp_asm16.S b/board/sc520_cdp/sc520_cdp_asm16.S
index a3cadcfa547..0e0b0c028a0 100644
--- a/board/sc520_cdp/sc520_cdp_asm16.S
+++ b/board/sc520_cdp/sc520_cdp_asm16.S
@@ -60,3 +60,25 @@ board_init16:
 	/* the return address is tored in bp */
 	jmp	*%bp
 	
+
+.section .bios, "ax"
+.code16
+.globl realmode_reset
+realmode_reset:
+	/* Alias MMCR to 0xdf000 */
+	movw	$0xfffc, %dx
+	movl	$0x800df0cb, %eax	
+	outl	%eax, %dx		
+	
+	/* Set ds to point to MMCR alias */
+	movw	$0xdf00, %ax		
+	movw	%ax, %ds		 
+		
+	/* issue software reset thorugh MMCR */
+	movl    $0xd72, %edi
+        movb	$0x01, %al
+        movb	%al, (%di)		
+
+1:	hlt
+	jmp	1
+	
diff --git a/board/sc520_cdp/u-boot.lds b/board/sc520_cdp/u-boot.lds
index ac8d3dbecda..fbab9b85f25 100644
--- a/board/sc520_cdp/u-boot.lds
+++ b/board/sc520_cdp/u-boot.lds
@@ -27,7 +27,7 @@ ENTRY(_start)
 
 SECTIONS
 {	
-	. = 0x387e0000;                     /* Where bootcode in the flash is mapped */
+	. = 0x387c0000;                     /* Where bootcode in the flash is mapped */
 	.text  : { *(.text); }
 	
         . = ALIGN(4);