Work on the MPU port. The exception VTOR redirection doesn't work as expected.

6 files changed, 97 insertions, 28 deletions

diff --git a/arch/Makefile b/arch/Makefile
index 3e57f0c..7dab0ab 100644
--- a/arch/Makefile
+++ b/arch/Makefile
@@ -11,7 +11,7 @@ ifeq ($(CPU),arm)
 ifeq ($(CPU_FLAVOR),lpc1768)
 TARGET_SRCS += arm/lpc17xx/Core/CM3/DeviceSupport/NXP/LPC17xx/system_LPC17xx.c arm/lpc17xx/Core/CM3/CoreSupport/core_cm3.c
 TARGET_SRCS += $(addprefix arm/lpc17xx/Drivers/source/lpc17xx_, spi.c rit.c exti.c wdt.c uart.c dac.c rtc.c i2s.c pwm.c mcpwm.c pinsel.c nvic.c emac.c systick.c ssp.c can.c gpio.c libcfg_default.c i2c.c timer.c gpdma.c clkpwr.c qei.c adc.c)
-TARGET_SRCS += arm/lpc17xx/startup.s arm/lpc17xx/deinit-all.c arm/lpc17xx/handlers.c arm/lpc17xx/hooks.c arm/lpc17xx/Drivers/source/debug_frmwrk.c arm/lpc17xx/mbed/BoardConsole.c
+TARGET_SRCS += arm/lpc17xx/startup.s arm/lpc17xx/deinit-all.c arm/lpc17xx/handlers.c arm/lpc17xx/hooks.c arm/lpc17xx/Drivers/source/debug_frmwrk.c arm/lpc17xx/mbed/BoardConsole.c arm/lpc17xx/mbed/BoardInit.c
 endif
 endif
 
diff --git a/arch/arm/lpc17xx/handlers.c b/arch/arm/lpc17xx/handlers.c
index faee7f9..b3366e0 100644
--- a/arch/arm/lpc17xx/handlers.c
+++ b/arch/arm/lpc17xx/handlers.c
@@ -1,6 +1,12 @@
+#include <stdint.h>
+#include <FreeRTOS.h>
+#include <task.h>
+#include <queue.h>
 #include <osdebug.h>
 #include "LPC17xx.h"
 
+void BoardExceptionHandler(int code);
+
 enum FaultType {
     NMI = 2, HardFault, MemManage, BusFault, UsageFault,
 };
@@ -9,17 +15,19 @@ struct fault_data_t {
     uint32_t r4, r5, r6, r7, r8, r9, r10, r11, r0, r1, r2, r3, r12, lr, pc, xPSR;
 };
 
