git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@346 35acf78f-673a-0410-8e92-d51de3d6d3f4

4 files changed, 587 insertions, 563 deletions

diff --git a/ports/ARMCM3-STM32F103/stm32_serial.c b/ports/ARMCM3-STM32F103/stm32_serial.c
index 148453b48..fbb1775c9 100644
--- a/ports/ARMCM3-STM32F103/stm32_serial.c
+++ b/ports/ARMCM3-STM32F103/stm32_serial.c
@@ -1,198 +1,200 @@
-/*

-    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

-

-    This file is part of ChibiOS/RT.

-

-    ChibiOS/RT 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 3 of the License, or

-    (at your option) any later version.

-

-    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

-*/

-

-#include <ch.h>

-

-#include "board.h"

-#include "nvic.h"

-#include "stm32_serial.h"

-#include "stm32lib/stm32f10x_nvic.h"

-

-#ifdef USE_USART1

-FullDuplexDriver COM1;

-static uint8_t ib1[SERIAL_BUFFERS_SIZE];

-static uint8_t ob1[SERIAL_BUFFERS_SIZE];

-#endif

-

-#ifdef USE_USART2

-FullDuplexDriver COM2;

-static uint8_t ib2[SERIAL_BUFFERS_SIZE];

-static uint8_t ob2[SERIAL_BUFFERS_SIZE];

-#endif

-

-#ifdef USE_USART3

-FullDuplexDriver COM3;

-static uint8_t ib3[SERIAL_BUFFERS_SIZE];

-static uint8_t ob3[SERIAL_BUFFERS_SIZE];

-#endif

-

-static void SetError(uint16_t sr, FullDuplexDriver *com) {

-  dflags_t sts = 0;

-

-  if (sr & SR_ORE)

-    sts |= SD_OVERRUN_ERROR;

-  if (sr & SR_PE)

-    sts |= SD_PARITY_ERROR;

-  if (sr & SR_FE)

-    sts |= SD_FRAMING_ERROR;

-  if (sr & SR_LBD)

-    sts |= SD_BREAK_DETECTED;

-  chSysLock();

-  chFDDAddFlagsI(com, sts);

-  chSysUnlock();

-}

-

-static void ServeInterrupt(USART_TypeDef *u, FullDuplexDriver *com) {

-  uint16_t sr = u->SR;

-

-  if (sr & (SR_ORE | SR_FE | SR_PE | SR_LBD))

-    SetError(sr, com);

-  if (sr & SR_RXNE) {

-    chSysLock();

-    chFDDIncomingDataI(com, u->DR);

-    chSysUnlock();

-  }

-  if (sr & SR_TXE) {

-    chSysLock();

-    msg_t b = chFDDRequestDataI(com);

-    chSysUnlock();

-    if (b < Q_OK)

-      u->CR1 &= ~CR1_TXEIE;

-    else

-      u->DR = b;

-  }

-}

-

-#ifdef USE_USART1

-/*

- * USART1 IRQ service routine.

- */

-void VectorD4(void) {

-

-  chSysIRQEnterI();

-  ServeInterrupt(USART1, &COM1);

-  chSysIRQExitI();

-}

-

-/*

- * Invoked by the high driver when one or more bytes are inserted in the

- * output queue.

- */

-static void OutNotify1(void) {

-

-  USART1->CR1 |= CR1_TXEIE;

-}

-#endif

-

-#ifdef USE_USART2

-/*

- * USART2 IRQ service routine.

- */

-void VectorD8(void) {

-

-  chSysIRQEnterI();

-  ServeInterrupt(USART2, &COM2);

-  chSysIRQExitI();

-}

-

-/*

- * Invoked by the high driver when one or more bytes are inserted in the

- * output queue.

- */

-static void OutNotify2(void) {

-

-  USART2->CR1 |= CR1_TXEIE;

-}

-#endif

-

-#ifdef USE_USART3

-/*

- * USART3 IRQ service routine.

- */

-void VectorDC(void) {

-

-  chSysIRQEnterI();

-  ServeInterrupt(USART3, &COM3);

-  chSysIRQExitI();

-}

-

-/*

- * Invoked by the high driver when one or more bytes are inserted in the

- * output queue.

- */

-static void OutNotify3(void) {

-

-  USART3->CR1 |= CR1_TXEIE;

-}

-#endif

-

-/*

- * USART setup, must be invoked with interrupts disabled.

- * NOTE: Does not reset I/O queues.

- */

