2 files changed, 34 insertions, 13 deletions

diff --git a/main.c b/main.c
index 2ad2e86..bd2d5e1 100644
--- a/main.c
+++ b/main.c
@@ -9,13 +9,20 @@
 #define PIO_OER  *((volatile unsigned int *)(PORT + 0x0010u))
 #define PIO_SODR *((volatile unsigned int *)(PORT + 0x0030u))
 #define PIO_CODR *((volatile unsigned int *)(PORT + 0x0034u))
-#define PIO_PUDR *((volatile unsigned int *)(PORT + 0x0060u))
 #define PIO_WPMR *((volatile unsigned int *)(PORT + 0x00E4u))
 
 #define PIO_WPKEY 0x50494Fu
 
 #define LED_PIN 1
 
+void wait(int t)
+{
+	volatile int i;
+
+	for (i = t; i > 0; i--)
+		__asm("nop");
+}
+
 int main(void)
 {
 	volatile int i;
@@ -23,25 +30,35 @@ int main(void)
 	PIO_WPMR = PIO_WPKEY << 8;
 	PIO_PER  |= (1 << LED_PIN);
 	PIO_OER  |= (1 << LED_PIN);
-	PIO_PUDR |= (1 << LED_PIN);
-	PIO_WPMR = (PIO_WPKEY << 8) | 1u;
+	PIO_WPMR = (PIO_WPKEY << 8) | 1;
 	
 	for (;;) {
 		PIO_SODR |= (1 << LED_PIN);
-		for (i = 0; i < 100000; i++)
-			;		
+		wait(200000);
 		PIO_CODR |= (1 << LED_PIN);
-		for (i = 0; i < 100000; i++)
-			;		
+		wait(200000);
 	}		
 
 	return 0;
 }
 
-__attribute__ ((noreturn)) void reset_handler(void)
+static inline void mem_init(void)
 {
+	unsigned long *dst, *src;
+	extern unsigned long _sbss, _ebss, _sdata, _edata, _sidata;
+
+	for (dst = &_sbss; dst < &_ebss; dst++)
+		*dst = 0;
+
+	for (dst = &_sdata, src = &_sidata; dst < &_edata;)
+		*dst++ = *src++;
+}
+
+__attribute__((noreturn)) void reset(void) {
+	mem_init();
 	main();
-	for(;;)
+
+	for (;;)
 		;
 }
 
@@ -49,5 +66,5 @@ extern const unsigned int sp;
 
 __attribute__ ((section(".vtor"))) const void* tab[] = {
     &sp,
-    reset_handler
+    reset
 };
diff --git a/script.ld b/script.ld
index a063ffe..64492fc 100644
--- a/script.ld
+++ b/script.ld
@@ -17,14 +17,18 @@ SECTIONS
 
 	.data :
 	{
+		_sdata = . ;
 		*(.data*)
-	} > ram AT >rom
+		_edata = . ;
+	} > ram AT > rom
+
+	_sidata = LOADADDR(.data);
 
 	.bss (NOLOAD) :
 	{
-		__bss_start__ = . ;
+		_sbss = . ;
 		*(.bss*)
 		*(COMMON)
-		__bss_end__ = . ;
+		_ebss = . ;
 	} > ram
 }