From 8d84757d5170969e8bdfebc7951f43c5aa2b05fd Mon Sep 17 00:00:00 2001
From: Gregory Hermant <gregory.hermant@calao-systems.com>
Date: Fri, 6 Jul 2012 16:32:47 +0200
Subject: [PATCH] Add support for the Calao-systems USB-A9G20-LPW


Signed-off-by: Gregory Hermant <gregory.hermant@calao-systems.com>
---
 board/usb_a9g20_lpw/nandflash/Makefile        |  121 ++++++++++
 board/usb_a9g20_lpw/nandflash/usb-a9g20-lpw.h |  112 +++++++++
 board/usb_a9g20_lpw/usb_a9g20_lpw.c           |  303 +++++++++++++++++++++++++
 crt0_gnu.S                                    |    7 +
 include/part.h                                |    6 +-
 5 files changed, 548 insertions(+), 1 deletions(-)
 create mode 100644 board/usb_a9g20_lpw/nandflash/Makefile
 create mode 100644 board/usb_a9g20_lpw/nandflash/usb-a9g20-lpw.h
 create mode 100644 board/usb_a9g20_lpw/usb_a9g20_lpw.c

diff --git a/board/usb_a9g20_lpw/nandflash/Makefile b/board/usb_a9g20_lpw/nandflash/Makefile
new file mode 100644
index 0000000..8c9d99a
--- /dev/null
+++ b/board/usb_a9g20_lpw/nandflash/Makefile
@@ -0,0 +1,121 @@
+# TODO: set this appropriately for your local toolchain
+ifndef ERASE_FCT
+ERASE_FCT=rm -f
+endif
+ifndef CROSS_COMPILE
+CROSS_COMPILE=arm-elf-
+endif
+
+TOOLCHAIN=gcc
+
+BOOTSTRAP_PATH=../../..
+
+# NandFlashBoot Configuration for USB-A9G20-LPW
+
+# Target name (case sensitive!!!)
+TARGET=AT91SAM9G20
+# Board name (case sensitive!!!)
+BOARD=usb_a9g20_lpw
+# Link Address and Top_of_Memory
+LINK_ADDR=0x200000
+TOP_OF_MEMORY=0x301000
+# Name of current directory
+PROJECT=nandflash
+
+ifndef BOOT_NAME
+BOOT_NAME=$(PROJECT)_$(BOARD)
+endif
+
+INCL=./$(BOOTSTRAP_PATH)/board/$(BOARD)/$(PROJECT)
+
+ifeq ($(TOOLCHAIN), gcc)
+
+AS=$(CROSS_COMPILE)gcc
+CC=$(CROSS_COMPILE)gcc
+LD=$(CROSS_COMPILE)gcc
+NM= $(CROSS_COMPILE)nm
+SIZE=$(CROSS_COMPILE)size
+OBJCOPY=$(CROSS_COMPILE)objcopy
+OBJDUMP=$(CROSS_COMPILE)objdump
+CCFLAGS=-g -mcpu=arm926ej-s -Os -Wall -D$(TARGET) -D$(BOARD) -I$(INCL)
+ASFLAGS=-g -mcpu=arm926ej-s -c -Os -Wall -D$(TARGET) -D$(BOARD) -I$(INCL) -DTOP_OF_MEM=$(TOP_OF_MEMORY)
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to map file
+LDFLAGS+=-nostartfiles -nostdlib -Wl,-Map=$(BOOT_NAME).map,--cref
+LDFLAGS+=-T $(BOOTSTRAP_PATH)/elf32-littlearm.lds -Ttext $(LINK_ADDR)
+OBJS=crt0_gnu.o
+
+endif
+
+OBJS+=\
+	$(BOARD).o \
+	main.o \
+	gpio.o \
+	pmc.o \
+	debug.o \
+	sdramc.o \
+	nandflash.o \
+	_udivsi3.o \
+	_umodsi3.o \
+	div0.o \
+	udiv.o \
+	string.o
+
+rebuild: clean all
+
+all: 	$(BOOT_NAME)
+
+ifeq ($(TOOLCHAIN), gcc)
+$(BOOT_NAME): $(OBJS)
+	$(LD) $(LDFLAGS) -n -o $(BOOT_NAME).elf $(OBJS)
+	$(OBJCOPY) --strip-debug --strip-unneeded $(BOOT_NAME).elf -O binary $(BOOT_NAME).bin
+endif
+
+
+$(BOARD).o: $(BOOTSTRAP_PATH)/board/$(BOARD)/$(BOARD).c
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/board/$(BOARD)/$(BOARD).c -o $(BOARD).o
+
+main.o: $(BOOTSTRAP_PATH)/main.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/main.c -o main.o
+
+gpio.o: $(BOOTSTRAP_PATH)/driver/gpio.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/gpio.c -o gpio.o
+
+pmc.o:  $(BOOTSTRAP_PATH)/driver/pmc.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/pmc.c -o pmc.o
+
+debug.o: $(BOOTSTRAP_PATH)/driver/debug.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/debug.c -o debug.o
+
+sdramc.o:  $(BOOTSTRAP_PATH)/driver/sdramc.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/sdramc.c -o sdramc.o
+
+dataflash.o:  $(BOOTSTRAP_PATH)/driver/dataflash.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/dataflash.c -o dataflash.o
+
+nandflash.o:  $(BOOTSTRAP_PATH)/driver/nandflash.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/driver/nandflash.c -o nandflash.o
+
+crt0_gnu.o:  $(BOOTSTRAP_PATH)/crt0_gnu.S
+	$(AS) $(ASFLAGS) $(BOOTSTRAP_PATH)/crt0_gnu.S -o crt0_gnu.o
+
+div0.o:  $(BOOTSTRAP_PATH)/lib/div0.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/lib/div0.c -o div0.o
+
+string.o:  $(BOOTSTRAP_PATH)/lib/string.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/lib/string.c -o string.o
+
+udiv.o:  $(BOOTSTRAP_PATH)/lib/udiv.c 
+	$(CC) -c $(CCFLAGS) $(BOOTSTRAP_PATH)/lib/udiv.c -o udiv.o
+
+_udivsi3.o:  $(BOOTSTRAP_PATH)/lib/_udivsi3.S
+	$(AS) $(ASFLAGS) $(BOOTSTRAP_PATH)/lib/_udivsi3.S -o _udivsi3.o
+
+_umodsi3.o:  $(BOOTSTRAP_PATH)/lib/_umodsi3.S
+	$(AS) $(ASFLAGS) $(BOOTSTRAP_PATH)/lib/_umodsi3.S -o _umodsi3.o
+
+clean:
+	$(ERASE_FCT) *.o *.bin *.elf *.map
diff --git a/board/usb_a9g20_lpw/nandflash/usb-a9g20-lpw.h b/board/usb_a9g20_lpw/nandflash/usb-a9g20-lpw.h
new file mode 100644
index 0000000..c0bdc6e
--- /dev/null
+++ b/board/usb_a9g20_lpw/nandflash/usb-a9g20-lpw.h
@@ -0,0 +1,112 @@
+/* ----------------------------------------------------------------------------
+ *         ATMEL Microcontroller Software Support  -  ROUSSET  -
+ * ----------------------------------------------------------------------------
+ * Copyright (c) 2008, Atmel Corporation
+
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the disclaimer below.
+ *
+ * Atmel's name may not be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
+ * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * ----------------------------------------------------------------------------
+ * File Name           : usb-a9g20-lpw.h
+ * Object              :
+ * Creation            : GH July 6th 2012
+ *-----------------------------------------------------------------------------
+ */
+#ifndef _USB_A9G20_LPW_H
+#define _USB_A9G20_LPW_H
+
+/* ******************************************************************* */
+/* PMC Settings                                                        */
+/*                                                                     */
+/* The main oscillator is enabled as soon as possible in the c_startup */
+/* and MCK is switched on the main oscillator.                         */
+/* PLL initialization is done later in the hw_init() function          */
+/* ******************************************************************* */
+#define MASTER_CLOCK		(133000000)
+#define PLL_LOCK_TIMEOUT	1000000
+
+/* Set PLLA to 798Mhz */
+#define PLLA_SETTINGS	0x20843F02
+#define PLLB_SETTINGS	0x100F3F02
+
+/* Switch MCK on PLLA output PCK = PLLA/2 = 3 * MCK */
+#define MCKR_SETTINGS		0x1300
+#define MCKR_CSS_SETTINGS	(AT91C_PMC_CSS_PLLA_CLK | MCKR_SETTINGS)
+
+/* ******************************************************************* */
+/* NandFlash Settings                                                  */
+/*                                                                     */
+/* ******************************************************************* */
+#define AT91C_SMARTMEDIA_BASE	0x40000000
+
+#define AT91_SMART_MEDIA_ALE    (1 << 21)	/* our ALE is AD21 */
+#define AT91_SMART_MEDIA_CLE    (1 << 22)	/* our CLE is AD22 */
+
+#define NAND_DISABLE_CE() do { *(volatile unsigned int *)AT91C_PIOC_SODR = AT91C_PIO_PC14;} while(0)
+#define NAND_ENABLE_CE() do { *(volatile unsigned int *)AT91C_PIOC_CODR = AT91C_PIO_PC14;} while(0)
+
+#define NAND_WAIT_READY() while (!(*(volatile unsigned int *)AT91C_PIOC_PDSR & AT91C_PIO_PC13))
+
+
+/* ******************************************************************** */
+/* SMC Chip Select 3 Timings for NandFlash for MASTER_CLOCK = 133000000.*/
+/* Please refer to SMC section in AT91SAM9 datasheet to learn how 	*/
+/* to generate these values. 						*/
+/* ******************************************************************** */
+#define AT91C_SM_NWE_SETUP	(2 << 0)
+#define AT91C_SM_NCS_WR_SETUP	(0 << 8)
+#define AT91C_SM_NRD_SETUP	(2 << 16)
+#define AT91C_SM_NCS_RD_SETUP	(0 << 24)
+  
+#define AT91C_SM_NWE_PULSE 	(4 << 0)
+#define AT91C_SM_NCS_WR_PULSE	(4 << 8)
+#define AT91C_SM_NRD_PULSE	(4 << 16)
+#define AT91C_SM_NCS_RD_PULSE	(4 << 24)
+  
+#define AT91C_SM_NWE_CYCLE 	(7 << 0)
+#define AT91C_SM_NRD_CYCLE	(7 << 16)
+
+#define AT91C_SM_TDF	        (3 << 16)
+
+/* ******************************************************************* */
+/* BootStrap Settings                                                  */
+/*                                                                     */
+/* ******************************************************************* */
+#define IMG_ADDRESS 		0x20000			/* Image Address in NandFlash */
+#define	IMG_SIZE		0x40000			/* Image Size in NandFlash */
+
+#define MACH_TYPE		0x731			/* USB-A9G20 */
+#define JUMP_ADDR		0x23F00000		/* Final Jump Address */
+
+/* ******************************************************************* */
+/* Application Settings                                                */
+/* ******************************************************************* */
+#undef CFG_DEBUG
+#undef CFG_DATAFLASH
+
+#define CFG_NANDFLASH
+#undef	NANDFLASH_SMALL_BLOCKS	/* NANDFLASH_LARGE_BLOCKS used instead */
+#undef  CFG_NANDFLASH_RECOVERY
+
+#define	CFG_SDRAM
+#define	CFG_HW_INIT
+
+#endif	/* _USB_A9G20_LPW_H */
diff --git a/board/usb_a9g20_lpw/usb_a9g20_lpw.c b/board/usb_a9g20_lpw/usb_a9g20_lpw.c
new file mode 100644
index 0000000..c372307
--- /dev/null
+++ b/board/usb_a9g20_lpw/usb_a9g20_lpw.c
@@ -0,0 +1,303 @@
+/* ----------------------------------------------------------------------------
+ *         ATMEL Microcontroller Software Support  -  ROUSSET  -
+ * ----------------------------------------------------------------------------
+ * Copyright (c) 2008, Atmel Corporation
+
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the disclaimer below.
+ *
+ * Atmel's name may not be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
+ * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * ----------------------------------------------------------------------------
+ * File Name           : usb_a9g20_lpw.c
+ * Object              :
+ * Creation            : GH July 6th 2012
+ *-----------------------------------------------------------------------------
+ */
+#include "../../include/part.h"
+#include "../../include/gpio.h"
+#include "../../include/pmc.h"
+#include "../../include/debug.h"
+#include "../../include/sdramc.h"
+#include "../../include/main.h"
+#ifdef CFG_NANDFLASH
+#include "../../include/nandflash.h"
+#endif
+#ifdef CFG_DATAFLASH
+#include "../../include/dataflash.h"
+#endif
+
+static inline unsigned int get_cp15(void)
+{
+	unsigned int value;
+	__asm__("mrc p15, 0, %0, c1, c0, 0" : "=r" (value));
+	return value;
+}
+
+static inline void set_cp15(unsigned int value)
+{
+	__asm__("mcr p15, 0, %0, c1, c0, 0" : : "r" (value));
+}
+
+#ifdef CFG_HW_INIT
+/*----------------------------------------------------------------------------*/
+/* \fn    hw_init							      */
+/* \brief This function performs very low level HW initialization	      */
+/* This function is invoked as soon as possible during the c_startup	      */
+/* The bss segment must be initialized					      */
+/*----------------------------------------------------------------------------*/
+void hw_init(void)
+{
+	unsigned int cp15;
+	
+	/* Configure PIOs */
+	const struct pio_desc hw_pio[] = {
+#ifdef CFG_DEBUG
+		{"RXD", AT91C_PIN_PB(14), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"TXD", AT91C_PIN_PB(15), 0, PIO_DEFAULT, PIO_PERIPH_A},
+#endif
+		{(char *) 0, 0, 0, PIO_DEFAULT, PIO_PERIPH_A},
+	};
+
+	/* Disable watchdog */
+	writel(AT91C_WDTC_WDDIS, AT91C_BASE_WDTC + WDTC_WDMR);
+
+	/* At this stage the main oscillator is supposed to be enabled
+	 * PCK = MCK = MOSC */
+	writel(0x00, AT91C_BASE_PMC + PMC_PLLICPR);
+
+	/* Configure PLLA = MOSC * (PLL_MULA + 1) / PLL_DIVA */
+	pmc_cfg_plla(PLLA_SETTINGS, PLL_LOCK_TIMEOUT);
+
+	/* PCK = PLLA/2 = 3 * MCK */
+	pmc_cfg_mck(MCKR_SETTINGS, PLL_LOCK_TIMEOUT);
+	/* Switch MCK on PLLA output */
+	pmc_cfg_mck(MCKR_CSS_SETTINGS, PLL_LOCK_TIMEOUT);
+
+	/* Configure PLLB */
+	pmc_cfg_pllb(PLLB_SETTINGS, PLL_LOCK_TIMEOUT);
+
+	/* Configure CP15 */
+	cp15 = get_cp15();
+	cp15 |= I_CACHE;
+	set_cp15(cp15);
+
+	/* Configure the PIO controller */
+	pio_setup(hw_pio);
+
+	/* Configure the EBI Slave Slot Cycle to 64 */
+	writel((readl((AT91C_BASE_MATRIX + MATRIX_SCFG3)) & ~0xFF) | 0x40, (AT91C_BASE_MATRIX + MATRIX_SCFG3));
+
+#ifdef CFG_DEBUG
+	/* Enable Debug messages on the DBGU */
+	dbg_init(BAUDRATE(MASTER_CLOCK, 115200));
+
+	dbg_print("Start AT91Bootstrap...\n\r");
+#endif /* CFG_DEBUG */
+
+#ifdef CFG_SDRAM
+	/* Initialize the matrix (VDDIOSEL=0: memory voltage = 1.8V ) */
+	writel((readl(AT91C_BASE_CCFG + CCFG_EBICSA) & ~0x00010000) | AT91C_EBI_CS1A_SDRAMC , AT91C_BASE_CCFG + CCFG_EBICSA);
+
+	/* Configure SDRAM Controller */
+	sdram_init(	AT91C_SDRAMC_NC_9  |
+				AT91C_SDRAMC_NR_13 |
+				AT91C_SDRAMC_CAS_3 |
+				AT91C_SDRAMC_NB_4_BANKS |
+				AT91C_SDRAMC_DBW_32_BITS |
+				AT91C_SDRAMC_TWR_3 |
+				AT91C_SDRAMC_TRC_9 |
+				AT91C_SDRAMC_TRP_3 |
+				AT91C_SDRAMC_TRCD_3 |
+				AT91C_SDRAMC_TRAS_6 |
+				AT91C_SDRAMC_TXSR_10,		/* Control Register */
+				(MASTER_CLOCK * 7)/1000000,	/* Refresh Timer Register */
+                                AT91C_SDRAMC_MD_SDRAM);         /* SDRAM (no low power)   */
+
+#endif /* CFG_SDRAM */
+}
+#endif /* CFG_HW_INIT */
+
+#ifdef CFG_SDRAM
+/*------------------------------------------------------------------------------*/
+/* \fn    sdramc_hw_init							*/
+/* \brief This function performs SDRAMC HW initialization			*/
+/*------------------------------------------------------------------------------*/
+void sdramc_hw_init(void)
+{
+	/* Configure PIOs */
+/*	const struct pio_desc sdramc_pio[] = {
+		{"D16", AT91C_PIN_PC(16), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D17", AT91C_PIN_PC(17), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D18", AT91C_PIN_PC(18), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D19", AT91C_PIN_PC(19), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D20", AT91C_PIN_PC(20), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D21", AT91C_PIN_PC(21), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D22", AT91C_PIN_PC(22), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D23", AT91C_PIN_PC(23), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D24", AT91C_PIN_PC(24), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D25", AT91C_PIN_PC(25), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D26", AT91C_PIN_PC(26), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D27", AT91C_PIN_PC(27), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D28", AT91C_PIN_PC(28), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D29", AT91C_PIN_PC(29), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D30", AT91C_PIN_PC(30), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"D31", AT91C_PIN_PC(31), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{(char *) 0, 0, 0, PIO_DEFAULT, PIO_PERIPH_A},
+	};
+*/
+	/* Configure the SDRAMC PIO controller to output PCK0 */
+/*	pio_setup(sdramc_pio); */
+
+	writel(0xFFFF0000, AT91C_BASE_PIOC + PIO_ASR(0));
+	writel(0xFFFF0000, AT91C_BASE_PIOC + PIO_PDR(0));
+
+}
+#endif /* CFG_SDRAM */
+
+#ifdef CFG_DATAFLASH
+
+/*------------------------------------------------------------------------------*/
+/* \fn    df_recovery								*/
+/* \brief This function erases DataFlash Page 0 if USR PB is pressed 		*/
+/*        during boot sequence							*/
+/*------------------------------------------------------------------------------*/
+void df_recovery(AT91PS_DF pDf)
+{
+#if (AT91C_SPI_PCS_DATAFLASH == AT91C_SPI_PCS1_DATAFLASH)
+	/* Configure PIOs */
+	const struct pio_desc usrpb[] = {
+		{"USRPB", AT91C_PIN_PB(10), 0, PIO_PULLUP, PIO_INPUT},
+		{(char *) 0, 0, 0, PIO_DEFAULT, PIO_PERIPH_A},
+	};
+
+	/* Configure the PIO controller */
+	writel((1 << AT91C_ID_PIOB), PMC_PCER + AT91C_BASE_PMC);
+	pio_setup(usrpb);
+	
+	/* If USR PB is pressed during Boot sequence */
+	/* Erase NandFlash block 0*/
+	if ( !pio_get_value(AT91C_PIN_PB(10)) )
+		df_page_erase(pDf, 0);
+#endif
+}
+
+/*------------------------------------------------------------------------------*/
+/* \fn    df_hw_init								*/
+/* \brief This function performs DataFlash HW initialization			*/
+/*------------------------------------------------------------------------------*/
+void df_hw_init(void)
+{
+	/* Configure PIOs */
+	const struct pio_desc df_pio[] = {
+		{"MISO",  AT91C_PIN_PA(0), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"MOSI",  AT91C_PIN_PA(1), 0, PIO_DEFAULT, PIO_PERIPH_A},
+		{"SPCK",  AT91C_PIN_PA(2), 0, PIO_DEFAULT, PIO_PERIPH_A},
+#if (AT91C_SPI_PCS_DATAFLASH == AT91C_SPI_PCS0_DATAFLASH)
+		{"NPCS0", AT91C_PIN_PA(3), 0, PIO_DEFAULT, PIO_PERIPH_A},
+#endif
+#if (AT91C_SPI_PCS_DATAFLASH == AT91C_SPI_PCS1_DATAFLASH)
+		{"NPCS1", AT91C_PIN_PC(11), 0, PIO_DEFAULT, PIO_PERIPH_B},
+#endif
+		{(char *) 0, 0, 0, PIO_DEFAULT, PIO_PERIPH_A},
+	};
+
+	/* Configure the PIO controller */
+	pio_setup(df_pio);
+}
+#endif /* CFG_DATAFLASH */
+
+
+
+#ifdef CFG_NANDFLASH
+/*------------------------------------------------------------------------------*/
+/* \fn    nand_recovery						*/
+/* \brief This function erases NandFlash Block 0 if USER PB is pressed 		*/
+/*        during boot sequence							*/
+/*------------------------------------------------------------------------------*/
+#ifdef CFG_NANDFLASH_RECOVERY
+static void nand_recovery(void)
+{
+	/* Configure PIOs */
+	const struct pio_desc usrpb[] = {
+		{"USRPB", AT91C_PIN_PB(10), 0, PIO_PULLUP, PIO_INPUT},
+		{(char *) 0, 0, 0, PIO_DEFAULT, PIO_PERIPH_A},
+	};
+
+	/* Configure the PIO controller */
+	writel((1 << AT91C_ID_PIOB), PMC_PCER + AT91C_BASE_PMC);
+	pio_setup(usrpb);
+	
+	/* If USER PB is pressed during Boot sequence */
+	/* Erase NandFlash block 0*/
+	if (!pio_get_value(AT91C_PIN_PB(10)) )
+		AT91F_NandEraseBlock0();
+}
+#else
+static void nand_recovery(void) {}
+#endif
+/*------------------------------------------------------------------------------*/
+/* \fn    nandflash_hw_init							*/
+/* \brief NandFlash HW init							*/
+/*------------------------------------------------------------------------------*/
+void nandflash_hw_init(void)
+{
+	/* Configure PIOs */
+	const struct pio_desc nand_pio[] = {
+		{"RDY_BSY", AT91C_PIN_PC(13), 0, PIO_PULLUP, PIO_INPUT},
+		{"NANDCS",  AT91C_PIN_PC(14), 0, PIO_PULLUP, PIO_OUTPUT},
+		{(char *) 0, 0, 0,  PIO_DEFAULT, PIO_PERIPH_A},
+	};
+
+	/* Setup Smart Media, first enable the address range of CS3 in HMATRIX user interface */
+	writel(readl(AT91C_BASE_CCFG + CCFG_EBICSA) | AT91C_EBI_CS3A_SM, AT91C_BASE_CCFG + CCFG_EBICSA);
+		    
+	/* Configure SMC CS3 */
+ 	writel((AT91C_SM_NWE_SETUP | AT91C_SM_NCS_WR_SETUP | AT91C_SM_NRD_SETUP | AT91C_SM_NCS_RD_SETUP), AT91C_BASE_SMC + SMC_SETUP3);
+  	writel((AT91C_SM_NWE_PULSE | AT91C_SM_NCS_WR_PULSE | AT91C_SM_NRD_PULSE | AT91C_SM_NCS_RD_PULSE), AT91C_BASE_SMC + SMC_PULSE3);
+	writel((AT91C_SM_NWE_CYCLE | AT91C_SM_NRD_CYCLE)						, AT91C_BASE_SMC + SMC_CYCLE3);
+	writel((AT91C_SMC_READMODE | AT91C_SMC_WRITEMODE | AT91C_SMC_NWAITM_NWAIT_DISABLE | 
+  		AT91C_SMC_DBW_WIDTH_SIXTEEN_BITS | AT91C_SM_TDF)						, AT91C_BASE_SMC + SMC_CTRL3);
+
+	/* Configure the PIO controller */
+	writel((1 << AT91C_ID_PIOC), PMC_PCER + AT91C_BASE_PMC);
+	pio_setup(nand_pio);
+	
+	nand_recovery();
+}
+
+/*------------------------------------------------------------------------------*/
+/* \fn    nandflash_cfg_16bits_dbw_init						*/
+/* \brief Configure SMC in 16 bits mode						*/
+/*------------------------------------------------------------------------------*/
+void nandflash_cfg_16bits_dbw_init(void)
+{
+	writel(readl(AT91C_BASE_SMC + SMC_CTRL3) | AT91C_SMC_DBW_WIDTH_SIXTEEN_BITS, AT91C_BASE_SMC + SMC_CTRL3);
+}
+
+/*------------------------------------------------------------------------------*/
+/* \fn    nandflash_cfg_8bits_dbw_init						*/
+/* \brief Configure SMC in 8 bits mode						*/
+/*------------------------------------------------------------------------------*/
+void nandflash_cfg_8bits_dbw_init(void)
+{
+	writel((readl(AT91C_BASE_SMC + SMC_CTRL3) & ~(AT91C_SMC_DBW)) | AT91C_SMC_DBW_WIDTH_EIGTH_BITS, AT91C_BASE_SMC + SMC_CTRL3);
+}
+
+#endif /* #ifdef CFG_NANDFLASH */
diff --git a/crt0_gnu.S b/crt0_gnu.S
index 042b617..002feef 100644
--- a/crt0_gnu.S
+++ b/crt0_gnu.S
@@ -106,6 +106,13 @@ _relocate_to_sram:
 #endif /* CFG_NORFLASH */
 
 _setup_clocks:
+/* Test if main osc is bypassed */
+	ldr 	r0,=AT91C_PMC_MOR
+	ldr 	r1, [r0]
+	ldr 	r2,=AT91C_CKGR_OSCBYPASS 
+	ands 	r1, r1, r2
+	bne	_init_data	/* branch if OSCBYPASS=1 */
+
 /* Test if main oscillator is enabled */
 	ldr 	r0,=AT91C_PMC_SR
 	ldr 	r1, [r0]
diff --git a/include/part.h b/include/part.h
index ba5985a..1d7392a 100644
--- a/include/part.h
+++ b/include/part.h
@@ -46,7 +46,11 @@
 
 #ifdef AT91SAM9G20
 #include "AT91SAM9260_inc.h"
-#include "at91sam9g20ek.h"
+	#ifdef at91sam9g20ek
+	#include "at91sam9g20ek.h"
+	#elif usb_a9g20_lpw
+	#include "usb-a9g20-lpw.h"
+	#endif
 #endif
 
 #ifdef AT91SAM9261
-- 
1.5.6.3