-void SetUSARTI(USART_TypeDef *u, uint32_t speed, uint16_t cr1,

-               uint16_t cr2, uint16_t cr3) {

-

-  /*

-   * Baud rate setting.

-   */

-  if (u == USART1)

-    u->BRR = APB2CLK / speed;

-  else

-    u->BRR = APB1CLK / speed;

-

-  /*

-   * Note that some bits are enforced.

-   */

-  u->CR1 = cr1 | CR1_UE | CR1_PEIE | CR1_RXNEIE | CR1_TE | CR1_RE;

-  u->CR2 = cr2;

-  u->CR3 = cr3 | CR3_EIE;

-}

-

-/*

- * Serial subsystem initialization.

- * NOTE: Handshake pins are not switched to their function because they may have

- *       another use. Enable them externally if needed.

- */

-void InitSerial(uint32_t prio1, uint32_t prio2, uint32_t prio3) {

-

-#ifdef USE_USART1

-  chFDDInit(&COM1, ib1, sizeof ib1, NULL, ob1, sizeof ob1, OutNotify1);

-  RCC->APB2ENR |= 0x00004000;

-  SetUSARTI(USART1, 38400, 0, CR2_STOP1_BITS | CR2_LINEN, 0);

-  GPIOA->CRH = (GPIOA->CRH & 0xFFFFF00F) | 0x000004B0;

-  NVICEnableVector(USART1_IRQChannel, prio1);

-#endif

-

-#ifdef USE_USART2

-  chFDDInit(&COM2, ib2, sizeof ib2, NULL, ob2, sizeof ob2, OutNotify2);

-  RCC->APB1ENR |= 0x00020000;

-  SetUSARTI(USART2, 38400, 0, CR2_STOP1_BITS | CR2_LINEN, 0);

-  GPIOA->CRL = (GPIOA->CRL & 0xFFFF00FF) | 0x00004B00;

-  NVICEnableVector(USART2_IRQChannel, prio2);

-#endif

-

-#ifdef USE_USART3

-  chFDDInit(&COM3, ib3, sizeof ib3, NULL, ob3, sizeof ob3, OutNotify3);

-  RCC->APB1ENR |= 0x00040000;

-  SetUSARTI(USART3, 38400, 0, CR2_STOP1_BITS | CR2_LINEN, 0);

-  GPIOB->CRH = (GPIOB->CRH & 0xFFFF00FF) | 0x00004B00;

-  NVICEnableVector(USART3_IRQChannel, prio3);

-#endif

-}