-static void print_fault_data(struct fault_data_t * fault_data) {
+static void print_fault_data(struct fault_data_t * fault_data1, struct fault_data_t * fault_data2) {
     // These ones are saved naturally by the CPU
-    DBGOUT("pc:   %p\r\n", fault_data->pc);
-    DBGOUT("sp  = %p - lr  = %p - r12 = %p -xPSR = %p\r\n", fault_data + 1, fault_data->lr, fault_data->r12, fault_data->xPSR);
-    DBGOUT("r0  = %p - r1  = %p - r2  = %p - r3  = %p\r\n", fault_data->r0, fault_data->r1, fault_data->r2, fault_data->r3);
+    DBGOUT("pc:   %p\r\n", fault_data1->pc);
+    DBGOUT("sp  = %p - lr  = %p - r12 = %p -xPSR = %p\r\n", fault_data1 + 1, fault_data1->lr, fault_data1->r12, fault_data1->xPSR);
+    DBGOUT("r0  = %p - r1  = %p - r2  = %p - r3  = %p\r\n", fault_data1->r0, fault_data1->r1, fault_data1->r2, fault_data1->r3);
     // These ones are saved by the ASM handler; hence the reason they are before in memory.
-    DBGOUT("r4  = %p - r5  = %p - r6  = %p - r7  = %p\r\n", fault_data->r4, fault_data->r5, fault_data->r6, fault_data->r7);
-    DBGOUT("r8  = %p - r9  = %p - r10 = %p - r11 = %p\r\n", fault_data->r8, fault_data->r9, fault_data->r10, fault_data->r11);
+    DBGOUT("r4  = %p - r5  = %p - r6  = %p - r7  = %p\r\n", fault_data2->r4, fault_data2->r5, fault_data2->r6, fault_data2->r7);
+    DBGOUT("r8  = %p - r9  = %p - r10 = %p - r11 = %p\r\n", fault_data2->r8, fault_data2->r9, fault_data2->r10, fault_data2->r11);
 }
 
-void general_C_handler(enum FaultType fault, struct fault_data_t * fault_data) {
+void general_C_handler(enum FaultType fault, struct fault_data_t * fault_data2) {
+    uintptr_t eflags = ((uintptr_t) __builtin_return_address(0)) & 15;
+    struct fault_data_t * fault_data1 = (eflags & 4 ? fault_data2 : (struct fault_data_t *) __get_MSP());
     DBGOUT("***FAULT***\r\nType: ");
     switch (fault) {
     case NMI:
@@ -37,10 +45,13 @@ void general_C_handler(enum FaultType fault, struct fault_data_t * fault_data) {
     case UsageFault:
         DBGOUT("UsageFault\r\nCFSR: %p\r\nBFAR: %p\r\n", SCB->CFSR, SCB->BFAR);
         break;
+    default:
+        DBGOUT("Unknown\r\n");
     }
     
-    print_fault_data(fault_data);
+    print_fault_data(fault_data1, fault_data2);
     
     DBGOUT("HALTING\r\n");
+    BoardExceptionHandler(-1);
     while(1);
 }
diff --git a/arch/arm/lpc17xx/hooks.c b/arch/arm/lpc17xx/hooks.c
index e68c74a..1689db9 100644
--- a/arch/arm/lpc17xx/hooks.c
+++ b/arch/arm/lpc17xx/hooks.c
@@ -15,5 +15,4 @@ void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskN
 }
 
 void vApplicationIdleHook() {
-//    DBGOUT("vApplicationIdleHook()\r\n");
 }
diff --git a/arch/arm/lpc17xx/ldscript b/arch/arm/lpc17xx/ldscript
index 14fe819..4f1f3b9 100644
--- a/arch/arm/lpc17xx/ldscript
+++ b/arch/arm/lpc17xx/ldscript
@@ -36,6 +36,7 @@ MEMORY
  */
 EXTERN(__cs3_reset_cortex_m)
 EXTERN(__cs3_interrupt_vector_cortex_m)
+EXTERN(__cs3_interrupt_vector_cortex_m_mutable)
 EXTERN(__cs3_start_c main __cs3_stack __cs3_stack_size __cs3_heap_end)
 
 PROVIDE(__cs3_stack = __cs3_region_start_ram + __cs3_region_size_ram);
@@ -67,19 +68,27 @@ SECTIONS
     *(.rom)
     *(.rom.b)
 
-    __cs3_reset = __cs3_reset_cortex_m;
-    *(.cs3.reset)
-    /* Make sure we pulled in some reset code.  */
-    ASSERT (. != __cs3_reset, "No reset code");
-    
     *( .privileged_code )
     *( privileged_functions )
     
-    . = ALIGN(32);
+    __privileged_code_end___len = . - __privileged_code_start__ - 1 ;
+    __privileged_code_end___len |= __privileged_code_end___len >> 1 ;
+    __privileged_code_end___len |= __privileged_code_end___len >> 2 ;
+    __privileged_code_end___len |= __privileged_code_end___len >> 4 ;
+    __privileged_code_end___len |= __privileged_code_end___len >> 8 ;
+    __privileged_code_end___len |= __privileged_code_end___len >> 16 ;
+    . = ALIGN( MAX( __privileged_code_end___len + 1, 32 ) ) ;
 
     __privileged_code_end__ = .;
     __privileged_functions_end__ = .;
 
+    __cs3_reset = __cs3_reset_cortex_m;
+    *(.cs3.reset)
+    /* Make sure we pulled in some reset code.  */
+    ASSERT (. != __cs3_reset, "No reset code");
+    
+    *(.handlers)
+    
     *(.text .text.* .gnu.linkonce.t.*)
     *(.plt)
     *(.gnu.warning)
@@ -152,18 +161,28 @@ SECTIONS
   
   .data_begin :
   {
-    . = ALIGN(8);
+    . = ALIGN(32);
     __rom_data_begin = .;
   } > rom
 
   .data :
   {
-    . = ALIGN(32);
     __cs3_region_start_ram = .;
     __ram_data_begin = .;
     __privileged_data_start__ = .;
+    __cs3_interrupt_vector_mutable = __cs3_interrupt_vector_cortex_m_mutable;
+    *(.cs3.interrupt_vector_mutable)
+    /* Make sure we pulled in an interrupt vector.  */
+    ASSERT (. != __cs3_interrupt_vector_cortex_m_mutable, "No interrupt vector");
     *( privileged_data )
-    . = ALIGN(32);
+
+    __privileged_data_end___len = . - __privileged_data_start__ - 1 ;
+    __privileged_data_end___len |= __privileged_data_end___len >> 1 ;
+    __privileged_data_end___len |= __privileged_data_end___len >> 2 ;
+    __privileged_data_end___len |= __privileged_data_end___len >> 4 ;
+    __privileged_data_end___len |= __privileged_data_end___len >> 8 ;
+    __privileged_data_end___len |= __privileged_data_end___len >> 16 ;
+    . = ALIGN( MAX( __privileged_data_end___len + 1, 32 ) ) ;
 
     __privileged_data_end__ = .;
     *(.cs3.region-head.ram)
diff --git a/arch/arm/lpc17xx/mbed/BoardInit.c b/arch/arm/lpc17xx/mbed/BoardInit.c
new file mode 100644
index 0000000..9c5aa89
--- /dev/null
+++ b/arch/arm/lpc17xx/mbed/BoardInit.c
@@ -0,0 +1,16 @@
+#include "lpc17xx_nvic.h"
+
+extern void * __cs3_interrupt_vector_mutable;
+
+void BoardEarlyInit() {
+    //NVIC_SetVTOR((uint32_t) __cs3_interrupt_vector_mutable);
+}
+
+void BoardLateInit() {
+}
+
+void BoardShutdown() {
+}
+
+void BoardExceptionHandler(int code) {
+}
diff --git a/arch/arm/lpc17xx/startup.s b/arch/arm/lpc17xx/startup.s
index 7b9e56c..d0917b6 100644
--- a/arch/arm/lpc17xx/startup.s
+++ b/arch/arm/lpc17xx/startup.s
@@ -23,7 +23,7 @@
     .section ".stack", "w"
     .align  3
     .globl  __cs3_stack_mem
-    .globl  __stack_mem
+    .globl  __stack_start
     .globl  __cs3_stack_size
 __cs3_stack_mem:
 __stack_start:
@@ -54,12 +54,11 @@ __cs3_heap_end:
 
 
 /* Vector Table */
+    .section ".cs3.interrupt_vector_mutable"
+    .globl  __cs3_interrupt_vector_cortex_m_mutable
+    .type   __cs3_interrupt_vector_cortex_m_mutable, %object
 
-    .section ".cs3.interrupt_vector"
-    .globl  __cs3_interrupt_vector_cortex_m
-    .type   __cs3_interrupt_vector_cortex_m, %object
-
-__cs3_interrupt_vector_cortex_m:
+__cs3_interrupt_vector_cortex_m_mutable:
     .long   __cs3_stack                 /* Top of Stack                 */
     .long   __cs3_reset_cortex_m        /* Reset Handler                */
     .long   NMI_Handler                 /* NMI Handler                  */
@@ -114,6 +113,32 @@ __cs3_interrupt_vector_cortex_m:
     .long   USBActivity_IRQHandler      /* 49: USB Activity                 */
     .long   CANActivity_IRQHandler      /* 50: CAN Activity                 */
 
+    .size   __cs3_interrupt_vector_cortex_m_mutable, . - __cs3_interrupt_vector_cortex_m_mutable
+
+
+    .section ".cs3.interrupt_vector"
+    .globl  __cs3_interrupt_vector_cortex_m
+    .type   __cs3_interrupt_vector_cortex_m, %object
+
+__cs3_interrupt_vector_cortex_m:
+    .long   __cs3_stack                 /* Top of Stack                 */
+    .long   __cs3_reset_cortex_m        /* Reset Handler                */
+    .long   NMI_Handler                 /* NMI Handler                  */
+    .long   HardFault_Handler           /* Hard Fault Handler           */
+    .long   MemManage_Handler           /* MPU Fault Handler            */
+    .long   BusFault_Handler            /* Bus Fault Handler            */
+    .long   UsageFault_Handler          /* Usage Fault Handler          */
+    .long   0                           /* Reserved - ROM CRC check ?   */
+    .long   0                           /* Reserved                     */
+    .long   0                           /* Reserved                     */
+    .long   0                           /* Reserved                     */
+    .long   vPortSVCHandler             /* SVCall Handler               */
+    .long   DebugMon_Handler            /* Debug Monitor Handler        */
+    .long   0                           /* Reserved                     */
+    .long   xPortPendSVHandler          /* PendSV Handler               */
+    .long   xPortSysTickHandler         /* SysTick Handler              */
+
+
     .size   __cs3_interrupt_vector_cortex_m, . - __cs3_interrupt_vector_cortex_m
 
 
@@ -121,7 +146,7 @@ __cs3_interrupt_vector_cortex_m:
 
 /* Fault handlers wrappers */
 
-    .section .privileged_code,"x",%progbits
+    .section .handlers,"x",%progbits
     .thumb_func
     .type   NMI_Handler, %function
 NMI_Handler:
@@ -142,7 +167,6 @@ BusFault_Handler:
     .type   UsageFault_Handler, %function
 UsageFault_Handler:
     MOV     R0, 6
-    B       general_handler
     .type   general_handler, %function
 general_handler:
     MOV     R1, SP
@@ -188,7 +212,7 @@ __cs3_reset_cortex_m:
     .fnend
     .size   __cs3_reset_cortex_m,.-__cs3_reset_cortex_m
 
-    .section ".privileged_code"
+    .section .handlers,"x",%progbits
 
 
     .weak   DebugMon_Handler