ARM: Add common Cortex-M mtarch implementation

This includes support for preemption ready to be integrated into
exception handlers.

Signed-off-by: Benoît Thébaudeau <benoit.thebaudeau.dev@gmail.com>

Author: bthebaudeau

cpu/arm/common/sys/mtarch.c

@@ -0,0 +1,285 @@
+/*
+ * Copyright (c) 2016, Benoît Thébaudeau <benoit.thebaudeau.dev@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+ */
+/**
+ * \addtogroup arm-cm-mtarch
+ * @{
+ *
+ * \file
+ * Implmentation of the ARM Cortex-M support for Contiki multi-threading.
+ */
+#include CMSIS_DEV_HDR
+#include "sys/mt.h"
+
+#include <stdint.h>
+
+#define EXC_RETURN_PROCESS_THREAD_BASIC_FRAME   0xfffffffd
+
+/* Check whether EXC_RETURN[3:0] in LR indicates a preempted process thread. */
+#if __ARM_ARCH == 7
+#define PREEMPTED_PROCESS_THREAD() \
+  "and r0, lr, #0xf\n\t" \
+  "cmp r0, #0xd\n\t"
+#elif __ARM_ARCH == 6
+#define PREEMPTED_PROCESS_THREAD() \
+  "mov r0, lr\n\t" \
+  "movs r1, #0xf\n\t" \
+  "and r0, r1\n\t" \
+  "cmp r0, #0xd\n\t"
+#else
+#error Unsupported ARM architecture
+#endif
+/*----------------------------------------------------------------------------*/
+/**
+ * \brief SVCall system handler
+ *
+ * This exception handler executes the action requested by the corresponding
+ * \c svc instruction, which is a task switch from the main Contiki thread to an
+ * mt thread or the other way around.
+ */
+__attribute__ ((__naked__))
+void
+svcall_handler(void)
+{
+  /* This is a controlled system handler, so do not use ENERGEST_TYPE_IRQ. */
+
+  /*
+   * Decide whether to switch to the main thread or to a process thread,
+   * depending on the type of the thread preempted by SVCall.
+   */
+  __asm__ (PREEMPTED_PROCESS_THREAD()
+#if __ARM_ARCH == 7
+           "it eq\n\t"
+#endif
+           "beq switch_to_main_thread\n\t"
+
+  /*
+   * - Retrieve from the main stack the PSP passed to SVCall through R0. Note
+   *   that it cannot be retrieved directly from R0 on exception entry because
+   *   this register may have been overwritten by other exceptions on SVCall
+   *   entry.
+   * - Save the main thread context to the main stack.
+   * - Restore the process thread context from the process stack.
+   * - Return to Thread mode, resuming the process thread.
+   */
+#if __ARM_ARCH == 7
+           "ldr r0, [sp]\n\t"
+           "push {r4-r11, lr}\n\t"
+           "add r1, r0, #9 * 4\n\t"
+           "msr psp, r1\n\t"
+           "ldmia r0, {r4-r11, pc}");
+#elif __ARM_ARCH == 6
+           "mov r0, r8\n\t"
+           "mov r1, r9\n\t"
+           "mov r2, r10\n\t"
+           "mov r3, r11\n\t"
+           "push {r0-r7, lr}\n\t"
+           "ldr r0, [sp, #9 * 4]\n\t"
+           "ldmia r0!, {r4-r7}\n\t"
+           "mov r8, r4\n\t"
+           "mov r9, r5\n\t"
+           "mov r10, r6\n\t"
+           "mov r11, r7\n\t"
+           "ldmia r0!, {r3-r7}\n\t"
+           "msr psp, r0\n\t"
+           "bx r3");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * \brief PendSV system handler
+ *
+ * This exception handler executes following a call to mtarch_pstart() from
+ * another exception handler. It performs a task switch to the main Contiki
+ * thread if it is not already running.
+ */
+__attribute__ ((__naked__))
+void
+pendsv_handler(void)
+{
+  /* This is a controlled system handler, so do not use ENERGEST_TYPE_IRQ. */
+
+  /*
+   * Return without doing anything if PendSV has not preempted a process thread.
+   * This can occur either because PendSV has preempted the main thread, in
+   * which case there is nothing to do, or because mtarch_pstart() has been
+   * called from an exception handler without having called mt_init() first, in
+   * which case PendSV may have preempted an exception handler and nothing must
+   * be done because mt is not active.
+   */
+  __asm__ (  PREEMPTED_PROCESS_THREAD()
+#if __ARM_ARCH == 7
+             "it ne\n\t"
+             "bxne lr\n"
+#elif __ARM_ARCH == 6
+             "beq switch_to_main_thread\n\t"
+             "bx lr\n"
+#endif
+
+  /*
+   * - Save the process thread context to the process stack.
+   * - Place into the main stack the updated PSP that SVCall must return through
+   *   R0.
+   * - Restore the main thread context from the main stack.
+   * - Return to Thread mode, resuming the main thread.
+   */
+           "switch_to_main_thread:\n\t"
+             "mrs r0, psp\n\t"
+#if __ARM_ARCH == 7
+             "stmdb r0!, {r4-r11, lr}\n\t"
+             "str r0, [sp, #9 * 4]\n\t"
+             "pop {r4-r11, pc}");
+#elif __ARM_ARCH == 6
+             "mov r3, lr\n\t"
+             "sub r0, #5 * 4\n\t"
+             "stmia r0!, {r3-r7}\n\t"
+             "mov r4, r8\n\t"
+             "mov r5, r9\n\t"
+             "sub r0, #9 * 4\n\t"
+             "mov r6, r10\n\t"
+             "mov r7, r11\n\t"
+             "stmia r0!, {r4-r7}\n\t"
+             "pop {r4-r7}\n\t"
+             "sub r0, #4 * 4\n\t"
+             "mov r8, r4\n\t"
+             "mov r9, r5\n\t"
+             "str r0, [sp, #5 * 4]\n\t"
+             "mov r10, r6\n\t"
+             "mov r11, r7\n\t"
+             "pop {r4-r7, pc}");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_init(void)
+{
+  SCB->CCR = (SCB->CCR
+#ifdef SCB_CCR_NONBASETHRDENA_Msk
+              /*
+               * Make sure that any attempt to enter Thread mode with exceptions
+               * active faults.
+               *
+               * Only SVCall and PendSV are allowed to forcibly enter Thread
+               * mode, and they are configured with the same, lowest exception
+               * priority, so no other exceptions may be active.
+               */
+              & ~SCB_CCR_NONBASETHRDENA_Msk
+#endif
+             /*
+              * Force 8-byte stack pointer alignment on exception entry in order
+              * to be able to use AAPCS-conforming functions as exception
+              * handlers.
+              */
+             ) | SCB_CCR_STKALIGN_Msk;
+
+  /*
+   * Configure SVCall and PendSV with the same, lowest exception priority.
+   *
+   * This makes sure that they cannot preempt each other, and that the processor
+   * executes them after having handled all other exceptions. If both are
+   * pending at the same time, then SVCall takes precedence because of its lower
+   * exception number. In addition, the associated exception handlers do not
+   * have to check whether they are returning to Thread mode, because they
+   * cannot preempt any other exception.
+   */
+  NVIC_SetPriority(SVCall_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+  NVIC_SetPriority(PendSV_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+
+  /*
+   * Force the preceding configurations to take effect before further
+   * operations.
+   */
+  __DSB();
+  __ISB();
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_start(struct mtarch_thread *thread,
+             void (*function)(void *data), void *data)
+{
+  struct mtarch_thread_context *context = &thread->start_stack.context;
+
+  /*
+   * Initialize the thread context with the appropriate values to call
+   * function() with data and to make function() return to mt_exit() without
+   * having to call it explicitly.
+   */
+  context->exc_return = EXC_RETURN_PROCESS_THREAD_BASIC_FRAME;
+  context->r0 = (uint32_t)data;
+  context->lr = (uint32_t)mt_exit;
+  context->pc = (uint32_t)function;
+  context->xpsr = xPSR_T_Msk;
+  thread->psp = (uint32_t)context;
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_exec(struct mtarch_thread *thread)
+{
+  /* Pass the PSP to SVCall, and get the updated PSP as its return value. */
+  register uint32_t psp __asm__ ("r0") = thread->psp;
+  __asm__ volatile ("svc #0"
+                    : "+r" (psp)
+                    :: "memory");
+  thread->psp = psp;
+}
+/*----------------------------------------------------------------------------*/
+__attribute__ ((__naked__))
+void
+mtarch_yield(void)
+{
+  /* Invoke SVCall. */
+  __asm__ ("svc #0\n\t"
+           "bx lr");
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_stop(struct mtarch_thread *thread)
+{
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_pstart(void)
+{
+  /* Trigger PendSV. */
+  SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_pstop(void)
+{
+}
+/*----------------------------------------------------------------------------*/
+void
+mtarch_remove(void)
+{
+}
+/*----------------------------------------------------------------------------*/
+
+/** @} */