+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <ch.h>
+
+#include "board.h"
+#include "nvic.h"
+#include "stm32_serial.h"
+#include "stm32lib/stm32f10x_nvic.h"
+
+#define USART_BITRATE (38400)
+
+#ifdef USE_USART1
+FullDuplexDriver COM1;
+static uint8_t ib1[SERIAL_BUFFERS_SIZE];
+static uint8_t ob1[SERIAL_BUFFERS_SIZE];
+#endif
+
+#ifdef USE_USART2
+FullDuplexDriver COM2;
+static uint8_t ib2[SERIAL_BUFFERS_SIZE];
+static uint8_t ob2[SERIAL_BUFFERS_SIZE];
+#endif
+
+#ifdef USE_USART3
+FullDuplexDriver COM3;
+static uint8_t ib3[SERIAL_BUFFERS_SIZE];
+static uint8_t ob3[SERIAL_BUFFERS_SIZE];
+#endif
+
+static void SetError(uint16_t sr, FullDuplexDriver *com) {
+  dflags_t sts = 0;
+
+  if (sr & SR_ORE)
+    sts |= SD_OVERRUN_ERROR;
+  if (sr & SR_PE)
+    sts |= SD_PARITY_ERROR;
+  if (sr & SR_FE)
+    sts |= SD_FRAMING_ERROR;
+  if (sr & SR_LBD)
+    sts |= SD_BREAK_DETECTED;
+  chSysLock();
+  chFDDAddFlagsI(com, sts);
+  chSysUnlock();
+}
+
+static void ServeInterrupt(USART_TypeDef *u, FullDuplexDriver *com) {
+  uint16_t sr = u->SR;
+
+  if (sr & (SR_ORE | SR_FE | SR_PE | SR_LBD))
+    SetError(sr, com);
+  if (sr & SR_RXNE) {
+    chSysLock();
+    chFDDIncomingDataI(com, u->DR);
+    chSysUnlock();
+  }
+  if (sr & SR_TXE) {
+    chSysLock();
+    msg_t b = chFDDRequestDataI(com);
+    chSysUnlock();
+    if (b < Q_OK)
+      u->CR1 &= ~CR1_TXEIE;
+    else
+      u->DR = b;
+  }
+}
+
+#ifdef USE_USART1
+/*
+ * USART1 IRQ service routine.
+ */
+void VectorD4(void) {
+
+  chSysIRQEnterI();
+  ServeInterrupt(USART1, &COM1);
+  chSysIRQExitI();
+}
+
+/*
+ * Invoked by the high driver when one or more bytes are inserted in the
+ * output queue.
+ */
+static void OutNotify1(void) {
+
+  USART1->CR1 |= CR1_TXEIE;
+}
+#endif
+
+#ifdef USE_USART2
+/*
+ * USART2 IRQ service routine.
+ */
+void VectorD8(void) {
+
+  chSysIRQEnterI();
+  ServeInterrupt(USART2, &COM2);
+  chSysIRQExitI();
+}
+
+/*
+ * Invoked by the high driver when one or more bytes are inserted in the
+ * output queue.
+ */
+static void OutNotify2(void) {
+
+  USART2->CR1 |= CR1_TXEIE;
+}
+#endif
+
+#ifdef USE_USART3
+/*
+ * USART3 IRQ service routine.
+ */
+void VectorDC(void) {
+
+  chSysIRQEnterI();
+  ServeInterrupt(USART3, &COM3);
+  chSysIRQExitI();
+}
+
+/*
+ * Invoked by the high driver when one or more bytes are inserted in the
+ * output queue.
+ */
+static void OutNotify3(void) {
+
+  USART3->CR1 |= CR1_TXEIE;
+}
+#endif
+
+/*
+ * USART setup, must be invoked with interrupts disabled.
+ * NOTE: Does not reset I/O queues.
+ */
+void SetUSARTI(USART_TypeDef *u, uint32_t speed, uint16_t cr1,
+               uint16_t cr2, uint16_t cr3) {
+
+  /*
+   * Baud rate setting.
+   */
+  if (u == USART1)
+    u->BRR = APB2CLK / speed;
+  else
+    u->BRR = APB1CLK / speed;
+
+  /*
+   * Note that some bits are enforced.
+   */
+  u->CR1 = cr1 | CR1_UE | CR1_PEIE | CR1_RXNEIE | CR1_TE | CR1_RE;
+  u->CR2 = cr2;
+  u->CR3 = cr3 | CR3_EIE;
+}
+
+/*
+ * Serial subsystem initialization.
+ * NOTE: Handshake pins are not switched to their function because they may have
+ *       another use. Enable them externally if needed.
+ */
+void InitSerial(uint32_t prio1, uint32_t prio2, uint32_t prio3) {
+
+#ifdef USE_USART1
+  chFDDInit(&COM1, ib1, sizeof ib1, NULL, ob1, sizeof ob1, OutNotify1);
+  RCC->APB2ENR |= 0x00004000;
+  SetUSARTI(USART1, USART_BITRATE, 0, CR2_STOP1_BITS | CR2_LINEN, 0);
+  GPIOA->CRH = (GPIOA->CRH & 0xFFFFF00F) | 0x000004B0;
+  NVICEnableVector(USART1_IRQChannel, prio1);
+#endif
+
+#ifdef USE_USART2
+  chFDDInit(&COM2, ib2, sizeof ib2, NULL, ob2, sizeof ob2, OutNotify2);
+  RCC->APB1ENR |= 0x00020000;
+  SetUSARTI(USART2, USART_BITRATE, 0, CR2_STOP1_BITS | CR2_LINEN, 0);
+  GPIOA->CRL = (GPIOA->CRL & 0xFFFF00FF) | 0x00004B00;
+  NVICEnableVector(USART2_IRQChannel, prio2);
+#endif
+
+#ifdef USE_USART3
+  chFDDInit(&COM3, ib3, sizeof ib3, NULL, ob3, sizeof ob3, OutNotify3);
+  RCC->APB1ENR |= 0x00040000;
+  SetUSARTI(USART3, USART_BITRATE, 0, CR2_STOP1_BITS | CR2_LINEN, 0);
+  GPIOB->CRH = (GPIOB->CRH & 0xFFFF00FF) | 0x00004B00;
+  NVICEnableVector(USART3_IRQChannel, prio3);
+#endif
+}
diff --git a/ports/ARMCM3/chcore.c b/ports/ARMCM3/chcore.c
index 8c4c74315..aac58f9e6 100644
--- a/ports/ARMCM3/chcore.c
+++ b/ports/ARMCM3/chcore.c
@@ -1,152 +1,167 @@
-/*

-    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

-

-    This file is part of ChibiOS/RT.

-

-    ChibiOS/RT 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 3 of the License, or

-    (at your option) any later version.

-

-    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

-*/

