Start working at dfu by copying from main openpcd repository (with modifications), doesn't boot application yet

git-svn-id: https://svn.openpcd.org:2342/trunk@315 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

3 files changed, 706 insertions, 39 deletions

diff --git a/openpicc/os/boot/Cstartup.S b/openpicc/os/boot/Cstartup.S
new file mode 100644
index 0000000..83e7696
--- /dev/null
+++ b/openpicc/os/boot/Cstartup.S
@@ -0,0 +1,450 @@
+/* AT91SAM7 low-level startup outines for OpenPCD / OpenPICC DFU loader
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  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
+ *
+ */
+
+/*------------------------------------------------------------------------------
+//*-         ATMEL Microcontroller Software Support  -  ROUSSET  -
+//*------------------------------------------------------------------------------
+//* The software is delivered "AS IS" without warranty or condition of any
+//* kind, either express, implied or statutory. This includes without
+//* limitation any warranty or condition with respect to merchantability or
+//* fitness for any particular purpose, or against the infringements of
+//* intellectual property rights of others.
+//*-----------------------------------------------------------------------------
+//*- File source          : Cstartup.s
+//*- Object               : Generic CStartup for KEIL and GCC No Use REMAP
+//*- Compilation flag     : None
+//*-
+//*- 1.0 18/Oct/04 JPP    : Creation
+//*- 1.1 21/Feb/05 JPP    : Set Interrupt
+//*- 1.1 01/Apr/05 JPP    : save SPSR 
+//*-----------------------------------------------------------------------------*/
+
+/* Enable DFU by press of hardware POI_BOOTLDR switch */
+#define CONFIG_DFU_SWITCH
+
+/* Enable DFU by magic value in RAM and software reset */
+#define CONFIG_DFU_MAGIC
+
+//#define DEBUG_LL
+
+#define PIOA_PER	0xFFFFF400
+#define PIOA_OER	0xFFFFF410
+#define PIOA_SODR	0xFFFFF430
+#define PIOA_CODR	0xFFFFF434
+#define LED1		25		/* this only works on OpenPICC, not Olimex */
+
+#ifdef DEBUG_LL
+/* Debugging macros for switching on/off LED1 (green) */
+	.macro	led1on
+		ldr	r2, =PIOA_CODR
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+	.endm
+	.macro	led1off
+		ldr	r2, =PIOA_SODR
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+	.endm
+	.macro	ledinit
+		ldr	r2, =PIOA_PER
+		mov	r1, #(1 << LED1)
+		str	r1, [r2]
+		ldr	r2, =PIOA_OER
+		str	r1, [r2]
+		led1off
+	.endm
+#else
+	.macro ledinit
+	.endm
+	.macro led1on
+	.endm
+	.macro led1off
+	.endm
+#endif
+
+     .equ   IRQ_Stack_Size,     0x00000400
+     .equ   FIQ_Stack_Size,     0x00000400
+
+	.equ AIC_IVR,         (256) 
+	.equ AIC_FVR,         (260) 
+	.equ AIC_EOICR,       (304)
+	.equ AIC_MCR_RCR,     (0xf00)
+	.equ AT91C_BASE_AIC,  (0xFFFFF000)
+	.equ AT91C_PMC_PCER,  (0xFFFFFC10)
+	.equ AT91C_BASE_PIOA, (0xFFFFF400)
+	.equ AT91C_ID_PIOA,   (2)
+	.equ PIOA_PDSR,	      (0x3c)
+#if defined(PCD)
+	.equ PIO_BOOTLDR,     (1 << 27)
+#elif defined(PICC)
+	.equ PIO_BOOTLDR,     (1 << 6)
+#elif defined(OLIMEX)
+	.equ PIO_BOOTLDR,     (1 << 19)
+#else
+#error please define PIO_BOOTLDR
+#endif
+
+
+/* #include "AT91SAM7S64_inc.h"	  */
+
+/* Exception Vectors in RAM */
+
+	.text
+	.arm
+	.section	.vectram, "ax"
+
+	.global _remap_call_dfu
+	.func	_remap_call_dfu
+_remap_call_dfu:
+		led1on
+		/* Remap RAM to 0x00000000 for DFU */
+		ldr	r1, =AT91C_BASE_AIC
+		mov	r2, #0x01
+		str	r2, [r1, #AIC_MCR_RCR]
+		
+		ldr	r4, =dfu_main
+		bx	r4
+
+	.size	_remap_call_dfu, . - _remap_call_dfu
+	.endfunc
+
+
+#;------------------------------------------------------------------------------
+#;- Section Definition
+#;-----------------
+#;-  Section    
+#;- .internal_ram_top 	Top_Stack: used by the cstartup for vector initalisation 
+#;-			management defined by ld and affect from ldscript 
+#;------------------------------------------------------------------------------
+	.section 	.internal_ram_top
+	.code 32
+	.align 	0
+	.global	Top_Stack
+Top_Stack:
+	
+/*------------------------------------------------------------------------------
+*- Area Definition
+*------------------------------------------------------------------------------
+* .text is used instead of .section .text so it works with arm-aout too.  */
+        .section 	.reset
+        .text
+reset: 
+/*------------------------------------------------------------------------------
+//*- Exception vectors 
+//*--------------------
+//*- These vectors can be read at address 0 or at RAM address
+//*- They ABSOLUTELY requires to be in relative addresssing mode in order to
+//*- guarantee a valid jump. For the moment, all are just looping.
+//*- If an exception occurs before remap, this would result in an infinite loop.
+//*- To ensure if a exeption occurs before start application to infinite loop.
+//*------------------------------------------------------------------------------*/
+
+                B           InitReset           /* 0x00 Reset handler */
+undefvec:
+                B           undefvec            /* 0x04 Undefined Instruction */
+swivec:
+                B           swivec              /* 0x08 Software Interrupt */
+pabtvec:
+                B           pabtvec             /* 0x0C Prefetch Abort */
+dabtvec:
+		b	dabtvec             /* 0x10 Data Abort */
+rsvdvec:
+		b	rsvdvec             /* 0x14 reserved  */
+irqvec:
+		b	IRQ_Handler_Entry	/* 0x18 IRQ */
+fiqvec:
+		ldr	pc, [pc, #-0xF20]	 /* 0x1c FIQ */
+
+dfu_state_dummy:
+		.word	0
+
+	.global IRQ_Handler_Entry
+	.func   IRQ_Handler_Entry
+		
+FIQ_Handler_Entry:
+
+/*- Switch in SVC/User Mode to allow User Stack access for C code 	*/
+/* because the FIQ is not yet acknowledged*/
+
+/*- Save and r0 in FIQ_Register */
+		mov	r9, r0
+		ldr	r0, [r8, #AIC_FVR]
+		msr	CPSR_c, #I_BIT | F_BIT | ARM_MODE_SVC
+
+		/*- Save scratch/used registers and LR in User Stack */
+		stmfd	sp!, { r1-r3, r12, lr}
+
+		/*- Branch to the routine pointed by the AIC_FVR */
+		mov	r14, pc
+		bx	r0
+
+		/*- Restore scratch/used registers and LR from User Stack */
+		ldmia	sp!, { r1-r3, r12, lr}
+
+		/*- Leave Interrupts disabled and switch back in FIQ mode */
+		msr	CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ
+
+		/*- Restore the R0 ARM_MODE_SVC register */
+		mov	r0,r9
+
+		/*- Restore the Program Counter using the LR_fiq directly in the PC */
+		subs        pc, lr, #4
+
+IRQ_Handler_Entry:
+
+		/*- Manage Exception Entry  */
+		/*- Adjust and save LR_irq in IRQ stack  */
+		sub         lr, lr, #4
+		stmfd       sp!, {lr}
+
+		/*- Save SPSR need to be saved for nested interrupt */
+		mrs	r14, SPSR
+		stmfd	sp!, {r14}
+
+		/*- Save and r0 in IRQ stack  */
+		stmfd	sp!, {r0}
+
+		/*- Write in the IVR to support Protect Mode  */
+		/*- No effect in Normal Mode  */
+		/*- De-assert the NIRQ and clear the source in Protect Mode */
+		ldr	r14, =AT91C_BASE_AIC
+		ldr	r0 , [r14, #AIC_IVR]
+		str	r14, [r14, #AIC_IVR]
+
+		/*- Enable Interrupt and Switch in Supervisor Mode */
+		msr	CPSR_c, #ARM_MODE_SVC
+
+		/*- Save scratch/used registers and LR in User Stack */
+		stmfd	sp!, { r1-r3, r12, r14}
+
+		/*- Branch to the routine pointed by the AIC_IVR  */
+		mov	r14, pc
+		bx	r0
+
+		/*- Restore scratch/used registers and LR from User Stack*/
+		ldmia	sp!, { r1-r3, r12, r14}
+
+		/*- Disable Interrupt and switch back in IRQ mode */
+		msr	CPSR_c, #I_BIT | ARM_MODE_IRQ
+
+		/*- Mark the End of Interrupt on the AIC */
+		ldr	r14, =AT91C_BASE_AIC
+		str	r14, [r14, #AIC_EOICR]
+
+		/*- Restore SPSR_irq and r0 from IRQ stack */
+		ldmia	sp!, {r0}
+
+		/*- Restore SPSR_irq and r0 from IRQ stack */
+		ldmia	sp!, {r14}
+		msr	SPSR_cxsf, r14
+
+		/*- Restore adjusted  LR_irq from IRQ stack directly in the PC */
+		ldmia	sp!, {pc}^
+
+	.size   IRQ_Handler_Entry, . - IRQ_Handler_Entry
+	.endfunc
+	.align 0
+.RAM_TOP:
+	.word	Top_Stack
+
+	.global _startup
+	.func _startup
+InitReset:
+/*------------------------------------------------------------------------------
+/*- Low level Init (PMC, AIC, ? ....) by C function AT91F_LowLevelInit
+/*------------------------------------------------------------------------------*/
+            	.extern   AT91F_LowLevelInit
+/*- minumum C initialization */
+/*- call  AT91F_LowLevelInit( void) */
+
+	ldr	r13,.RAM_TOP            /* temporary stack in internal RAM */
+/*--Call Low level init function in ABSOLUTE through the Interworking	*/
+        ldr	r0,=AT91F_LowLevelInit
+	mov	lr, pc
+	bx	r0
+	ledinit
+
+/*------------------------------------------------------------------------------
+//*- Top of Stack Definition
+//*-------------------------
+//*- Interrupt and Supervisor Stack are located at the top of internal memory in 
+//*- order to speed the exception handling context saving and restoring.
+//*- ARM_MODE_SVC (Application, C) Stack is located at the top of the external memory.
+//*------------------------------------------------------------------------------*/
+
+          .EQU		ARM_MODE_FIQ,       0x11
+          .EQU		ARM_MODE_IRQ,       0x12
+          .EQU		ARM_MODE_SVC,       0x13
+
+          .EQU		I_BIT,              0x80
+          .EQU		F_BIT,              0x40
+
+
+#define AT91C_RSTC_RSR			0xFFFFFD04
+#define AT91C_RSTC_RSTTYP_SOFTWARE	(0x03 << 8)
+#define DFU_STATE_appDETACH		1
+
+
+/*------------------------------------------------------------------------------
+//*- Setup the stack for each mode
+//*-------------------------------*/
+                mov     r0,r13
+
+/*- Set up Fast Interrupt Mode and set FIQ Mode Stack*/
+                msr     CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
+		mov	r13, r0
+		sub	r0, r0, #FIQ_Stack_Size
+
+/*- Init the FIQ register*/
+            	ldr     r8, =AT91C_BASE_AIC
+
+/*- Set up Interrupt Mode and set IRQ Mode Stack*/
+                msr     CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT
+                mov     r13, r0                     /* Init stack IRQ */
+                sub     r0, r0, #IRQ_Stack_Size
+
+/*- Set up Supervisor Mode and set Supervisor Mode Stack*/
+                msr     CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
+                mov     r13, r0                     /* Init stack Sup */
+
+/* - Enable Interrupts and FIQ */
+                msr     CPSR_c, #ARM_MODE_SVC
+
+#ifdef CONFIG_DFU_MAGIC
+		ldr	r1, =AT91C_RSTC_RSR
+		ldr	r2, [r1]
+		#and	r2, r2, AT91C_RSTC_RSTTYP
+		tst	r2, #AT91C_RSTC_RSTTYP_SOFTWARE
+		beq	dfu_magic_end
+
+		ldr	r1, =dfu_state
+		ldr	r2, [r1]
+		cmp	r2, #DFU_STATE_appDETACH
+		beq	_reloc_dfu
+dfu_magic_end:
+#endif
+
+# Relocate DFU .data.shared section (Copy from ROM to RAM)
+                LDR     R1, =_etext
+                LDR     R2, =_data_shared
+                LDR     R3, =_edata_shared
+LoopRelDS:      CMP     R2, R3
+                LDRLO   R0, [R1], #4
+                STRLO   R0, [R2], #4
+                BLO     LoopRelDS
+
+/*
+# Clear DFU .bss section (Zero init)
+                MOV     R0, #0
+                LDR     R1, =__bss_start__
+                LDR     R2, =__bss_end__
+LoopZI:         CMP     R1, R2
+                STRLO   R0, [R1], #4
+                BLO     LoopZI
+*/
+
+		/* prepare c function call to main */
+		mov	r0, #0	/* argc = 0 */
+		ldr	lr, =exit
+		ldr	r10, =0x00104000
+
+#ifdef CONFIG_DFU_SWITCH
+		/* check whether bootloader button is pressed */
+		ldr	r1, =AT91C_PMC_PCER
+		mov	r2, #(1 << AT91C_ID_PIOA)
+		str	r2, [r1] 
+
+		ldr 	r1, =AT91C_BASE_PIOA
+		ldr	r2, [r1, #PIOA_PDSR]
+		tst	r2, #PIO_BOOTLDR
+		bne	_reloc_dfu
+#endif
+
+		bx	r10
+
+_reloc_dfu:
+		/* Relocate DFU .data section (Copy from ROM to RAM) */
+                LDR     R1, =_data_flash
+                LDR     R2, =_data
+                LDR     R3, =_edata
+LoopRel:        CMP     R2, R3
+                LDRLO   R0, [R1], #4
+                STRLO   R0, [R2], #4
+                BLO     LoopRel
+
+		/* Clear DFU .bss section (Zero init) */
+                MOV     R0, #0
+                LDR     R1, =__bss_start__
+                LDR     R2, =__bss_end__
+LoopZI:         CMP     R1, R2
+                STRLO   R0, [R1], #4
+                BLO     LoopZI
+
+		/* relocate DFU .text into RAM */
+		ldr	r1, =0x00100000
+		ldr	r2, =0x00200000
+		ldr	r3, =_etext
+		add	r3, r3, r2
+loop_rel_t:	cmp	r2, r3
+		ldrlo	r4, [r1], #4
+		strlo	r4, [r2], #4
+		blo	loop_rel_t
+		ldr	r4, =_remap_call_dfu
+		bx	r4
+		
+        .size   _startup, . - _startup
+        .endfunc
+
+/* "exit" dummy to avoid sbrk write read etc. needed by the newlib default "exit" */
+        .global exit
+        .func   exit
+exit:
+        b    .
+		.size   exit, . - exit
+        .endfunc
+
+/*---------------------------------------------------------------
+//* ?EXEPTION_VECTOR
+//* This module is only linked if needed for closing files.
+//*---------------------------------------------------------------*/
+        .global AT91F_Default_FIQ_handler
+        .func   AT91F_Default_FIQ_handler
+AT91F_Default_FIQ_handler:
+            b     AT91F_Default_FIQ_handler
+        .size   AT91F_Default_FIQ_handler, . - AT91F_Default_FIQ_handler
+        .endfunc
+
+        .global AT91F_Default_IRQ_handler
+        .func   AT91F_Default_IRQ_handler
+AT91F_Default_IRQ_handler:
+            b     AT91F_Default_IRQ_handler
+        .size   AT91F_Default_IRQ_handler, . - AT91F_Default_IRQ_handler
+        .endfunc
+
+        .global AT91F_Spurious_handler
+        .func   AT91F_Spurious_handler
+AT91F_Spurious_handler:
+            b     AT91F_Spurious_handler
+        .size   AT91F_Spurious_handler, . - AT91F_Spurious_handler
+        .endfunc
+
+
+
+        .end
+
diff --git a/openpicc/os/boot/Cstartup_SAM7.c b/openpicc/os/boot/Cstartup_SAM7.c
index 450a95e..4ab263f 100644
--- a/openpicc/os/boot/Cstartup_SAM7.c
+++ b/openpicc/os/boot/Cstartup_SAM7.c
@@ -8,60 +8,83 @@
 //* intellectual property rights of others.
 //*----------------------------------------------------------------------------
 //* File Name           : Cstartup_SAM7.c
-//* Object              : Low level initializations written in C for IAR
-//*                       tools
-//* 1.0   08/Sep/04 JPP : Creation
-//* 1.10  10/Sep/04 JPP : Update AT91C_CKGR_PLLCOUNT filed
+//* Object              : Low level initializations written in C for GCC Tools
+//* Creation            : 12/Jun/04
+//* 1.2   28/Feb/05 JPP : LIB change AT91C_WDTC_WDDIS & PLL
+//* 1.3   21/Mar/05 JPP : Change PLL Wait time
 //*----------------------------------------------------------------------------
 
-
 // Include the board file description
 #include <board.h>
 
+// The following functions must be write in ARM mode this function called directly
+// by exception vector
+extern void AT91F_Spurious_handler (void);
+extern void AT91F_Default_IRQ_handler (void);
+extern void AT91F_Default_FIQ_handler (void);
+
 //*----------------------------------------------------------------------------
 //* \fn    AT91F_LowLevelInit
 //* \brief This function performs very low level HW initialization
 //*        this function can be use a Stack, depending the compilation
 //*        optimization mode
 //*----------------------------------------------------------------------------
-void AT91F_LowLevelInit (void)
+void
+AT91F_LowLevelInit (void)
 {
-    AT91PS_PMC pPMC = AT91C_BASE_PMC;
-
-    //* Set flash wait state
-    //  Single Cycle Access at Up to 30 MHz, or 40
-    //  if MCK = 47923200 I have 50 Cycle for 1 useconde ( flied MC_FMR->FMCN
-    AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN) & (75 << 16)) | AT91C_MC_FWS_1FWS;
+  volatile int i;
+  
+  //* Debounce power supply
+  for(i=0;i<1024;i++);
+  
+  AT91PS_PMC pPMC = AT91C_BASE_PMC;
+  //* Set Flash Waite sate
+  //  Single Cycle Access at Up to 30 MHz, or 40
+  //  if MCK = 47923200 I have 50 Cycle for 1 usecond ( flied MC_FMR->FMCN
+  AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN) & (48 << 16)) | AT91C_MC_FWS_1FWS;
 
-    //* Watchdog Enable
-    AT91C_BASE_WDTC->WDTC_WDMR = (0x80 << 16) | AT91C_WDTC_WDRSTEN | 0x80;
+  //* Set MCK at 47 923 200
+  // 1 Enabling the Main Oscillator:
+  // SCK = 1/32768 = 30.51 uSecond
+  // Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
+  pPMC->PMC_MOR = ((AT91C_CKGR_OSCOUNT) & (0x06 << 8)) | AT91C_CKGR_MOSCEN;
+  // Wait the startup time
+  while (!(pPMC->PMC_SR & AT91C_PMC_MOSCS));
+  // 2 Checking the Main Oscillator Frequency (Optional)
+  // 3 Setting PLL and divider:
+  // - div by 24 Fin = 0,7680 =(18,432 / 24)
+  // - Mul 125: Fout =   96,0000 =(0,7680 *125)
+  // for 96 MHz the erroe is 0.16%
+  // Field out NOT USED = 0
+  // PLLCOUNT pll startup time estimate at : 0.844 ms
+  // PLLCOUNT 28 = 0.000844 /(1/32768)
+#if 0
+  pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 0x05) |
+		    (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
+		    (AT91C_CKGR_MUL & (25 << 16)));
+#else
+  pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 24) |
+		    (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
+		    (AT91C_CKGR_MUL & (125 << 16)));
+#endif
 
-    //* Set MCK at 47 923 200
-    // 1 Enabling the Main Oscillator:
-    // SCK = 1/32768 = 30.51 uSeconde
-    // Start up time = 8 * 6 / SCK = 56 * 30.51 = 1,46484375 ms
-    pPMC->PMC_MOR = (((AT91C_CKGR_OSCOUNT & (0x06 << 8)) | AT91C_CKGR_MOSCEN));
-    // Wait the startup time
+  // Wait the startup time
+  while (!(pPMC->PMC_SR & AT91C_PMC_LOCK));
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
+  // 4. Selection of Master Clock and Processor Clock
+  // select the PLL clock divided by 2
+  pPMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2;
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
 
-    while (!(pPMC->PMC_SR & AT91C_PMC_MOSCS));
-    // 2 Checking the Main Oscillator Frequency (Optional)
-    // 3 Setting PLL and divider:
-    // - div by 5 Fin = 3,6864 =(18,432 / 5)
-    // - Mul 25+1: Fout =   95,8464 =(3,6864 *26)
-    // for 96 MHz the erroe is 0.16%
-    //eld out NOT USED = 0 Fi
-    pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 5) |
-		     (AT91C_CKGR_PLLCOUNT & (28 << 8)) |
-		     (AT91C_CKGR_MUL & (25 << 16)));
+  pPMC->PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK;
+  while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
 
