* Add support for 16 MB flash configuration of TRAB board

* Patch by Erwin Rol, 27 Feb 2003:
  Add support for RTEMS

* Add image information to README

* Fix dual PCMCIA slot support (when running with just one
  slot populated)

* Add VFD type detection to trab board

* extend drivers/cs8900.c driver to synchronize  ethaddr  environment
  variable with value in the EEPROM

* Start adding MIPS support files

5 files changed, 435 insertions, 298 deletions

diff --git a/board/sc520_cdp/sc520_cdp.c b/board/sc520_cdp/sc520_cdp.c
index ba6255f4eb6..2809ff72af1 100644
--- a/board/sc520_cdp/sc520_cdp.c
+++ b/board/sc520_cdp/sc520_cdp.c
@@ -31,35 +31,35 @@
 
 static void irq_init(void)
 {
-
+	
 	/* disable global interrupt mode */
-	write_mmcr_byte(SC520_PICICR, 0x40);
-
+	write_mmcr_byte(SC520_PICICR, 0x40); 
+	
 	/* set irq0-7 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);
-
-	/* active low polarity on PIC interrupt pins,
+	
+	/* active low polarity on PIC interrupt pins, 
 	   active high polarity on all other irq pins */
 	write_mmcr_word(SC520_INTPINPOL, 0);
 
 	/* set irq number mapping */
-	write_mmcr_byte(SC520_GPTMR0MAP,0);            /* disable GP timer 0 INT */
+	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_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_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 */
@@ -69,28 +69,28 @@ static void irq_init(void)
 	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 */
-
+	
 	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_GP3IMAP,11);     /* Set GPIRQ3 (ISA IRQ3) to IRQ3 */ 
 		write_mmcr_byte(SC520_GP4IMAP,12);     /* 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_GP3IMAP,0);      /* disable GPIRQ3 (ISA IRQ3) */ 
 		write_mmcr_byte(SC520_GP4IMAP,0);      /* 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_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 */
-
+ 
 }
 
 /* PCI stuff */