-

-#include <ch.h>

-#include <nvic.h>

-

-/*

- * System idle thread loop.

- */

-__attribute__((weak))

-void _IdleThread(void *p) {

-

-  while (TRUE) {

-//    asm volatile ("wfi");

-  }

-}

-

-/*

- * System console message (not implemented).

- */

-__attribute__((weak))

-void chSysPuts(char *msg) {

-}

-

-/*

- * System halt.

- */

-__attribute__((naked, weak))

-void chSysHalt(void) {

-

-  asm volatile ("cpsid   i");

-  while (TRUE) {

-  }

-}

-

-__attribute__((naked, weak))

-void threadstart(void) {

-

-  asm volatile ( \

-                "blx     r1                                     \n\t" \

-                "bl      chThdExit                              \n\t" \

-                "bl      chSysHalt                              \n\t" \

-               );

-}

-

-/*

- * System Timer vector.

- */

-void SysTickVector(void) {

-

-  chSysIRQEnterI();

-  chSysLock();

-

-  chSysTimerHandlerI();

-

-  chSysUnlock();

-  chSysIRQExitI();

-}

-

-void *retaddr;

-

-/*

- * System invoked context switch.

- */

-__attribute__((naked))

-void SVCallVector(Thread *otp, Thread *ntp) {

-

-#ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("mrs     r3, BASEPRI                            \n\t" \

-                "mrs     r12, PSP                               \n\t" \

-                "stmdb   r12!, {r3-r6,r8-r11, lr}               \n\t" \

-                "str     r12, [r0, #16]                         \n\t" \

-                "ldr     r12, [r1, #16]                         \n\t" \

-                "ldmia   r12!, {r3-r6,r8-r11, lr}               \n\t" \

-                "msr     PSP, r12                               \n\t" \

-                "msr     BASEPRI, r3                            \n\t" \

-                "bx      lr                                     ");

-#else

-  asm volatile ("mrs     r3, BASEPRI                            \n\t" \

-                "mrs     r12, PSP                               \n\t" \

-                "stmdb   r12!, {r3-r11, lr}                     \n\t" \

-                "str     r12, [r0, #16]                         \n\t" \

-                "ldr     r12, [r1, #16]                         \n\t" \

-                "ldmia   r12!, {r3-r11, lr}                     \n\t" \

-                "msr     PSP, r12                               \n\t" \

-                "msr     BASEPRI, r3                            \n\t" \

-                "bx      lr                                     ");

-#endif

-}

-

-/*

- * Preemption invoked context switch.

- */

-__attribute__((naked))

-void PendSVVector(void) {

-  Thread *otp;

-  register struct intctx *sp_thd asm("r12");

-

-  chSysLock();

-  asm volatile ("push    {lr}");

-  if (!chSchRescRequiredI()) {

-    chSysUnlock();

-    asm volatile ("pop     {pc}");

-  }

-

-  asm volatile ("pop     {lr}                                   \n\t" \

-                "movs    r3, #0                                 \n\t" \

-                "mrs     %0, PSP" : "=r" (sp_thd) : );

-#ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("stmdb   %0!, {r3-r6,r8-r11, lr}" :

-                "=r" (sp_thd) :

-                "r" (sp_thd));

-#else

-  asm volatile ("stmdb   %0!, {r3-r11,lr}" :

-                "=r" (sp_thd) :

-                "r" (sp_thd));

-#endif

-

-  (otp = currp)->p_ctx.r13 = sp_thd;

-  chSchReadyI(otp);

-  (currp = fifo_remove(&rlist.r_queue))->p_state = PRCURR;

-  rlist.r_preempt = CH_TIME_QUANTUM;

-#ifdef CH_USE_TRACE

-  chDbgTrace(otp, currp);

-#endif

-  sp_thd = currp->p_ctx.r13;

-

-#ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("ldmia   %0!, {r3-r6,r8-r11, lr}" : : "r" (sp_thd));

-#else

-  asm volatile ("ldmia   %0!, {r3-r11, lr}" : : "r" (sp_thd));

-#endif

-  asm volatile ("msr     PSP, %0                                \n\t" \

-                "msr     BASEPRI, r3                            \n\t" \

-                "bx      lr" : : "r" (sp_thd));

-}