-    // Wait the startup time
-    while (!(pPMC->PMC_SR & AT91C_PMC_LOCK));
-    
-    // 4. Selection of Master Clock and Processor Clock
-    // select the PLL clock divided by 2
-    pPMC->PMC_MCKR = AT91C_PMC_PRES_CLK_2;
-    while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
+  // Set up the default interrupts handler vectors
+  AT91C_BASE_AIC->AIC_SVR[0] = (int) AT91F_Default_FIQ_handler;
+  for (i = 1; i < 31; i++)
+    {
+      AT91C_BASE_AIC->AIC_SVR[i] = (int) AT91F_Default_IRQ_handler;
+    }
+  AT91C_BASE_AIC->AIC_SPU = (int) AT91F_Spurious_handler;
 
-    pPMC->PMC_MCKR |= AT91C_PMC_CSS_PLL_CLK;
-    while (!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));
 }
diff --git a/openpicc/os/boot/Cstartup_app.S b/openpicc/os/boot/Cstartup_app.S
new file mode 100644
index 0000000..448cc93
--- /dev/null
+++ b/openpicc/os/boot/Cstartup_app.S
@@ -0,0 +1,194 @@
+/* Cstartup header for the application to be started by at91dfu 
+ * (C) 2006 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  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
+ *
+ */
+
+
+//#define DEBUG_LL
+
+	.equ AIC_FVR,         (260) 
+	.equ AIC_EOICR,       (304)
+	.equ AT91C_BASE_AIC,  (0xFFFFF000)
+	.equ ARM_MODE_FIQ,       0x11
+	.equ ARM_MODE_IRQ,       0x12
+	.equ ARM_MODE_SVC,       0x13
+
+	.equ I_BIT,              0x80
+	.equ F_BIT,              0x40
+
+
+#define AT91C_BASE_PIOA	0xFFFFF400
+#define AT91C_BASE_TC0	0xFFFA0000
+#define AT91C_TC_SWTRG	(1 << 2)
+#define PIOA_SODR	0x30
+#define PIOA_CODR	0x34
+#define PIOA_PDSR	0x3c
+#define PIOA_ISR	0x4c
+#define PIOA_IDR	0x44
+#define PIO_DATA	(1 << 27)
+#define TC_CCR		0x00
+
+
+#define PIO_LED1	(1 << 25) /* this only works on OpenPICC, not Olimex */
+
+#ifdef DEBUG_LL
+/* Debugging macros for switching on/off LED1 (green) */
+#define PIOA_PER	0xFFFFF400
+#define PIOA_OER	0xFFFFF410
+	.macro	led1on
+		ldr	r2, =AT91C_BASE_PIOA
+		mov	r1, #PIO_LED1
+		str	r1, [r2, #PIOA_CODR]
+	.endm
+	.macro	led1off
+		ldr	r2, =AT91C_BASE_PIOA
+		mov	r1, #PIO_LED1
+		str	r1, [r2, #PIOA_SODR]
+	.endm
+	.macro	ledinit
+		ldr	r2, =PIOA_PER
+		mov	r1, #PIO_LED1
+		str	r1, [r2]
+		ldr	r2, =PIOA_OER
+		str	r1, [r2]
+		led1off
+	.endm
+#else
+	.macro ledinit
+	.endm
+	.macro led1on
+	.endm
+	.macro led1off
+	.endm
+#endif
+
+	.global	_startup
+	.func _startup
+_startup:
+	/* Relocate .data section (copy from Flash to RAM) */
+	ldr	r1, =_etext
+	ldr	r2, =_data
+	ldr	r3, =_edata
+loop_r:	cmp	r2, r3
+	ldrlo	r0, [r1], #4
+	strlo	r0, [r2], #4
+	blo	loop_r
+
+	/* Clear .bss section (Zero init) */
+	mov	r0, #0
+	ldr	r1, =__bss_start__
+	ldr	r2, =__bss_end__
+loop_z:	cmp	r1, r2
+	strlo	r0, [r1], #4
+	blo	loop_z
+
+	/* initialize FIQ mode registers */ 
+	msr	CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
+	ldr	r10, =AT91C_BASE_PIOA
+	ldr	r12, =AT91C_BASE_TC0
+	mov	r9, #AT91C_TC_SWTRG
+	msr	CPSR_c, #ARM_MODE_SVC
+
+	led1on
+
+	/* prepare C function call to main */
+	mov	r0, #0	/* argc = 0 */
+	ldr	lr, =exit
+	ldr	r10, =main
+
+	bx	r10
+
+        .size   _startup, . - _startup
+        .endfunc
+		
+/* "exit" dummy to avoid sbrk write read etc. needed by the newlib default "exit" */
+        .global exit
+        .func   exit
+exit:
+        b    .
+	.size   exit, . - exit
+        .endfunc
+
+
+#define LED_TRIGGER
+#define CALL_PIO_IRQ_DEMUX
+
+	.text
+	.arm
+	.section .fastrun, "ax"
+
+	.global fiq_handler
+	.func fiq_handler
+fiq_handler:
+		/* code that uses pre-initialized FIQ reg */
+		/* r8	AT91C_BASE_AIC (dfu init)
+		   r9	AT91C_TC_SWTRG
+		   r10	AT91C_BASE_PIOA
+		   r11	tmp
+		   r12	AT91C_BASE_TC0
+		   r13	stack
+		   r14	lr
+		 */
+
+		ldr	r8, [r10, #PIOA_ISR]
+		tst	r8, #PIO_DATA		/* check for PIO_DATA change */
+		ldrne	r11, [r10, #PIOA_PDSR]
+		tstne	r11, #PIO_DATA		/* check for PIO_DATA == 1 */
+		strne	r9, [r12, #TC_CCR]	/* software trigger */
+#ifdef LED_TRIGGER
+		movne 	r11, #PIO_LED1
+		strne	r11, [r10, #PIOA_CODR] /* enable LED */
+#endif
+
+#if 1
+		movne	r11, #PIO_DATA
+		strne	r11, [r10, #PIOA_IDR]	/* disable further PIO_DATA FIQ */
+#endif
+
+		/*- Mark the End of Interrupt on the AIC */
+		ldr	r11, =AT91C_BASE_AIC
+		str	r11, [r11, #AIC_EOICR]
+
+#ifdef LED_TRIGGER
+		mov 	r11, #PIO_LED1
+		str	r11, [r10, #PIOA_SODR] /* disable LED */
+#endif
+
+#ifdef CALL_PIO_IRQ_DEMUX
+		/* push r0, r1-r3, r12, r14 onto FIQ stack */
+		stmfd	sp!, { r0-r3, r12, lr}
+		mov	r0, r8
+
+		/* enable interrupts while handling demux */
+		/* msr	CPSR_c, #F_BIT | ARM_MODE_SVC */
+
+		/* Call C function, give PIOA_ISR as argument */
+		ldr	r11, =__pio_irq_demux
+		mov	r14, pc
+		bx	r11 
+
+		/* msr	CPSR_c, #I_BIT | F_BIT | ARM_MODE_FIQ */
+		ldmia	sp!, { r0-r3, r12, lr }
+#endif
+
+		/*- Restore the Program Counter using the LR_fiq directly in the PC */
+		subs        pc, lr, #4
+
+        .size   fiq_handler, . - fiq_handler
+        .endfunc
+        .end
+