@@ -98,11 +98,11 @@ static void pci_sc520_cdp_fixup_irq(struct pci_controller *hose, pci_dev_t dev)
 {
 	char pin;
 	int irq;
-
-
+	
+	
 	pci_hose_read_config_byte(hose, dev, PCI_INTERRUPT_PIN, &pin);
 	irq = pin-1;
-
+	
 	switch (PCI_DEV(dev)) {
 	case 20:
 		break;
@@ -115,20 +115,20 @@ static void pci_sc520_cdp_fixup_irq(struct pci_controller *hose, pci_dev_t dev)
 	case 17:
 		irq+=3;
 		break;
-	default:
+	default: 
 		return;
 	}
-
+	
 	irq&=3; /* wrap around */
 	irq+=9; /* lowest IRQ is 9 */
-
+	
 	pci_hose_write_config_byte(hose, dev, PCI_INTERRUPT_LINE, irq);
-#if 0
-	printf("fixup_irq: device %d pin %c irq %d\n",
+#if 0	
+	printf("fixup_irq: device %d pin %c irq %d\n", 
 	       PCI_DEV(dev), 'A' + pin -1, irq);
 #endif
 }
-
+ 
 static struct pci_controller sc520_cdp_hose = {
 	fixup_irq: pci_sc520_cdp_fixup_irq,
 };
@@ -147,7 +147,7 @@ static void silence_uart(int port)
 void setup_ali_sio(int uart_primary)
 {
 	ali512x_init();
-
+	
 	ali512x_set_fdc(ALI_ENABLED, 0x3f2, 6, 0);
 	ali512x_set_pp(ALI_ENABLED, 0x278, 7, 3);
 	ali512x_set_uart(ALI_ENABLED, ALI_UART1, uart_primary?0x3f8:0x3e8, 4);
@@ -155,21 +155,21 @@ void setup_ali_sio(int uart_primary)
 	ali512x_set_rtc(ALI_DISABLED, 0, 0);
 	ali512x_set_kbc(ALI_ENABLED, 1, 12);
 	ali512x_set_cio(ALI_ENABLED);
-
+	
 	/* IrDa pins */
 	ali512x_cio_function(12, 1, 0, 0);
 	ali512x_cio_function(13, 1, 0, 0);
-
+	
 	/* 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(15, 0, 0, 0);  /* SSI_MV */					     
 	ali512x_cio_function(16, 0, 1, 0);  /* SSI_SPI# (inverted) */
 
 	/* Board REV pins */
 	ali512x_cio_function(20, 0, 0, 1);
 	ali512x_cio_function(21, 0, 0, 1);
 	ali512x_cio_function(22, 0, 0, 1);
-	ali512x_cio_function(23, 0, 0, 1);
+	ali512x_cio_function(23, 0, 0, 1);      
 }
 
 
@@ -178,13 +178,13 @@ static void bus_init(void)
 {
 
 	/* set up the GP IO pins */
-	write_mmcr_word(SC520_PIOPFS31_16, 0xf7ff); 	/* set the GPIO pin function 31-16 reg */
+	write_mmcr_word(SC520_PIOPFS31_16, 0xf7ff); 	/* set the GPIO pin function 31-16 reg */		   
 	write_mmcr_word(SC520_PIOPFS15_0, 0xffff);  	/* set the GPIO pin function 15-0 reg */
-	write_mmcr_byte(SC520_CSPFS, 0xf8);  		/* set the CS pin function  reg */
+	write_mmcr_byte(SC520_CSPFS, 0xf8);  		/* set the CS pin function  reg */	
 	write_mmcr_byte(SC520_CLKSEL, 0x70);
 
-
-	write_mmcr_byte(SC520_GPCSRT, 1);   /* set the GP CS offset */
+	
+	write_mmcr_byte(SC520_GPCSRT, 1);   /* set the GP CS offset */    	
 	write_mmcr_byte(SC520_GPCSPW, 3);   /* set the GP CS pulse width */
 	write_mmcr_byte(SC520_GPCSOFF, 1);  /* set the GP CS offset */
 	write_mmcr_byte(SC520_GPRDW, 3);    /* set the RD pulse width */
@@ -192,40 +192,40 @@ static void bus_init(void)
         write_mmcr_byte(SC520_GPWRW, 3);    /* set the GP WR pulse width */
 	write_mmcr_byte(SC520_GPWROFF, 1);  /* set the GP WR offset */
 
-	write_mmcr_word(SC520_BOOTCSCTL, 0x1823);		/* set up timing of BOOTCS */
+	write_mmcr_word(SC520_BOOTCSCTL, 0x1823);		/* set up timing of BOOTCS */ 
 	write_mmcr_word(SC520_ROMCS1CTL, 0x1823);		/* set up timing of ROMCS1 */
-	write_mmcr_word(SC520_ROMCS2CTL, 0x1823);		/* set up timing of ROMCS2 */
-
+	write_mmcr_word(SC520_ROMCS2CTL, 0x1823);		/* set up timing of ROMCS2 */ 
+	
 	/* adjust the memory map:
 	 * by default the first 256MB (0x00000000 - 0x0fffffff) is mapped to SDRAM
 	 * and 256MB to 1G-128k  (0x1000000 - 0x37ffffff) is mapped to PCI mmio
-	 * we need to map 1G-128k - 1G (0x38000000 - 0x3fffffff) to CS1 */
-
-
+	 * we need to map 1G-128k - 1G (0x38000000 - 0x3fffffff) to CS1 */ 
+	
+		
 	/* SRAM = GPCS3 128k @ d0000-effff*/
-	write_mmcr_long(SC520_PAR2,  0x4e00400d);
-
+	write_mmcr_long(SC520_PAR2,  0x4e00400d);		
+	
 	/* IDE0 = GPCS6 1f0-1f7 */
-	write_mmcr_long(SC520_PAR3,  0x380801f0);
+	write_mmcr_long(SC520_PAR3,  0x380801f0);		
 
 	/* IDE1 = GPCS7 3f6 */
-	write_mmcr_long(SC520_PAR4,  0x3c0003f6);
+	write_mmcr_long(SC520_PAR4,  0x3c0003f6);		
 	/* bootcs */
-	write_mmcr_long(SC520_PAR12, 0x8bffe800);
+	write_mmcr_long(SC520_PAR12, 0x8bffe800);		
 	/* romcs2 */
-	write_mmcr_long(SC520_PAR13, 0xcbfff000);
+	write_mmcr_long(SC520_PAR13, 0xcbfff000);		
 	/* romcs1 */
-	write_mmcr_long(SC520_PAR14, 0xabfff800);
+	write_mmcr_long(SC520_PAR14, 0xabfff800);		
 	/* 680 LEDS */
-	write_mmcr_long(SC520_PAR15, 0x30000640);
-
-	asm ("wbinvd\n"); /* Flush cache, req. after setting the unchached attribute ona PAR */
+	write_mmcr_long(SC520_PAR15, 0x30000640);		
+	
+	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);
+		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);
@@ -242,21 +242,21 @@ static void bus_init(void)
 int board_init(void)
 {
 	DECLARE_GLOBAL_DATA_PTR;
-
-	init_sc520();
+	
+	init_sc520();	
 	bus_init();
 	irq_init();
-
+		
 	/* max drive current on SDRAM */
 	write_mmcr_word(SC520_DSCTL, 0x0100);
-
+		
 	/* 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;
-
+	
 	return 0;
 }
 
@@ -277,14 +277,14 @@ int last_stage_init(void)
 {
 	int minor;
 	int major;
-
+	
 	major = minor = 0;
 	major |= ali512x_cio_in(23)?2:0;
 	major |= ali512x_cio_in(22)?1:0;
 	minor |= ali512x_cio_in(21)?2:0;
 	minor |= ali512x_cio_in(20)?1:0;
-
+	
 	printf("AMD SC520 CDP revision %d.%d\n", major, minor);
-
+	
 	return 0;
 }
diff --git a/board/smdk2400/flash.c b/board/smdk2400/flash.c
index c4d6bae8f2b..6882b884513 100644
--- a/board/smdk2400/flash.c
+++ b/board/smdk2400/flash.c
@@ -76,7 +76,7 @@ ulong flash_init (void)
 		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;
+			flashbase = CFG_FLASH_BASE;
 		else
 			panic ("configured too many flash banks!\n");
 		for (j = 0; j < flash_info[i].sector_count; j++) {
diff --git a/board/trab/flash.c b/board/trab/flash.c
index 2443777ae18..a4f164b469b 100644
--- a/board/trab/flash.c
+++ b/board/trab/flash.c
@@ -26,12 +26,7 @@
 #include <common.h>
 #include <environment.h>
 
-ulong myflush (void);
-
-
-#define FLASH_BANK_SIZE	0x800000	/* 8 MB */
-/* this varies depending on the sector */
-#define MAIN_SECT_SIZE  0x20000		/* 2 x 64 kB */
+static ulong flash_get_size (vu_long *addr, flash_info_t *info);
 
 flash_info_t flash_info[CFG_MAX_FLASH_BANKS];
 
@@ -43,6 +38,7 @@ flash_info_t flash_info[CFG_MAX_FLASH_BANKS];
 #define CMD_ERASE_CONFIRM	0x00300030
 #define CMD_PROGRAM		0x00A000A0
 #define CMD_UNLOCK_BYPASS	0x00200020
+#define CMD_READ_MANF_ID	0x00900090
 
 #define MEM_FLASH_ADDR1		(*(volatile u32 *)(CFG_FLASH_BASE + (0x00000555 << 2)))
 #define MEM_FLASH_ADDR2		(*(volatile u32 *)(CFG_FLASH_BASE + (0x000002AA << 2)))
@@ -64,27 +60,38 @@ ulong flash_init (void)
 	int i, j;
 	ulong size = 0;
 
-	for (i = 0; i < CFG_MAX_FLASH_BANKS; i++) {
+	for (i=0; i<CFG_MAX_FLASH_BANKS; ++i) {
 		ulong flashbase = 0;
+		flash_info_t *info = &flash_info[i];
+
+		/* Init: no FLASHes known */
+		info->flash_id = FLASH_UNKNOWN;
+
+		size += flash_get_size (CFG_FLASH_BASE, info);
 
-		flash_info[i].flash_id =
-			(AMD_MANUFACT  & FLASH_VENDMASK) |
-			(AMD_ID_LV320B & 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;
+			flashbase = CFG_FLASH_BASE;
 		else
 			panic ("configured too many flash banks!\n");
-		for (j = 0; j < flash_info[i].sector_count; j++) {
-
-			flash_info[i].start[j] = flashbase;
-
-			/* the first 8 sectors are 8 kB */
-			flashbase += (j < 8) ? 0x4000 : MAIN_SECT_SIZE;
+		for (j = 0; j < info->sector_count; j++) {
+
+			info->protect[j] = 0;
+			info->start[j] = flashbase;
+
+			switch (info->flash_id & FLASH_TYPEMASK) {
+			case (FLASH_AM320B & FLASH_TYPEMASK):
+				/* Boot sector type: 8 x 8 + N x 128 kB */
+				flashbase += (j < 8) ? 0x4000 : 0x20000;
+				break;
+			case (FLASH_AM640U & FLASH_TYPEMASK):
+				/* Uniform sector type: 128 kB */
+				flashbase += 0x20000;
+				break;
+			default:
+				printf ("## Bad flash chip type 0x%04lX\n",
+					info->flash_id & FLASH_TYPEMASK);
+			}
 		}
-		size += flash_info[i].size;
 	}
 
 	/*
@@ -116,7 +123,7 @@ void flash_print_info (flash_info_t * info)
 	int i;
 
 	switch (info->flash_id & FLASH_VENDMASK) {
-	case (AMD_MANUFACT & FLASH_VENDMASK):
+	case (FLASH_MAN_AMD & FLASH_VENDMASK):
 		printf ("AMD: ");
 		break;
 	default:
@@ -125,9 +132,12 @@ void flash_print_info (flash_info_t * info)
 	}
 
 	switch (info->flash_id & FLASH_TYPEMASK) {
-	case (AMD_ID_LV320B & FLASH_TYPEMASK):
+	case (FLASH_AM320B & FLASH_TYPEMASK):
 		printf ("2x Am29LV320DB (32Mbit)\n");
 		break;
+	case (FLASH_AM640U & FLASH_TYPEMASK):
+		printf ("2x Am29LV640D (64Mbit)\n");
+		break;
 	default:
 		printf ("Unknown Chip Type\n");
 		goto Done;
@@ -177,7 +187,7 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 	}
 
 	if ((info->flash_id & FLASH_VENDMASK) !=
-		(AMD_MANUFACT & FLASH_VENDMASK)) {
+		(FLASH_MAN_AMD & FLASH_VENDMASK)) {
 		return ERR_UNKNOWN_FLASH_VENDOR;
 	}
 
@@ -265,13 +275,6 @@ int flash_erase (flash_info_t * info, int s_first, int s_last)
 				rc = ERR_TIMOUT;
 				goto outahere;
 			}
-
-#if 0
-			printf ("ok.\n");
-		} else {		/* it was protected */
-
-			printf ("protected!\n");
-#endif
 		}
 	}
 
@@ -453,3 +456,71 @@ int write_buff (flash_info_t * info, uchar * src, ulong addr, ulong cnt)
 
 	return write_word (info, wp, data);
 }
+
+/*-----------------------------------------------------------------------
+ */
+
+static ulong flash_get_size (vu_long *addr, flash_info_t *info)
+{
+	ulong value;
+
+	/* Write auto select command sequence and read Manufacturer ID */
+	addr[0x0555] = CMD_UNLOCK1;
+	addr[0x02AA] = CMD_UNLOCK2;
+	addr[0x0555] = CMD_READ_MANF_ID;
+
+	value = addr[0];
+
+	debug ("Manuf. ID @ 0x%08lx: 0x%08lx\n", (ulong)addr, value);
+
+	switch (value) {
+	case AMD_MANUFACT:
+		info->flash_id = FLASH_MAN_AMD;
+		break;
+
+	default:
+		info->flash_id = FLASH_UNKNOWN;
+		info->sector_count = 0;
+		info->size = 0;
+		addr[0] = 0x00FF00FF;		/* restore read mode */
+		debug ("## flash_init: unknown manufacturer\n");
+		return (0);			/* no or unknown flash	*/
+	}
+
+	value = addr[1];			/* device ID		*/
+
+	debug ("Device ID @ 0x%08lx: 0x%08lx\n", (ulong)(&addr[1]), value);
+
+	switch (value) {
+	case AMD_ID_LV320B:
+		info->flash_id += FLASH_AM320B;
+		info->sector_count = 71;
+		info->size = 0x00800000;
+
+		addr[0] = 0x00FF00FF;		/* restore read mode */
+		break;				/* =>  8 MB		*/
+
+	case AMD_ID_LV640U:
+		info->flash_id += FLASH_AM640U;
+		info->sector_count = 128;
+		info->size = 0x01000000;
+
+		addr[0] = 0x00F000F0;		/* restore read mode */
+		break;				/* => 16 MB		*/
+
+	default:
+		debug ("## flash_init: unknown flash chip\n");
+		info->flash_id = FLASH_UNKNOWN;
+		addr[0] = 0x00FF00FF;		/* restore read mode */
+		return (0);			/* => no or unknown flash */
+
+	}
+
+	if (info->sector_count > CFG_MAX_FLASH_SECT) {
+		printf ("** ERROR: sector count %d > max (%d) **\n",
+			info->sector_count, CFG_MAX_FLASH_SECT);
+		info->sector_count = CFG_MAX_FLASH_SECT;
+	}
+
+	return (info->size);
+}
diff --git a/board/trab/trab.c b/board/trab/trab.c
index 32f27efaeb9..111c861410b 100644
--- a/board/trab/trab.c
+++ b/board/trab/trab.c
@@ -60,11 +60,8 @@ static void udelay_no_timer (int usec)
 
 int board_init ()
 {
-#if defined(CONFIG_MODEM_SUPPORT) && defined(CONFIG_VFD)
-	ulong size;
-	unsigned long addr;
-	extern void mem_malloc_init (ulong);
-	extern int drv_vfd_init(void);
+#if defined(CONFIG_VFD)
+	extern int vfd_init_clocks(void);
 #endif
 	DECLARE_GLOBAL_DATA_PTR;
 
@@ -107,26 +104,11 @@ int board_init ()
 	/* adress of boot parameters */
 	gd->bd->bi_boot_params = 0x0c000100;
 
-#ifdef CONFIG_MODEM_SUPPORT
 #ifdef CONFIG_VFD
-#ifndef PAGE_SIZE
-#define PAGE_SIZE 4096
-#endif
-	/*
-	 * reserve memory for VFD display (always full pages)
-	 */
-	/* armboot_real_end is defined in the board-specific linker script */
-	addr = (_armboot_real_end + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1);
-	size = vfd_setmem (addr);
-	gd->fb_base = addr;
-	/* round to the next page boundary */
-	addr += size;
-	addr = (addr + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1);
-	mem_malloc_init (addr);
-	/* must do this after the framebuffer is allocated */
-	drv_vfd_init();
+	vfd_init_clocks();
 #endif /* CONFIG_VFD */
 
+#ifdef CONFIG_MODEM_SUPPORT
 	udelay_no_timer (KBD_MDELAY);
 
 	if (key_pressed()) {
diff --git a/board/trab/vfd.c b/board/trab/vfd.c
index 234a9d4c027..fa1194ca7fb 100644
--- a/board/trab/vfd.c
+++ b/board/trab/vfd.c
@@ -55,12 +55,16 @@
 #define BLAU	0x0C
 #define VIOLETT	0X0D
 
-ulong vfdbase;
-ulong frame_buf_size;
+ulong	vfdbase;
+ulong	frame_buf_size;
 #define frame_buf_offs 4
 
+/* Supported VFD Types */
+#define	VFD_TYPE_T119C		1	/* Noritake T119C VFD */
+#define	VFD_TYPE_MN11236	2
+
 /* taken from armboot/common/vfd.c */
-ulong         adr_vfd_table[112][18][2][4][2];
+unsigned long adr_vfd_table[112][18][2][4][2];
 unsigned char bit_vfd_table[112][18][2][4][2];
 
 /*
@@ -68,26 +72,42 @@ unsigned char bit_vfd_table[112][18][2][4][2];
  */
 void init_grid_ctrl(void)
 {
+	DECLARE_GLOBAL_DATA_PTR;
 	ulong adr, grid_cycle;
 	unsigned int bit, display;
 	unsigned char temp, bit_nr;
+	ulong val;
 
-	for (adr=vfdbase; adr<=(vfdbase+7168); adr+=4) /*clear frame buffer */
-		(*(volatile ulong*)(adr))=0;
+	/*
+	 * clear frame buffer (logical clear => set to "black")
+	 */
+	if (gd->vfd_inv_data == 0)
+		val = 0;
+	else
+		val = ~0;
 
-	for(display=0;display<=3;display++)
-	{
-		for(grid_cycle=0;grid_cycle<=55;grid_cycle++)
-		{
-			bit = grid_cycle*256*4+(grid_cycle+200)*4+frame_buf_offs+display;
+	for (adr = vfdbase; adr <= (vfdbase+7168); adr += 4) {
+		(*(volatile ulong*)(adr)) = val;
+	}
+
+	switch (gd->vfd_type) {
+	case VFD_TYPE_T119C:
+	    for (display=0; display<4; display++) {
+		for(grid_cycle=0; grid_cycle<56; grid_cycle++) {
+			bit = grid_cycle * 256  * 4 +
+			     (grid_cycle + 200) * 4 +
+			     frame_buf_offs + display;
  			/* wrap arround if offset (see manual S3C2400) */
 			if (bit>=frame_buf_size*8)
-				bit = bit-(frame_buf_size*8);
-			adr = vfdbase+(bit/32)*4+(3-(bit%32)/8);
-			bit_nr = bit%8;
-			bit_nr = (bit_nr>3)?bit_nr-4:bit_nr+4;
+				bit = bit - (frame_buf_size * 8);
+			adr = vfdbase + (bit/32) * 4 + (3 - (bit%32) / 8);
+			bit_nr = bit % 8;
+			bit_nr = (bit_nr > 3) ? bit_nr-4 : bit_nr+4;
 			temp=(*(volatile unsigned char*)(adr));
-			temp|=(1<<bit_nr);
+			if (gd->vfd_inv_data)
+				temp &= ~(1<<bit_nr);
+			else
+				temp |=  (1<<bit_nr);
 			(*(volatile unsigned char*)(adr))=temp;
 
 			if(grid_cycle<55)
@@ -101,9 +121,54 @@ void init_grid_ctrl(void)
 			bit_nr = bit%8;
 			bit_nr = (bit_nr>3)?bit_nr-4:bit_nr+4;
 			temp=(*(volatile unsigned char*)(adr));
-			temp|=(1<<bit_nr);
+			if (gd->vfd_inv_data)
+				temp &= ~(1<<bit_nr);
+			else
+				temp |=  (1<<bit_nr);
+			(*(volatile unsigned char*)(adr))=temp;
+		}
+	    }
+	    break;
+	case VFD_TYPE_MN11236:
+	    for (display=0; display<4; display++) {
+		for (grid_cycle=0; grid_cycle<38; grid_cycle++) {
+			bit = grid_cycle * 256  * 4 +
+			     (253 - grid_cycle) * 4 +
+			     frame_buf_offs + display;
+			/* wrap arround if offset (see manual S3C2400) */
+			if (bit>=frame_buf_size*8)
+				bit = bit - (frame_buf_size * 8);
+			adr = vfdbase + (bit/32) * 4 + (3 - (bit%32) / 8);
+			bit_nr = bit % 8;
+			bit_nr = (bit_nr > 3) ? bit_nr-4 : bit_nr+4;
+			temp=(*(volatile unsigned char*)(adr));
+			if (gd->vfd_inv_data)
+				temp &= ~(1<<bit_nr);
+			else
+				temp |=  (1<<bit_nr);
+			(*(volatile unsigned char*)(adr))=temp;
+
+			if(grid_cycle<37)
+				bit = grid_cycle*256*4+(252-grid_cycle)*4+frame_buf_offs+display;
+
+			/* wrap arround if offset (see manual S3C2400) */
+			if (bit>=frame_buf_size*8)
+				bit = bit-(frame_buf_size*8);
+			adr = vfdbase+(bit/32)*4+(3-(bit%32)/8);
+			bit_nr = bit%8;
+			bit_nr = (bit_nr>3)?bit_nr-4:bit_nr+4;
+			temp=(*(volatile unsigned char*)(adr));
+			if (gd->vfd_inv_data)
+				temp &= ~(1<<bit_nr);
+			else
+				temp |=  (1<<bit_nr);
 			(*(volatile unsigned char*)(adr))=temp;
 		}
+	    }
+	    break;
+	default:
+	    printf ("Warning: unknown display type\n");
+	    break;
 	}
 }
 
@@ -113,175 +178,142 @@ void init_grid_ctrl(void)
  */
 void create_vfd_table(void)
 {
-	unsigned int vfd_table[112][18][2][4][2];
-	ulong adr;
-	unsigned int x, y, color, display, entry, pixel, bit_nr;
-
-	/*
-	 * Create translation table for Noritake-T119C-VFD-specific
-	 * organized frame-buffer.
-	 * Created is the number of the bit in the framebuffer (the
-	 * first transferred pixel of each frame is bit 0).
-	 */
-	for(y=0;y<=17;y++)   /* Zeile */
-	{
-		for(x=0;x<=111;x++)  /* Spalten */
-		{
-			/*Display 0 blaue Pixel Eintrag 1 */
-			vfd_table[x][y][0][0][0]=((x%4)*4+y*16+(x/4)*2048);
-			/*Display 0 rote Pixel Eintrag 1 */
-			vfd_table[x][y][1][0][0]=((x%4)*4+y*16+(x/4)*2048+512);
-			if(x<=1)
-			{
-				/*Display 0 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][0][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+1024);
-				/*Display 0 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][0][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+512+1024);
-			}
-			else
-			{
-				/*Display 0 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][0][1]=((x%4)*4+y*16+((x-2)/4)*2048+1024);
-				/*Display 0 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][0][1]=((x%4)*4+y*16+((x-2)/4)*2048+512+1024);
-			}
-			/*Display 1 blaue Pixel Eintrag 1 */
-			vfd_table[x][y][0][1][0]=((x%4)*4+y*16+(x/4)*2048+1);
-			/*Display 1 rote Pixel Eintrag 1 */
-			vfd_table[x][y][1][1][0]=((x%4)*4+y*16+(x/4)*2048+512+1);
-			if(x<=1)
-			{
-				/*Display 1 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][1][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+1+1024);
-				/*Display 1 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][1][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+512+1+1024);
-			}
-			else
-			{
-				/*Display 1 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][1][1]=((x%4)*4+y*16+((x-2)/4)*2048+1+1024);
-				/*Display 1 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][1][1]=((x%4)*4+y*16+((x-2)/4)*2048+512+1+1024);
-			}
-			/*Display 2 blaue Pixel Eintrag 1 */
-			vfd_table[x][y][0][2][0]=((x%4)*4+y*16+(x/4)*2048+2);
-			/*Display 2 rote Pixel Eintrag 1 */
-			vfd_table[x][y][1][2][0]=((x%4)*4+y*16+(x/4)*2048+512+2);
-			if(x<=1)
-			{
-				/*Display 2 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][2][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+2+1024);
-				/*Display 2 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][2][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+512+2+1024);
-			}
-			else
-			{
-				/*Display 2 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][2][1]=((x%4)*4+y*16+((x-2)/4)*2048+2+1024);
-				/*Display 2 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][2][1]=((x%4)*4+y*16+((x-2)/4)*2048+512+2+1024);
-			}
-			/*Display 3 blaue Pixel Eintrag 1 */
-			vfd_table[x][y][0][3][0]=((x%4)*4+y*16+(x/4)*2048+3);
-			/*Display 3 rote Pixel Eintrag 1 */
-			vfd_table[x][y][1][3][0]=((x%4)*4+y*16+(x/4)*2048+512+3);
-			if(x<=1)
-			{
-				/*Display 3 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][3][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+3+1024);
-				/*Display 3 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][3][1]=(((x+112)%4)*4+y*16+((x+110)/4)*2048+512+3+1024);
-			}
-			else
-			{
-				/*Display 3 blaue Pixel Eintrag 2 */
-				vfd_table[x][y][0][3][1]=((x%4)*4+y*16+((x-2)/4)*2048+3+1024);
-				/*Display 3 rote Pixel Eintrag 2 */
-				vfd_table[x][y][1][3][1]=((x%4)*4+y*16+((x-2)/4)*2048+512+3+1024);
-			}
+	DECLARE_GLOBAL_DATA_PTR;
+	unsigned long vfd_table[112][18][2][4][2];
+	unsigned int x, y, color, display, entry, pixel;
+	unsigned int x_abcdef = 0;
+
+	switch (gd->vfd_type) {
+	case VFD_TYPE_T119C:
+	    for(y=0; y<=17; y++) {	/* Line */
+		for(x=0; x<=111; x++) {	/* Column */
+		    for(display=0; display <=3; display++) {
+
+			    /* Display 0 blue pixels */
+			    vfd_table[x][y][0][display][0] =
+				(x==0) ? y*16+display
+				       : (x%4)*4+y*16+((x-1)/2)*1024+display;
+			    /* Display 0 red pixels */
+			    vfd_table[x][y][1][display][0] =
+				(x==0) ? y*16+512+display
+			    	       : (x%4)*4+y*16+((x-1)/2)*1024+512+display;
+		    }
+		}
+	    }
+	    break;
+	case VFD_TYPE_MN11236:
+	    for(y=0; y<=17; y++) {	/* Line */
+		for(x=0; x<=111; x++) {	/* Column */
+		    for(display=0; display <=3; display++) {
+
+			    vfd_table[x][y][0][display][0]=0;
+			    vfd_table[x][y][0][display][1]=0;
+			    vfd_table[x][y][1][display][0]=0;
+			    vfd_table[x][y][1][display][1]=0;
+
+			    switch (x%6) {
+			    case 0: x_abcdef=0; break; /* a -> a */
+			    case 1: x_abcdef=2; break; /* b -> c */
+			    case 2: x_abcdef=4; break; /* c -> e */
+			    case 3: x_abcdef=5; break; /* d -> f */
+			    case 4: x_abcdef=3; break; /* e -> d */
+			    case 5: x_abcdef=1; break; /* f -> b */
+			    }
+
+			    /* blue pixels */
+			    vfd_table[x][y][0][display][0] =
+				(x>1) ? x_abcdef*4+((x-1)/3)*1024+y*48+display
+				      : x_abcdef*4+             0+y*48+display;
+			    /* blue pixels */
+			    if (x>1 && (x-1)%3)
+				    vfd_table[x][y][0][display][1] = x_abcdef*4+((x-1)/3+1)*1024+y*48+display;
+
+			    /* red pixels */
+			    vfd_table[x][y][1][display][0] =
+				(x>1) ? x_abcdef*4+24+((x-1)/3)*1024+y*48+display
+				      : x_abcdef*4+24+             0+y*48+display;
+			    /* red pixels */
+			    if (x>1 && (x-1)%3)
+				    vfd_table[x][y][1][display][1] = x_abcdef*4+24+((x-1)/3+1)*1024+y*48+display;
+		    }
 		}
+	    }
+	    break;
+	default:
+	    /* do nothing */
+	    return;
 	}
 
 	/*
-	 * Create translation table for Noritake-T119C-VFD-specific
-	 * organized frame-buffer
 	 * Create table with entries for physical byte adresses and
 	 * bit-number within the byte
 	 * from table with bit-numbers within the total framebuffer
 	 */
-	for(y=0;y<=17;y++)
-	{
-		for(x=0;x<=111;x++)
-		{
-			for(color=0;color<=1;color++)
-			{
-				for(display=0;display<=3;display++)
-				{
-					for(entry=0;entry<=1;entry++)
-					{
-						pixel  = vfd_table[x][y][color][display][entry] + frame_buf_offs;
-						 /*
-						  * wrap arround if offset
-						  * (see manual S3C2400)
-						  */
-						if (pixel>=frame_buf_size*8)
-							pixel = pixel-(frame_buf_size*8);
-						adr    = vfdbase+(pixel/32)*4+(3-(pixel%32)/8);
-						bit_nr = pixel%8;
-						bit_nr = (bit_nr>3)?bit_nr-4:bit_nr+4;
-						adr_vfd_table[x][y][color][display][entry] = adr;
-						bit_vfd_table[x][y][color][display][entry] = bit_nr;
-					}
-				}
+	for(y=0;y<18;y++) {
+	    for(x=0;x<112;x++) {
+		for(color=0;color<2;color++) {
+		    for(display=0;display<4;display++) {
+			for(entry=0;entry<2;entry++) {
+			    unsigned long adr  = vfdbase;
+			    unsigned int bit_nr = 0;
+			    
+			    if (vfd_table[x][y][color][display][entry]) {
+
+				pixel  = vfd_table[x][y][color][display][entry] + frame_buf_offs;
+				 /*
+				  * wrap arround if offset
+				  * (see manual S3C2400)
+				  */
+				if (pixel>=frame_buf_size*8)
+					pixel = pixel-(frame_buf_size*8);
+				adr    = vfdbase+(pixel/32)*4+(3-(pixel%32)/8);
+				bit_nr = pixel%8;
+				bit_nr = (bit_nr>3)?bit_nr-4:bit_nr+4;
+			    }
+			    adr_vfd_table[x][y][color][display][entry] = adr;
+			    bit_vfd_table[x][y][color][display][entry] = bit_nr;
 			}
+		    }
 		}
+	    }
 	}
 }
 
 /*
  * Set/clear pixel of the VFDs
  */
-void set_vfd_pixel(unsigned char x, unsigned char y, unsigned char color, unsigned char display, unsigned char value)
+void set_vfd_pixel(unsigned char x, unsigned char y,
+		   unsigned char color, unsigned char display,
+		   unsigned char value)
 {
+	DECLARE_GLOBAL_DATA_PTR;
 	ulong adr;
 	unsigned char bit_nr, temp;
 
-	if (value!=0)
-	{
-		/* Pixel-Eintrag Nr. 1 */
-		adr = adr_vfd_table[x][y][color][display][0];
-		/* Pixel-Eintrag Nr. 1 */
-		bit_nr = bit_vfd_table[x][y][color][display][0];
-		temp=(*(volatile unsigned char*)(adr));
-		temp|=1<<bit_nr;
-		(*(volatile unsigned char*)(adr))=temp;
-
-		/* Pixel-Eintrag Nr. 2 */
-		adr = adr_vfd_table[x][y][color][display][1];
-		/* Pixel-Eintrag Nr. 2 */
-		bit_nr = bit_vfd_table[x][y][color][display][1];
-		temp=(*(volatile unsigned char*)(adr));
-		temp|=1<<bit_nr;
-		(*(volatile unsigned char*)(adr))=temp;
+	if (! gd->vfd_type) {
+		/* Unknown type. */
+		return;
 	}
-	else
-	{
-		/* Pixel-Eintrag Nr. 1 */
-		adr = adr_vfd_table[x][y][color][display][0];
-		/* Pixel-Eintrag Nr. 1 */
-		bit_nr = bit_vfd_table[x][y][color][display][0];
-		temp=(*(volatile unsigned char*)(adr));
-		temp&=~(1<<bit_nr);
-		(*(volatile unsigned char*)(adr))=temp;
-
-		/* Pixel-Eintrag Nr. 2 */
-		adr = adr_vfd_table[x][y][color][display][1];
-		/* Pixel-Eintrag Nr. 2 */
-		bit_nr = bit_vfd_table[x][y][color][display][1];
-		temp=(*(volatile unsigned char*)(adr));
-		temp&=~(1<<bit_nr);
-		(*(volatile unsigned char*)(adr))=temp;
+
+	/* Pixel-Eintrag Nr. 1 */
+	adr = adr_vfd_table[x][y][color][display][0];
+	/* Pixel-Eintrag Nr. 1 */
+	bit_nr = bit_vfd_table[x][y][color][display][0];
+	temp=(*(volatile unsigned char*)(adr));
+
+	if (gd->vfd_inv_data) {
+		if (value)
+			temp &= ~(1<<bit_nr);
+		else
+			temp |=  (1<<bit_nr);
+	} else {
+		if (value)
+			temp |=  (1<<bit_nr);
+		else
+			temp &= ~(1<<bit_nr);
 	}
+	
+	(*(volatile unsigned char*)(adr))=temp;
 }
 
 /*
@@ -334,12 +366,36 @@ void transfer_pic(int display, unsigned char *adr, int height, int width)
 }
 
 /*
+ * This function initializes VFD clock that is needed for the CPLD that
+ * manages the keyboard.
+ */
+int vfd_init_clocks(void)
+{
+	/* Port-Pins als LCD-Ausgang */
+	rPCCON =   (rPCCON & 0xFFFFFF00)| 0x000000AA;
+	/* Port-Pins als LCD-Ausgang */
+	rPDCON =   (rPDCON & 0xFFFFFF03)| 0x000000A8;
+#ifdef WITH_VFRAME
+	/* mit VFRAME zum Messen */
+	rPDCON =   (rPDCON & 0xFFFFFF00)| 0x000000AA;
+#endif
+
+	rLCDCON2 = 0x000DC000;
+	rLCDCON3 = 0x0051000A;
+	rLCDCON4 = 0x00000001;
+	rLCDCON5 = 0x00000440;
+	rLCDCON1 = 0x00000B75;
+}
+
+/*
  * initialize LCD-Controller of the S3C2400 for using VFDs
  */
 int drv_vfd_init(void)
 {
+	char *tmp;
 	ulong palette;
 	static int vfd_init_done = 0;
+	int vfd_id;
 
 	DECLARE_GLOBAL_DATA_PTR;
 
@@ -347,10 +403,58 @@ int drv_vfd_init(void)
 		return (0);
 	vfd_init_done = 1;
 
+	/* try to determine display type from the value
+	 * defined by pull-ups
+	 */
+	rPCUP  = (rPCUP | 0x000F);	/* activate  GPC0...GPC3 pullups */
+	rPCCON = (rPCCON & 0xFFFFFF00);	/* configure GPC0...GPC3 as inputs */
+
+	vfd_id = (~rPCDAT) & 0x000F;	/* read GPC0...GPC3 port pins */
+	debug("Detecting Revison of WA4-VFD: ID=0x%X\n", vfd_id);
+
+	switch (vfd_id) {
+	case 0:				/* board revision <= Rev.100 */
+/*-----*/
+		gd->vfd_inv_data = 0;
+		if (0)
+			gd->vfd_type = VFD_TYPE_MN11236;
+		else
+			gd->vfd_type = VFD_TYPE_T119C;
+/*-----*/
+		if ((tmp = getenv ("vfd_type")) == NULL) {
+			break;
+		}
+		if (strcmp(tmp, "T119C") == 0) {
+			gd->vfd_type = VFD_TYPE_T119C;
+		} else if (strcmp(tmp, "MN11236") == 0) {
+			gd->vfd_type = VFD_TYPE_MN11236;
+		} else {
+			/* cannot use printf for a warning here */
+			gd->vfd_type = 0;	/* unknown */
+		}
+		gd->vfd_inv_data = 0;
+
+		break;
+	default:			/* default to MN11236, data inverted */
+		gd->vfd_type = VFD_TYPE_MN11236;
+		gd->vfd_inv_data = 1;
+		setenv ("vfd_type", "MN11236");
+	}
+	debug ("VFD type: %s%s\n",
+		(gd->vfd_type == VFD_TYPE_T119C)   ? "T119C" :
+		(gd->vfd_type == VFD_TYPE_MN11236) ? "MN11236" :
+		"unknown",
+		gd->vfd_inv_data ? ", inverted data" : "");
+
 	vfdbase = gd->fb_base;
 	create_vfd_table();
 	init_grid_ctrl();
 
+	for (palette=0; palette < 16; palette++)
+		(*(volatile unsigned int*)(PALETTE+(palette*4)))=palette;
+	for (palette=16; palette < 256; palette++)
+		(*(volatile unsigned int*)(PALETTE+(palette*4)))=0x00;
+
 	/*
 	 * Hinweis: Der Framebuffer ist um genau ein Nibble verschoben
 	 * Das erste angezeigte Pixel wird aus dem zweiten Nibble geholt
@@ -364,30 +468,10 @@ int drv_vfd_init(void)
 	rLCDSADDR2 = (vfdbase + frame_buf_size) >> 1;
 	rLCDSADDR3 = ((256/4));
 
-	/* Port-Pins als LCD-Ausgang */
-	rPCCON =   (rPCCON & 0xFFFFFF00)| 0x000000AA;
-	/* Port-Pins als LCD-Ausgang */
-	rPDCON =   (rPDCON & 0xFFFFFF03)| 0x000000A8;
-#ifdef WITH_VFRAME
-	/* mit VFRAME zum Messen */
-	rPDCON =   (rPDCON & 0xFFFFFF00)| 0x000000AA;
-#endif
-
-	rLCDCON2 = 0x000DC000;
-	rLCDCON3 = 0x0051000A;
-	rLCDCON4 = 0x00000001;
-	rLCDCON5 = 0x00000440;
-	rLCDCON1 = 0x00000B75;
-
 	debug ("LCDSADDR1: %lX\n", rLCDSADDR1);
 	debug ("LCDSADDR2: %lX\n", rLCDSADDR2);
 	debug ("LCDSADDR3: %lX\n", rLCDSADDR3);
 
-	for(palette=0;palette<=15;palette++)
-		(*(volatile unsigned int*)(PALETTE+(palette*4)))=palette;
-	for(palette=16;palette<=255;palette++)
-		(*(volatile unsigned int*)(PALETTE+(palette*4)))=0x00;
-
 	return 0;
 }