+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <ch.h>
+#include <nvic.h>
+
+/*
+ * System idle thread loop.
+ */
+__attribute__((weak))
+void _IdleThread(void *p) {
+
+  while (TRUE) {
+//    asm volatile ("wfi");
+  }
+}
+
+/*
+ * System console message (not implemented).
+ */
+__attribute__((weak))
+void chSysPuts(char *msg) {
+}
+
+/*
+ * System halt.
+ */
+__attribute__((naked, weak))
+void chSysHalt(void) {
+
+  asm volatile ("cpsid   i");
+  while (TRUE) {
+  }
+}
+
+/*
+ * Start a thread by invoking its work function.
+ *
+ * Start a thread by calling its work function. If the work function returns,
+ * call chThdExit and chSysHalt.
+ */
+__attribute__((naked, weak))
+void threadstart(void) {
+
+  asm volatile ( \
+                "blx     r1                                     \n\t" \
+                "bl      chThdExit                              \n\t" \
+                "bl      chSysHalt                              \n\t" \
+               );
+}
+
+/*
+ * System Timer vector.
+ */
+void SysTickVector(void) {
+
+  chSysIRQEnterI();
+  chSysLock();
+
+  chSysTimerHandlerI();
+
+  chSysUnlock();
+  chSysIRQExitI();
+}
+
+void *retaddr;
+
+/*
+ * System invoked context switch.
+ */
+__attribute__((naked))
+void SVCallVector(Thread *otp, Thread *ntp) {
+  /* { r0 = otp, r1 = ntp } */
+  /* get the BASEPRI in r3 */
+  /* get the PSP in r12 */
+  /* push the registers on the PSP stack */
+  /* stores the modified PSP into the thread context */
+  /* fetches the PSP position from the new thread context */
+  /* pop the registers from the PSP stack */
+  /* set the PSP from r12 */
+  /* set the BASEPRI from R3 */
+#ifdef CH_CURRP_REGISTER_CACHE
+  asm volatile ("mrs     r3, BASEPRI                            \n\t" \
+                "mrs     r12, PSP                               \n\t" \
+                "stmdb   r12!, {r3-r6,r8-r11, lr}               \n\t" \
+                "str     r12, [r0, #16]                         \n\t" \
+                "ldr     r12, [r1, #16]                         \n\t" \
+                "ldmia   r12!, {r3-r6,r8-r11, lr}               \n\t" \
+                "msr     PSP, r12                               \n\t" \
+                "msr     BASEPRI, r3                            \n\t" \
+                "bx      lr                                     ");
+#else
+  asm volatile ("mrs     r3, BASEPRI                            \n\t" \
+                "mrs     r12, PSP                               \n\t" \
+                "stmdb   r12!, {r3-r11, lr}                     \n\t" \
+                "str     r12, [r0, #16]                         \n\t" \
+                "ldr     r12, [r1, #16]                         \n\t" \
+                "ldmia   r12!, {r3-r11, lr}                     \n\t" \
+                "msr     PSP, r12                               \n\t" \
+                "msr     BASEPRI, r3                            \n\t" \
+                "bx      lr                                     ");
+#endif
+}
+
+/*
+ * Preemption invoked context switch.
+ */
+__attribute__((naked))
+void PendSVVector(void) {
+  Thread *otp;
+  register struct intctx *sp_thd asm("r12");
+
+  chSysLock();
+  asm volatile ("push    {lr}");
+  if (!chSchRescRequiredI()) {
+    chSysUnlock();
+    asm volatile ("pop     {pc}");
+  }
+
+  asm volatile ("pop     {lr}                                   \n\t" \
+                "movs    r3, #0                                 \n\t" \
+                "mrs     %0, PSP" : "=r" (sp_thd) : );
+#ifdef CH_CURRP_REGISTER_CACHE
+  asm volatile ("stmdb   %0!, {r3-r6,r8-r11, lr}" :
+                "=r" (sp_thd) :
+                "r" (sp_thd));
+#else
+  asm volatile ("stmdb   %0!, {r3-r11,lr}" :
+                "=r" (sp_thd) :
+                "r" (sp_thd));
+#endif
+
+  (otp = currp)->p_ctx.r13 = sp_thd;
+  chSchReadyI(otp);
+  (currp = fifo_remove(&rlist.r_queue))->p_state = PRCURR;
+  /* set the round-robin time quantum */
+  rlist.r_preempt = CH_TIME_QUANTUM;
+#ifdef CH_USE_TRACE
+  chDbgTrace(otp, currp);
+#endif
+  sp_thd = currp->p_ctx.r13;
+
+#ifdef CH_CURRP_REGISTER_CACHE
+  asm volatile ("ldmia   %0!, {r3-r6,r8-r11, lr}" : : "r" (sp_thd));
+#else
+  asm volatile ("ldmia   %0!, {r3-r11, lr}" : : "r" (sp_thd));
+#endif
+  asm volatile ("msr     PSP, %0                                \n\t" \
+                "msr     BASEPRI, r3                            \n\t" \
+                "bx      lr" : : "r" (sp_thd));
+}
diff --git a/ports/ARMCM3/chcore.h b/ports/ARMCM3/chcore.h
index 9a8f00bee..3136f0d36 100644
--- a/ports/ARMCM3/chcore.h
+++ b/ports/ARMCM3/chcore.h
@@ -1,120 +1,126 @@
-/*

-    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

-

-    This file is part of ChibiOS/RT.

-

-    ChibiOS/RT 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 3 of the License, or

-    (at your option) any later version.

-

-    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

-*/

-

-#ifndef _CHCORE_H_

-#define _CHCORE_H_

-

-#define CH_ARCHITECTURE_ARMCM3

-

-typedef void *regarm;

-

-/*

- * Interrupt saved context, empty in this architecture.

- */

-struct extctx {

-};

-

-/*

- * System saved context.

- */

-struct intctx {

-  regarm  basepri;

-  regarm  r4;

-  regarm  r5;

-  regarm  r6;

-#ifndef CH_CURRP_REGISTER_CACHE

-  regarm  r7;

-#endif

-  regarm  r8;

-  regarm  r9;

-  regarm  r10;

-  regarm  r11;

-  regarm  lr_exc;

-  regarm  r0;

-  regarm  r1;

-  regarm  r2;

-  regarm  r3;

-  regarm  r12;

-  regarm  lr_thd;

-  regarm  pc;

-  regarm  xpsr;

-};

-

-/*

- * Port dependent part of the Thread structure, you may add fields in

- * this structure.

- */

-typedef struct {

-  struct intctx *r13;

-} Context;

-

-/*

- * Platform dependent part of the \p chThdCreate() API.

- */

-#define SETUP_CONTEXT(workspace, wsize, pf, arg) {                      \

-  tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace +              \

-                                     wsize -                            \

-                                     sizeof(struct intctx));            \

-  tp->p_ctx.r13->basepri = 0;                                           \

-  tp->p_ctx.r13->lr_exc = (regarm)0xFFFFFFFD;                           \

-  tp->p_ctx.r13->r0 = arg;                                              \

-  tp->p_ctx.r13->r1 = pf;                                               \

-  tp->p_ctx.r13->pc = threadstart;                                      \

-  tp->p_ctx.r13->xpsr = (regarm)0x01000000;                             \

-}

-

-#define chSysLock() {                                                   \

-  register uint32_t tmp asm ("r3");                                     \

-  asm volatile ("movs    %0, #0x10" : "=r" (tmp): );                    \

-  asm volatile ("msr     BASEPRI, %0" : : "r" (tmp));                   \

-}

-#define chSysUnlock() {                                                 \

-  register uint32_t tmp asm ("r3");                                     \

-  asm volatile ("movs    %0, #0" : "=r" (tmp): );                       \

-  asm volatile ("msr     BASEPRI, %0" : : "r" (tmp));                   \

-}

-#define chSysSwitchI(otp, ntp) {                                        \

-  register Thread *_otp asm ("r0") = (otp);                             \

-  register Thread *_ntp asm ("r1") = (ntp);                             \

-  asm volatile ("svc     #0" : : "r" (_otp), "r" (_ntp));               \

-}

-

-#define INT_REQUIRED_STACK 0

-#define StackAlign(n) ((((n) - 1) | 3) + 1)

-#define UserStackSize(n) StackAlign(sizeof(Thread) +                    \

-                                    sizeof(struct intctx) +             \

-                                    sizeof(struct extctx) +             \

-                                    (n) +                               \

-                                    INT_REQUIRED_STACK)

-#define WorkingArea(s, n) uint32_t s[UserStackSize(n) >> 2];

-

-#define chSysIRQEnterI()

-#define chSysIRQExitI() {                                               \

-  SCB_ICSR = ICSR_PENDSVSET;                                            \

-}

-

-/* It should be 8.*/

-#define IDLE_THREAD_STACK_SIZE 0

-void _IdleThread(void *p) __attribute__((noreturn));

-

-void chSysHalt(void);

-void chSysPuts(char *msg);

-void threadstart(void);

-

-#endif /* _CHCORE_H_ */

+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _CHCORE_H_
+#define _CHCORE_H_
+
+#define CH_ARCHITECTURE_ARMCM3
+
+typedef void *regarm;
+
+/*
+ * Interrupt saved context, empty in this architecture.
+ */
+struct extctx {
+};
+
+/*
+ * System saved context.
+ */
+struct intctx {
+  regarm  basepri;
+  regarm  r4;
+  regarm  r5;
+  regarm  r6;
+#ifndef CH_CURRP_REGISTER_CACHE
+  regarm  r7;
+#endif
+  regarm  r8;
+  regarm  r9;
+  regarm  r10;
+  regarm  r11;
+  regarm  lr_exc;
+  regarm  r0;
+  regarm  r1;
+  regarm  r2;
+  regarm  r3;
+  regarm  r12;
+  regarm  lr_thd;
+  regarm  pc;
+  regarm  xpsr;
+};
+
+/*
+ * Port dependent part of the Thread structure, you may add fields in
+ * this structure.
+ */
+typedef struct {
+  struct intctx *r13;
+} Context;
+
+/*
+ * Platform dependent part of the \p chThdCreate() API.
+ *
+ * The top of the workspace is used for the intctx datastructure.
+ *
+ */
+#define SETUP_CONTEXT(workspace, wsize, pf, arg) {                      \
+  tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace +              \
+                                     wsize -                            \
+                                     sizeof(struct intctx));            \
+  tp->p_ctx.r13->basepri = 0;                                           \
+  tp->p_ctx.r13->lr_exc = (regarm)0xFFFFFFFD;                           \
+  tp->p_ctx.r13->r0 = arg;                                              \
+  tp->p_ctx.r13->r1 = pf;                                               \
+  tp->p_ctx.r13->pc = threadstart;                                      \
+  tp->p_ctx.r13->xpsr = (regarm)0x01000000;                             \
+}
+
+#define chSysLock() {                                                   \
+  register uint32_t tmp asm ("r3");                                     \
+  asm volatile ("movs    %0, #0x10" : "=r" (tmp): );                    \
+  asm volatile ("msr     BASEPRI, %0" : : "r" (tmp));                   \
+}
+#define chSysUnlock() {                                                 \
+  register uint32_t tmp asm ("r3");                                     \
+  asm volatile ("movs    %0, #0" : "=r" (tmp): );                       \
+  asm volatile ("msr     BASEPRI, %0" : : "r" (tmp));                   \
+}
+#define chSysSwitchI(otp, ntp) {                                        \
+  register Thread *_otp asm ("r0") = (otp);                             \
+  register Thread *_ntp asm ("r1") = (ntp);                             \
+  asm volatile ("svc     #0" : : "r" (_otp), "r" (_ntp));               \
+}
+
+#define INT_REQUIRED_STACK 0
+#define StackAlign(n) ((((n) - 1) | 3) + 1)
+#define UserStackSize(n) StackAlign(sizeof(Thread) +                    \
+                                    sizeof(struct intctx) +             \
+                                    sizeof(struct extctx) +             \
+                                    (n) +                               \
+                                    INT_REQUIRED_STACK)
+#define WorkingArea(s, n) uint32_t s[UserStackSize(n) >> 2];
+
+/* called on each interrupt entry, currently nothing is done */
+#define chSysIRQEnterI()
+/* called on each interrupt exit, pends a supervisor handler for
+ * execution after all higher priority interrupts; PendSVVector() */
+#define chSysIRQExitI() {                                               \
+  SCB_ICSR = ICSR_PENDSVSET;                                            \
+}
+
+/* It should be 8.*/
+#define IDLE_THREAD_STACK_SIZE 0
+void _IdleThread(void *p) __attribute__((noreturn));
+
+void chSysHalt(void);
+void chSysPuts(char *msg);
+void threadstart(void);
+
+#endif /* _CHCORE_H_ */
diff --git a/ports/ARMCM3/crt0.s b/ports/ARMCM3/crt0.s
index 47a89c632..0f2f4667d 100644
--- a/ports/ARMCM3/crt0.s
+++ b/ports/ARMCM3/crt0.s
@@ -1,93 +1,94 @@
-/*

-    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

-

-    This file is part of ChibiOS/RT.

-

-    ChibiOS/RT 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 3 of the License, or

-    (at your option) any later version.

-

-    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.

-*/

-

-/*

- * Generic ARM-CortexM3 startup file for ChibiOS/RT.

- */

-

-.set    CONTROL_MODE_PRIVILEGED, 0

-.set    CONTROL_MODE_UNPRIVILEGED, 1

-.set    CONTROL_USE_MSP, 0

-.set    CONTROL_USE_PSP, 2

-

-.text

-.balign 2

-.syntax unified

-.thumb

-

-/*

- * Reset handler.

- */

-.thumb_func

-.global ResetHandler

-ResetHandler:

-        /*

-         * Stack pointers initialization.

-         */

-        ldr     r0, =__ram_end__

-        ldr     r1, =__main_stack_size__

-        sub     r0, r0, r1

-        msr     PSP, r0

-//        ldr     r1, =__process_stack_size__

-//        sub     r0, r0, r1

-        /*

-         * Data initialization.

-         * NOTE: It assumes that the DATA size is a multiple of 4.

-         */

-        ldr     r1, =_textdata

-        ldr     r2, =_data

-        ldr     r3, =_edata

-dloop:

-        cmp     r2, r3

-        ittt    lo

-        ldrlo   r0, [r1], #4

-        strlo   r0, [r2], #4

-        blo     dloop

-        /*

-         * BSS initialization.

-         * NOTE: It assumes that the BSS size is a multiple of 4.

-         */

-        movs    r0, #0

-        ldr     r1, =_bss_start

-        ldr     r2, =_bss_end

-bloop:

-        cmp     r1, r2

-        itt     lo

-        strlo   r0, [r1], #4

-        blo     bloop

-        /*

-         * Switches to the Process Stack and disables the interrupts globally.

-         */

-        movs    r0, #CONTROL_MODE_PRIVILEGED | CONTROL_USE_PSP

-        msr     CONTROL, r0

-        isb

-        movs    r0, #0x10

-        msr     BASEPRI, r0

-        cpsie   i

-        /*

-         * Application-provided HW initialization routine.

-         */

-        bl      hwinit

-        /*

-         * main(0, NULL).

-         */

-        movs    r0, #0

-        mov     r1, r0

-        bl      main

-        bl      chSysHalt

+/*
+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.
+
+    This file is part of ChibiOS/RT.
+
+    ChibiOS/RT 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 3 of the License, or
+    (at your option) any later version.
+
+    ChibiOS/RT 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, see <http://www.gnu.org/licenses/>.
+*/
+
+/*
+ * Generic ARM-CortexM3 startup file for ChibiOS/RT.
+ */
+
+.set    CONTROL_MODE_PRIVILEGED, 0
+.set    CONTROL_MODE_UNPRIVILEGED, 1
+.set    CONTROL_USE_MSP, 0
+.set    CONTROL_USE_PSP, 2
+
+.text
+.balign 2
+.syntax unified
+.thumb
+
+/*
+ * Reset handler.
+ */
+.thumb_func
+.global ResetHandler
+ResetHandler:
+        /*
+         * Stack pointers initialization.
+         */
+        ldr     r0, =__ram_end__
+        ldr     r1, =__main_stack_size__
+        sub     r0, r0, r1
+        /* { r0  = main stack low address } */
+        msr     PSP, r0
+//        ldr     r1, =__process_stack_size__
+//        sub     r0, r0, r1
+        /*
+         * Data initialization.
+         * NOTE: It assumes that the DATA size is a multiple of 4.
+         */
+        ldr     r1, =_textdata
+        ldr     r2, =_data
+        ldr     r3, =_edata
+dloop:
+        cmp     r2, r3
+        ittt    lo
+        ldrlo   r0, [r1], #4
+        strlo   r0, [r2], #4
+        blo     dloop
+        /*
+         * BSS initialization.
+         * NOTE: It assumes that the BSS size is a multiple of 4.
+         */
+        movs    r0, #0
+        ldr     r1, =_bss_start
+        ldr     r2, =_bss_end
+bloop:
+        cmp     r1, r2
+        itt     lo
+        strlo   r0, [r1], #4
+        blo     bloop
+        /*
+         * Switches to the Process Stack and disables the interrupts globally.
+         */
+        movs    r0, #CONTROL_MODE_PRIVILEGED | CONTROL_USE_PSP
+        msr     CONTROL, r0
+        isb
+        movs    r0, #0x10
+        msr     BASEPRI, r0
+        cpsie   i
+        /*
+         * Application-provided HW initialization routine.
+         */
+        bl      hwinit
+        /*
+         * main(0, NULL).
+         */
+        movs    r0, #0
+        mov     r1, r0
+        bl      main
+        bl      chSysHalt