From 1bd14d8d40b7132fb2648df8cccb36f73532b17c Mon Sep 17 00:00:00 2001
From: NIIBE Yutaka <gniibe@fsij.org>
Date: Fri, 23 Jun 2017 09:03:26 +0900
Subject: Factor out arch dependent code.
---
ChangeLog | 13 +
chopstx-cortex-m.c | 707 +++++++++++++++++++++++++++++++++++++++++++++++++++++
chopstx-cortex-m.h | 12 +
chopstx.c | 699 ++--------------------------------------------------
4 files changed, 758 insertions(+), 673 deletions(-)
create mode 100644 chopstx-cortex-m.c
create mode 100644 chopstx-cortex-m.h
diff --git a/ChangeLog b/ChangeLog
index 9c6b585..4d043e9 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,16 @@
+2017-06-23 NIIBE Yutaka <gniibe@fsij.org>
+
+ * chopstx.c (chx_init): Use chx_init_arch.
+ (chopstx_create): Use chopstx_create_arch.
+ (chx_systick_reset, chx_systick_reload, chx_systick_get)
+ (usec_to_ticks, chx_enable_intr, chx_clr_intr, chx_disable_intr)
+ (chx_set_intr_prio, chx_prio_init, chx_cpu_sched_lock)
+ (chx_cpu_sched_unlock, idle, chx_handle_intr)
+ (chx_request_preemption, chx_sched, preempt, svc): Move to...
+ * chopstx-cortex-m.c: ... here.
+ (chx_init_arch, chopstx_create_arch): New.
+ * chopstx-cortex-m.h: New for tcontext_t.
+
2017-06-22 NIIBE Yutaka <gniibe@fsij.org>
* chopstx.c (chx_sched): Use ->V for return value.
diff --git a/chopstx-cortex-m.c b/chopstx-cortex-m.c
new file mode 100644
index 0000000..014f28e
--- /dev/null
+++ b/chopstx-cortex-m.c
@@ -0,0 +1,707 @@
+/*
+ * chopstx-cortex-m.c - Threads and only threads: Arch specific code
+ * for Cortex-M0/M3
+ *
+ * Copyright (C) 2013, 2014, 2015, 2016, 2017
+ * Flying Stone Technology
+ * Author: NIIBE Yutaka <gniibe@fsij.org>
+ *
+ * This file is a part of Chopstx, a thread library for embedded.
+ *
+ * Chopstx 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.
+ *
+ * Chopstx 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/>.
+ *
+ * As additional permission under GNU GPL version 3 section 7, you may
+ * distribute non-source form of the Program without the copy of the
+ * GNU GPL normally required by section 4, provided you inform the
+ * receipents of GNU GPL by a written offer.
+ *
+ */
+
+/* Saved registers on the stack. */
+struct chx_stack_regs {
+ uint32_t reg[8]; /* r0, r1, r2, r3, r12, lr, pc, xpsr */
+};
+
+/*
+ * Constants for ARM.
+ */
+#define REG_SP 8
+
+#define REG_R0 0
+#define REG_LR 5
+#define REG_PC 6
+#define REG_XPSR 7
+
+#define INITIAL_XPSR 0x01000000 /* T=1 */
+
+/*
+ * Exception priority: lower has higher precedence.
+ *
+ * Cortex-M3
+ * =====================================
+ * Prio 0x30: svc
+ * ---------------------
+ * Prio 0x40: thread temporarily inhibiting schedule for critical region
+ * ...
+ * Prio 0xb0: systick, external interrupt
+ * Prio 0xc0: pendsv
+ * =====================================
+ *
+ * Cortex-M0
+ * =====================================
+ * Prio 0x00: thread temporarily inhibiting schedule for critical region
+ * ...
+ * Prio 0x40: systick, external interrupt
+ * Prio 0x80: pendsv
+ * Prio 0x80: svc
+ * =====================================
+ */
+
+#define CPU_EXCEPTION_PRIORITY_CLEAR 0
+
+#if defined(__ARM_ARCH_6M__)
+#define CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED 0x00
+/* ... */
+#define CPU_EXCEPTION_PRIORITY_SYSTICK CPU_EXCEPTION_PRIORITY_INTERRUPT
+#define CPU_EXCEPTION_PRIORITY_INTERRUPT 0x40
+#define CPU_EXCEPTION_PRIORITY_PENDSV 0x80
+#define CPU_EXCEPTION_PRIORITY_SVC 0x80 /* No use in this arch */
+#elif defined(__ARM_ARCH_7M__)
+#define CPU_EXCEPTION_PRIORITY_SVC 0x30
+
+#define CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED 0x40
+/* ... */
+#define CPU_EXCEPTION_PRIORITY_SYSTICK CPU_EXCEPTION_PRIORITY_INTERRUPT
+#define CPU_EXCEPTION_PRIORITY_INTERRUPT 0xb0
+#define CPU_EXCEPTION_PRIORITY_PENDSV 0xc0
+#else
+#error "no support for this arch"
+#endif
+
+/*
+ * Lower layer architecture specific functions.
+ *
+ * system tick and interrupt
+ */
+
+/*
+ * System tick
+ */
+/* SysTick registers. */
+static volatile uint32_t *const SYST_CSR = (uint32_t *)0xE000E010;
+static volatile uint32_t *const SYST_RVR = (uint32_t *)0xE000E014;
+static volatile uint32_t *const SYST_CVR = (uint32_t *)0xE000E018;
+
+static void
+chx_systick_reset (void)
+{
+ *SYST_RVR = 0;
+ *SYST_CVR = 0;
+ *SYST_CSR = 7;
+}
+
+static void
+chx_systick_reload (uint32_t ticks)
+{
+ *SYST_RVR = ticks;
+ *SYST_CVR = 0; /* write (any) to clear the counter to reload. */
+ *SYST_RVR = 0;
+}
+
+static uint32_t
+chx_systick_get (void)
+{
+ return *SYST_CVR;
+}
+
+static uint32_t usec_to_ticks (uint32_t usec)
+{
+ return usec * MHZ;
+}
+
+/*
+ * Interrupt Handling
+ */
+
+/* NVIC: Nested Vectored Interrupt Controller. */
+struct NVIC {
+ volatile uint32_t ISER[8];
+ uint32_t unused1[24];
+ volatile uint32_t ICER[8];
+ uint32_t unused2[24];
+ volatile uint32_t ISPR[8];
+ uint32_t unused3[24];
+ volatile uint32_t ICPR[8];
+ uint32_t unused4[24];
+ volatile uint32_t IABR[8];
+ uint32_t unused5[56];
+ volatile uint32_t IPR[60];
+};
+
+static struct NVIC *const NVIC = (struct NVIC *)0xE000E100;
+#define NVIC_ISER(n) (NVIC->ISER[n >> 5])
+#define NVIC_ICER(n) (NVIC->ICER[n >> 5])
+#define NVIC_ICPR(n) (NVIC->ICPR[n >> 5])
+#define NVIC_IPR(n) (NVIC->IPR[n >> 2])
+
+
+static void
+chx_enable_intr (uint8_t irq_num)
+{
+ NVIC_ISER (irq_num) = 1 << (irq_num & 0x1f);
+}
+
+static void
+chx_clr_intr (uint8_t irq_num)
+{ /* Clear pending interrupt. */
+ NVIC_ICPR (irq_num) = 1 << (irq_num & 0x1f);
+}
+
+static void
+chx_disable_intr (uint8_t irq_num)
+{
+ NVIC_ICER (irq_num) = 1 << (irq_num & 0x1f);
+}
+
+static void
+chx_set_intr_prio (uint8_t n)
+{
+ unsigned int sh = (n & 3) << 3;
+
+ NVIC_IPR (n) = (NVIC_IPR(n) & ~(0xFF << sh))
+ | (CPU_EXCEPTION_PRIORITY_INTERRUPT << sh);
+}
+
+static volatile uint32_t *const ICSR = (uint32_t *)0xE000ED04;
+
+/* Priority control. */
+static uint32_t *const AIRCR = (uint32_t *)0xE000ED0C;
+static uint32_t *const SHPR2 = (uint32_t *)0xE000ED1C;
+static uint32_t *const SHPR3 = (uint32_t *)0xE000ED20;
+
+static void
+chx_prio_init (void)
+{
+ *AIRCR = 0x05FA0000 | ( 5 << 8); /* PRIGROUP = 5, 2-bit:2-bit. */
+ *SHPR2 = (CPU_EXCEPTION_PRIORITY_SVC << 24);
+ *SHPR3 = ((CPU_EXCEPTION_PRIORITY_SYSTICK << 24)
+ | (CPU_EXCEPTION_PRIORITY_PENDSV << 16));
+}
+
+
+static void
+chx_cpu_sched_lock (void)
+{
+ if (running->prio < CHOPSTX_PRIO_INHIBIT_PREEMPTION)
+ {
+#if defined(__ARM_ARCH_6M__)
+ asm volatile ("cpsid i" : : : "memory");
+#else
+ register uint32_t tmp = CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED;
+ asm volatile ("msr BASEPRI, %0" : : "r" (tmp) : "memory");
+#endif
+ }
+}
+
+static void
+chx_cpu_sched_unlock (void)
+{
+ if (running->prio < CHOPSTX_PRIO_INHIBIT_PREEMPTION)
+ {
+#if defined(__ARM_ARCH_6M__)
+ asm volatile ("cpsie i" : : : "memory");
+#else
+ register uint32_t tmp = CPU_EXCEPTION_PRIORITY_CLEAR;
+ asm volatile ("msr BASEPRI, %0" : : "r" (tmp) : "memory");
+#endif
+ }
+}
+
+
+static void __attribute__((naked, used))
+idle (void)
+{
+#if defined(USE_WFI_FOR_IDLE)
+ for (;;)
+ asm volatile ("wfi" : : : "memory");
+#else
+ for (;;);
+#endif
+}
+
+
+void
+chx_handle_intr (void)
+{
+ struct chx_pq *p;
+ register uint32_t irq_num;
+
+ asm volatile ("mrs %0, IPSR\n\t"
+ "sub %0, #16" /* Exception # - 16 = interrupt number. */
+ : "=r" (irq_num) : /* no input */ : "memory");
+
+ chx_disable_intr (irq_num);
+ chx_spin_lock (&q_intr.lock);
+ for (p = q_intr.q.next; p != (struct chx_pq *)&q_intr.q; p = p->next)
+ if (p->v == irq_num)
+ { /* should be one at most. */
+ struct chx_px *px = (struct chx_px *)p;
+
+ ll_dequeue (p);
+ chx_wakeup (p);
+ chx_request_preemption (px->master->prio);
+ break;
+ }
+ chx_spin_unlock (&q_intr.lock);
+}
+
+static void
+chx_init_arch (struct chx_thread *tp)
+{
+ memset (&tp->tc, 0, sizeof (tp->tc));
+}
+
+static void
+chx_request_preemption (uint16_t prio)
+{
+ if (running == NULL || (uint16_t)running->prio < prio)
+ {
+ *ICSR = (1 << 28);
+ asm volatile ("" : : : "memory");
+ }
+}
+
+
+/*
+ * chx_sched: switch to another thread.
+ *
+ * There are two cases:
+ * YIELD=0 (SLEEP): Current RUNNING thread is already connected to
+ * something (mutex, cond, intr, etc.)
+ * YIELD=1 (YIELD): Current RUNNING thread is active,
+ * it is needed to be enqueued to READY queue.
+ *
+ * For Cortex-M0, this should be AAPCS-compliant function entry, so we
+ * put "noinline" attribute.
+ *
+ * AAPCS: ARM Architecture Procedure Call Standard
+ *
+ * Returns:
+ * 1 on wakeup by others.
+ * 0 on normal wakeup.
+ * -1 on cancellation.
+ */
+static uintptr_t __attribute__ ((naked, noinline))
+chx_sched (uint32_t yield)
+{
+ register struct chx_thread *tp asm ("r0");
+
+#if defined(__ARM_ARCH_7M__)
+ asm volatile (
+ "svc #0\n\t"
+ "bx lr"
+ : "=r" (tp) : "0" (yield): "memory");
+#else
+ register uint32_t arg_yield asm ("r1");
+
+ /* Build stack data as if it were an exception entry. */
+ /*
+ * r0: 0 scratch
+ * r1: 0 scratch
+ * r2: 0 scratch
+ * r3: 0 scratch
+ * r12: 0 scratch
+ * lr as-is
+ * pc: return address (= lr)
+ * psr: INITIAL_XPSR scratch
+ */
+ asm ("mov r1, lr\n\t"
+ "mov r2, r1\n\t"
+ "mov r3, #128\n\t"
+ "lsl r3, #17\n\t"
+ "push {r1, r2, r3}\n\t"
+ "mov r1, #0\n\t"
+ "mov r2, r1\n\t"
+ "mov r3, r1\n\t"
+ "push {r1, r2, r3}\n\t"
+ "push {r1, r2}"
+ : /* no output*/
+ : /* no input */
+ : "r1", "r2", "r3", "memory");
+
+ /* Save registers onto CHX_THREAD struct. */
+ asm ("mov r1, r0\n\t"
+ "ldr r2, =running\n\t"
+ "ldr r0, [r2]\n\t"
+ "add r0, #20\n\t"
+ "stm r0!, {r4, r5, r6, r7}\n\t"
+ "mov r2, r8\n\t"
+ "mov r3, r9\n\t"
+ "mov r4, r10\n\t"
+ "mov r5, r11\n\t"
+ "mov r6, sp\n\t"
+ "stm r0!, {r2, r3, r4, r5, r6}\n\t"
+ "sub r0, #56"
+ : "=r" (tp), "=r" (arg_yield)
+ : "0" (yield)
+ : "r2", "r3", "r4", "r5", "r6", "r7", "memory");
+
+ if (arg_yield)
+ {
+ if (tp->flag_sched_rr)
+ chx_timer_dequeue (tp);
+ chx_ready_enqueue (tp);
+ }
+
+ tp = chx_ready_pop ();
+ if (tp && tp->flag_sched_rr)
+ {
+ chx_spin_lock (&q_timer.lock);
+ tp = chx_timer_insert (tp, PREEMPTION_USEC);
+ chx_spin_unlock (&q_timer.lock);
+ }
+
+ asm volatile (/* Now, r0 points to the thread to be switched. */
+ /* Put it to *running. */
+ "ldr r1, =running\n\t"
+ /* Update running. */
+ "str r0, [r1]\n\t"
+ "cmp r0, #0\n\t"
+ "bne 0f\n\t"
+
+ /* Spawn an IDLE thread. */
+ "ldr r1, =__main_stack_end__\n\t"
+ "mov sp, r1\n\t"
+ "ldr r0, =idle\n\t" /* PC = idle */
+ /**/
+ /* Unmask interrupts. */
+ "cpsie i\n\t"
+ "bx r0\n"
+
+ /* Normal context switch */
+ "0:\n\t"
+ "add r0, #16\n\t" /* ->V */
+ "ldr r1, [r0]\n\t"
+ "str r1, [sp]\n\t"
+ /**/
+ "add r0, #4\n\t"
+ "ldm r0!, {r4, r5, r6, r7}\n\t"
+ "ldm r0!, {r1, r2, r3}\n\t"
+ "mov r8, r1\n\t"
+ "mov r9, r2\n\t"
+ "mov r10, r3\n\t"
+ "ldm r0!, {r1, r2}\n\t"
+ "mov r11, r1\n\t"
+ "mov sp, r2\n\t"
+ "sub r0, #45\n\t"
+ "ldrb r1, [r0]\n\t" /* ->PRIO field. */
+ "cmp r1, #247\n\t"
+ "bhi 1f\n\t" /* Leave interrupt disabled if >= 248 */
+ /**/
+ /* Unmask interrupts. */
+ "cpsie i\n"
+ /**/
+ "1:\n\t"
+ /*
+ 0: r0
+ 4: r1
+ 8: r2
+ 12: r3
+ 16: r12
+ 20: lr
+ 24: pc
+ 28: psr
+ 32: possibly exists for alignment
+ [28 or 32] <-- pc
+ */
+ "ldr r0, [sp, #28]\n\t"
+ "lsl r1, r0, #23\n\t"
+ "bcc 2f\n\t"
+ /**/
+ "ldr r2, [sp, #24]\n\t"
+ "mov r1, #1\n\t"
+ "orr r2, r1\n\t" /* Ensure Thumb-mode */
+ "str r2, [sp, #32]\n\t"
+ "msr APSR_nzcvq, r0\n\t"
+ /**/
+ "ldr r0, [sp, #20]\n\t"
+ "mov lr, r0\n\t"
+ "ldr r0, [sp, #16]\n\t"
+ "mov r12, r0\n\t"
+ "pop {r0, r1, r2, r3}\n\t"
+ "add sp, #16\n\t"
+ "pop {pc}\n"
+ "2:\n\t"
+ "ldr r2, [sp, #24]\n\t"
+ "mov r1, #1\n\t"
+ "orr r2, r1\n\t" /* Ensure Thumb-mode */
+ "str r2, [sp, #28]\n\t"
+ "msr APSR_nzcvq, r0\n\t"
+ /**/
+ "ldr r0, [sp, #20]\n\t"
+ "mov lr, r0\n\t"
+ "ldr r0, [sp, #16]\n\t"
+ "mov r12, r0\n\t"
+ "pop {r0, r1, r2, r3}\n\t"
+ "add sp, #12\n\t"
+ "pop {pc}"
+ : "=r" (tp) /* Return value in R0 */
+ : "0" (tp)
+ : "memory");
+#endif
+
+ return (uintptr_t)tp;
+}
+
+extern void cause_link_time_error_unexpected_size_of_struct_chx_thread (void);
+
+static struct chx_thread *
+chopstx_create_arch (uintptr_t stack_addr, size_t stack_size,
+ voidfunc thread_entry, void *arg)
+{
+ struct chx_thread *tp;
+ void *stack;
+ struct chx_stack_regs *p;
+
+ if (CHOPSTX_THREAD_SIZE != sizeof(struct chx_thread))
+ cause_link_time_error_unexpected_size_of_struct_chx_thread ();
+
+ if (stack_size < sizeof (struct chx_thread) + 8 * sizeof (uint32_t))
+ chx_fatal (CHOPSTX_ERR_THREAD_CREATE);
+
+ stack = (void *)(stack_addr + stack_size - sizeof (struct chx_thread)
+ - sizeof (struct chx_stack_regs));
+ memset (stack, 0, sizeof (struct chx_stack_regs));
+ tp = (struct chx_thread *)(stack + sizeof (struct chx_stack_regs));
+ p = (struct chx_stack_regs *)stack;
+ p->reg[REG_R0] = (uint32_t)arg;
+ p->reg[REG_LR] = (uint32_t)chopstx_exit;
+ p->reg[REG_PC] = (uint32_t)thread_entry;
+ p->reg[REG_XPSR] = INITIAL_XPSR;
+
+ memset (&tp->tc, 0, sizeof (tp->tc));
+ tp->tc.reg[REG_SP] = (uint32_t)stack;
+
+ return tp;
+}
+
+/*
+ * Lower layer architecture specific exception handling entries.
+ *
+ */
+
+void __attribute__ ((naked))
+preempt (void)
+{
+ register struct chx_thread *tp asm ("r0");
+ register struct chx_thread *cur asm ("r1");
+
+ asm volatile (
+#if defined(__ARM_ARCH_6M__)
+ "cpsid i\n\t"
+#else
+ "msr BASEPRI, r0\n\t"
+#endif
+ "ldr r2, =running\n\t"
+ "ldr r0, [r2]\n\t"
+ "mov r1, r0"
+ : "=r" (tp), "=r" (cur)
+ : "0" (CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED)
+ : "r2");
+
+ if (!cur)
+ /* It's idle thread. It's ok to clobber registers. */
+ ;
+ else
+ {
+ /* Save registers onto CHX_THREAD struct. */
+ asm volatile (
+ "add %0, #20\n\t"
+ "stm %0!, {r4, r5, r6, r7}\n\t"
+ "mov r2, r8\n\t"
+ "mov r3, r9\n\t"
+ "mov r4, r10\n\t"
+ "mov r5, r11\n\t"
+ "mrs r6, PSP\n\t" /* r13(=SP) in user space. */
+ "stm %0!, {r2, r3, r4, r5, r6}"
+ : "=r" (cur)
+ : "0" (cur)
+ /*
+ * Memory clobber constraint here is not accurate, but this
+ * works. R7 keeps its value, but having "r7" here prevents
+ * use of R7 before this asm statement.
+ */
+ : "r2", "r3", "r4", "r5", "r6", "r7", "memory");
+
+ if (tp)
+ {
+ if (tp->flag_sched_rr)
+ {
+ if (tp->state == THREAD_RUNNING)
+ {
+ chx_timer_dequeue (tp);
+ chx_ready_enqueue (tp);
+ }
+ /*
+ * It may be THREAD_READY after chx_timer_expired.
+ * Then, do nothing.
+ */
+ }
+ else
+ chx_ready_push (tp);
+ running = NULL;
+ }
+ }
+
+ /* Registers on stack (PSP): r0, r1, r2, r3, r12, lr, pc, xpsr */
+
+ tp = chx_ready_pop ();
+ if (tp && tp->flag_sched_rr)
+ {
+ chx_spin_lock (&q_timer.lock);
+ tp = chx_timer_insert (tp, PREEMPTION_USEC);
+ chx_spin_unlock (&q_timer.lock);
+ }
+
+ asm volatile (
+ ".L_CONTEXT_SWITCH:\n\t"
+ /* Now, r0 points to the thread to be switched. */
+ /* Put it to *running. */
+ "ldr r1, =running\n\t"
+ /* Update running. */
+ "str r0, [r1]\n\t"
+#if defined(__ARM_ARCH_6M__)
+ "cmp r0, #0\n\t"
+ "beq 1f\n\t"
+#else
+ "cbz r0, 1f\n\t"
+#endif
+ /**/
+ "add r0, #20\n\t"
+ "ldm r0!, {r4, r5, r6, r7}\n\t"
+#if defined(__ARM_ARCH_6M__)
+ "ldm r0!, {r1, r2, r3}\n\t"
+ "mov r8, r1\n\t"
+ "mov r9, r2\n\t"
+ "mov r10, r3\n\t"
+ "ldm r0!, {r1, r2}\n\t"
+ "mov r11, r1\n\t"
+ "msr PSP, r2\n\t"
+#else
+ "ldr r8, [r0], #4\n\t"
+ "ldr r9, [r0], #4\n\t"
+ "ldr r10, [r0], #4\n\t"
+ "ldr r11, [r0], #4\n\t"
+ "ldr r1, [r0], #4\n\t"
+ "msr PSP, r1\n\t"
+#endif
+ "sub r0, #45\n\t"
+ "ldrb r1, [r0]\n\t" /* ->PRIO field. */
+ "mov r0, #0\n\t"
+ "cmp r1, #247\n\t"
+ "bhi 0f\n\t" /* Leave interrupt disabled if >= 248 */
+ /**/
+ /* Unmask interrupts. */
+#if defined(__ARM_ARCH_6M__)
+ "cpsie i\n"
+#else
+ "msr BASEPRI, r0\n"
+#endif
+ /**/
+ "0:\n\t"
+ "sub r0, #3\n\t" /* EXC_RETURN to a thread with PSP */
+ "bx r0\n"
+ "1:\n\t"
+ /* Spawn an IDLE thread. */
+ "ldr r0, =__main_stack_end__-32\n\t"
+ "msr PSP, r0\n\t"
+ "mov r1, #0\n\t"
+ "mov r2, #0\n\t"
+ "mov r3, #0\n\t"
+ "stm r0!, {r1, r2, r3}\n\t"
+ "stm r0!, {r1, r2, r3}\n\t"
+ "ldr r1, =idle\n\t" /* PC = idle */
+ "mov r2, #0x010\n\t"
+ "lsl r2, r2, #20\n\t" /* xPSR = T-flag set (Thumb) */
+ "stm r0!, {r1, r2}\n\t"
+ /**/
+ /* Unmask interrupts. */
+ "mov r0, #0\n\t"
+#if defined(__ARM_ARCH_6M__)
+ "cpsie i\n\t"
+#else
+ "msr BASEPRI, r0\n"
+#endif
+ /**/
+ "sub r0, #3\n\t" /* EXC_RETURN to a thread with PSP */
+ "bx r0"
+ : /* no output */ : "r" (tp) : "memory");
+}
+
+#if defined(__ARM_ARCH_7M__)
+/*
+ * System call: switch to another thread.
+ * There are two cases:
+ * ORIG_R0=0 (SLEEP): Current RUNNING thread is already connected to
+ * something (mutex, cond, intr, etc.)
+ * ORIG_R0=1 (YIELD): Current RUNNING thread is active,
+ * it is needed to be enqueued to READY queue.
+ */
+void __attribute__ ((naked))
+svc (void)
+{
+ register struct chx_thread *tp asm ("r0");
+ register uint32_t orig_r0 asm ("r1");
+
+ asm ("ldr r1, =running\n\t"
+ "ldr r0, [r1]\n\t"
+ "add r1, r0, #20\n\t"
+ /* Save registers onto CHX_THREAD struct. */
+ "stm r1!, {r4, r5, r6, r7}\n\t"
+ "mov r2, r8\n\t"
+ "mov r3, r9\n\t"
+ "mov r4, r10\n\t"
+ "mov r5, r11\n\t"
+ "mrs r6, PSP\n\t" /* r13(=SP) in user space. */
+ "stm r1!, {r2, r3, r4, r5, r6}\n\t"
+ "ldr r1, [r6]"
+ : "=r" (tp), "=r" (orig_r0)
+ : /* no input */
+ : "r2", "r3", "r4", "r5", "r6", "memory");
+
+ if (orig_r0) /* yield */
+ {
+ if (tp->flag_sched_rr)
+ chx_timer_dequeue (tp);
+ chx_ready_enqueue (tp);
+ running = NULL;
+ }
+
+ tp = chx_ready_pop ();
+ if (tp && tp->flag_sched_rr)
+ {
+ chx_spin_lock (&q_timer.lock);
+ chx_timer_insert (tp, PREEMPTION_USEC);
+ chx_spin_unlock (&q_timer.lock);
+ }
+
+ asm volatile (
+ "cbz r0, 0f\n\t"
+ "ldr r1, [r0, #16]\n\t" /* ->V */
+ "str r1, [sp]\n\t"
+ "0:\n\t"
+ "b .L_CONTEXT_SWITCH"
+ : /* no output */ : "r" (tp) : "memory");
+}
+#endif
diff --git a/chopstx-cortex-m.h b/chopstx-cortex-m.h
new file mode 100644
index 0000000..36515f6
--- /dev/null
+++ b/chopstx-cortex-m.h
@@ -0,0 +1,12 @@
+/*
+ * The thread context: specific to ARM Cortex-M0/M3.
+ *
+ * In this structure, it's only partial information; Other part of the
+ * context is on the stack.
+ *
+ */
+struct tcontext {
+ uint32_t reg[9]; /* r4, r5, r6, r7, r8, r9, r10, r11, r13(sp) */
+};
+
+typedef struct tcontext tcontext_t;
diff --git a/chopstx.c b/chopstx.c
index 6dc1da5..fb90026 100644
--- a/chopstx.c
+++ b/chopstx.c
@@ -54,159 +54,7 @@
#define MHZ 72
#endif
-/*
- * Exception priority: lower has higher precedence.
- *
- * Cortex-M3
- * =====================================
- * Prio 0x30: svc
- * ---------------------
- * Prio 0x40: thread temporarily inhibiting schedule for critical region
- * ...
- * Prio 0xb0: systick, external interrupt
- * Prio 0xc0: pendsv
- * =====================================
- *
- * Cortex-M0
- * =====================================
- * Prio 0x00: thread temporarily inhibiting schedule for critical region
- * ...
- * Prio 0x40: systick, external interrupt
- * Prio 0x80: pendsv
- * Prio 0x80: svc
- * =====================================
- */
-
-#define CPU_EXCEPTION_PRIORITY_CLEAR 0
-
-#if defined(__ARM_ARCH_6M__)
-#define CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED 0x00
-/* ... */
-#define CPU_EXCEPTION_PRIORITY_SYSTICK CPU_EXCEPTION_PRIORITY_INTERRUPT
-#define CPU_EXCEPTION_PRIORITY_INTERRUPT 0x40
-#define CPU_EXCEPTION_PRIORITY_PENDSV 0x80
-#define CPU_EXCEPTION_PRIORITY_SVC 0x80 /* No use in this arch */
-#elif defined(__ARM_ARCH_7M__)
-#define CPU_EXCEPTION_PRIORITY_SVC 0x30
-
-#define CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED 0x40
-/* ... */
-#define CPU_EXCEPTION_PRIORITY_SYSTICK CPU_EXCEPTION_PRIORITY_INTERRUPT
-#define CPU_EXCEPTION_PRIORITY_INTERRUPT 0xb0
-#define CPU_EXCEPTION_PRIORITY_PENDSV 0xc0
-#else
-#error "no support for this arch"
-#endif
-
-/*
- * Lower layer architecture specific functions.
- *
- * system tick and interrupt
- */
-
-/*
- * System tick
- */
-/* SysTick registers. */
-static volatile uint32_t *const SYST_CSR = (uint32_t *)0xE000E010;
-static volatile uint32_t *const SYST_RVR = (uint32_t *)0xE000E014;
-static volatile uint32_t *const SYST_CVR = (uint32_t *)0xE000E018;
-
-static void
-chx_systick_reset (void)
-{
- *SYST_RVR = 0;
- *SYST_CVR = 0;
- *SYST_CSR = 7;
-}
-
-static void
-chx_systick_reload (uint32_t ticks)
-{
- *SYST_RVR = ticks;
- *SYST_CVR = 0; /* write (any) to clear the counter to reload. */
- *SYST_RVR = 0;
-}
-
-static uint32_t
-chx_systick_get (void)
-{
- return *SYST_CVR;
-}
-
-static uint32_t usec_to_ticks (uint32_t usec)
-{
- return usec * MHZ;
-}
-
-/*
- * Interrupt Handling
- */
-
-/* NVIC: Nested Vectored Interrupt Controller. */
-struct NVIC {
- volatile uint32_t ISER[8];
- uint32_t unused1[24];
- volatile uint32_t ICER[8];
- uint32_t unused2[24];
- volatile uint32_t ISPR[8];
- uint32_t unused3[24];
- volatile uint32_t ICPR[8];
- uint32_t unused4[24];
- volatile uint32_t IABR[8];
- uint32_t unused5[56];
- volatile uint32_t IPR[60];
-};
-
-static struct NVIC *const NVIC = (struct NVIC *)0xE000E100;
-#define NVIC_ISER(n) (NVIC->ISER[n >> 5])
-#define NVIC_ICER(n) (NVIC->ICER[n >> 5])
-#define NVIC_ICPR(n) (NVIC->ICPR[n >> 5])
-#define NVIC_IPR(n) (NVIC->IPR[n >> 2])
-
-
-static void
-chx_enable_intr (uint8_t irq_num)
-{
- NVIC_ISER (irq_num) = 1 << (irq_num & 0x1f);
-}
-
-static void
-chx_clr_intr (uint8_t irq_num)
-{ /* Clear pending interrupt. */
- NVIC_ICPR (irq_num) = 1 << (irq_num & 0x1f);
-}
-
-static void
-chx_disable_intr (uint8_t irq_num)
-{
- NVIC_ICER (irq_num) = 1 << (irq_num & 0x1f);
-}
-
-static void
-chx_set_intr_prio (uint8_t n)
-{
- unsigned int sh = (n & 3) << 3;
-
- NVIC_IPR (n) = (NVIC_IPR(n) & ~(0xFF << sh))
- | (CPU_EXCEPTION_PRIORITY_INTERRUPT << sh);
-}
-
-static volatile uint32_t *const ICSR = (uint32_t *)0xE000ED04;
-
-/* Priority control. */
-static uint32_t *const AIRCR = (uint32_t *)0xE000ED0C;
-static uint32_t *const SHPR2 = (uint32_t *)0xE000ED1C;
-static uint32_t *const SHPR3 = (uint32_t *)0xE000ED20;
-
-static void
-chx_prio_init (void)
-{
- *AIRCR = 0x05FA0000 | ( 5 << 8); /* PRIGROUP = 5, 2-bit:2-bit. */
- *SHPR2 = (CPU_EXCEPTION_PRIORITY_SVC << 24);
- *SHPR3 = ((CPU_EXCEPTION_PRIORITY_SYSTICK << 24)
- | (CPU_EXCEPTION_PRIORITY_PENDSV << 16));
-}
+typedef void *(voidfunc) (void *);
void __attribute__((weak)) chx_fatal (uint32_t err_code);
@@ -224,6 +72,12 @@ chx_fatal (uint32_t err_code)
for (;;);
}
+/* Include the definition of thread context structure. */
+#ifdef GNU_LINUX_EMULATION
+#include "chopstx-gnu-linux.h"
+#else
+#include "chopstx-cortex-m.h"
+#endif
/* RUNNING: the current thread. */
struct chx_thread *running;
@@ -242,10 +96,15 @@ static struct chx_queue q_timer;
/* Queue of threads which wait for the exit of some thread. */
static struct chx_queue q_join;
+/* Queue of threads which wait for some interrupts. */
+static struct chx_queue q_intr;
/* Forward declaration(s). */
static void chx_request_preemption (uint16_t prio);
static int chx_wakeup (struct chx_pq *p);
+static struct chx_thread * chx_timer_insert (struct chx_thread *tp, uint32_t usec);
+static void chx_timer_dequeue (struct chx_thread *tp);
+
/**************/
@@ -264,28 +123,6 @@ static void chx_spin_unlock (struct chx_spinlock *lk)
(void)lk;
}
-/* The thread context: specific to ARM Cortex-M3 now. */
-struct tcontext {
- uint32_t reg[9]; /* r4, r5, r6, r7, r8, r9, r10, r11, r13(sp) */
-};
-
-/* Saved registers on the stack. */
-struct chx_stack_regs {
- uint32_t reg[8]; /* r0, r1, r2, r3, r12, lr, pc, xpsr */
-};
-
-/*
- * Constants for ARM.
- */
-#define REG_SP 8
-
-#define REG_R0 0
-#define REG_LR 5
-#define REG_PC 6
-#define REG_XPSR 7
-
-#define INITIAL_XPSR 0x01000000 /* T=1 */
-
/**************/
struct chx_pq {
struct chx_pq *next, *prev;
@@ -329,41 +166,12 @@ struct chx_thread { /* inherits PQ */
uint32_t prio : 8;
struct chx_qh *parent;
uintptr_t v;
- struct tcontext tc;
+ tcontext_t tc;
struct chx_mtx *mutex_list;
struct chx_cleanup *clp;
};
-static void
-chx_cpu_sched_lock (void)
-{
- if (running->prio < CHOPSTX_PRIO_INHIBIT_PREEMPTION)
- {
-#if defined(__ARM_ARCH_6M__)
- asm volatile ("cpsid i" : : : "memory");
-#else
- register uint32_t tmp = CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED;
- asm volatile ("msr BASEPRI, %0" : : "r" (tmp) : "memory");
-#endif
- }
-}
-
-static void
-chx_cpu_sched_unlock (void)
-{
- if (running->prio < CHOPSTX_PRIO_INHIBIT_PREEMPTION)
- {
-#if defined(__ARM_ARCH_6M__)
- asm volatile ("cpsie i" : : : "memory");
-#else
- register uint32_t tmp = CPU_EXCEPTION_PRIORITY_CLEAR;
- asm volatile ("msr BASEPRI, %0" : : "r" (tmp) : "memory");
-#endif
- }
-}
-
-
/*
* Double linked list handling.
*/
@@ -482,18 +290,16 @@ chx_ready_enqueue (struct chx_thread *tp)
chx_spin_unlock (&q_ready.lock);
}
-static void __attribute__((naked, used))
-idle (void)
-{
-#if defined(USE_WFI_FOR_IDLE)
- for (;;)
- asm volatile ("wfi" : : : "memory");
+/*
+ * Here comes architecture specific code.
+ */
+
+#ifdef GNU_LINUX_EMULATION
+#include "chopstx-gnu-linux.c"
#else
- for (;;);
+#include "chopstx-cortex-m.c"
#endif
-}
-
-
+
static void
chx_set_timer (struct chx_thread *tp, uint32_t ticks)
{
@@ -607,37 +413,10 @@ chx_timer_expired (void)
}
}
- chx_request_preemption (prio);
chx_spin_unlock (&q_timer.lock);
+ chx_request_preemption (prio);
}
-/* Queue of threads which wait for some interrupts. */
-static struct chx_queue q_intr;
-
-void
-chx_handle_intr (void)
-{
- struct chx_pq *p;
- register uint32_t irq_num;
-
- asm volatile ("mrs %0, IPSR\n\t"
- "sub %0, #16" /* Exception # - 16 = interrupt number. */
- : "=r" (irq_num) : /* no input */ : "memory");
-
- chx_disable_intr (irq_num);
- chx_spin_lock (&q_intr.lock);
- for (p = q_intr.q.next; p != (struct chx_pq *)&q_intr.q; p = p->next)
- if (p->v == irq_num)
- { /* should be one at most. */
- struct chx_px *px = (struct chx_px *)p;
-
- ll_dequeue (p);
- chx_wakeup (p);
- chx_request_preemption (px->master->prio);
- break;
- }
- chx_spin_unlock (&q_intr.lock);
-}
void
chx_systick_init (void)
@@ -660,7 +439,8 @@ void
chx_init (struct chx_thread *tp)
{
chx_prio_init ();
- memset (&tp->tc, 0, sizeof (tp->tc));
+ chx_init_arch (tp);
+
q_ready.q.next = q_ready.q.prev = (struct chx_pq *)&q_ready.q;
chx_spin_init (&q_ready.lock);
q_timer.q.next = q_timer.q.prev = (struct chx_pq *)&q_timer.q;
@@ -692,203 +472,9 @@ chx_init (struct chx_thread *tp)
chopstx_main = (chopstx_t)tp;
}
-
-
-static void
-chx_request_preemption (uint16_t prio)
-{
- if (running == NULL || (uint16_t)running->prio < prio)
- {
- *ICSR = (1 << 28);
- asm volatile ("" : : : "memory");
- }
-}
-
-
#define CHX_SLEEP 0
#define CHX_YIELD 1
-/*
- * chx_sched: switch to another thread.
- *
- * There are two cases:
- * YIELD=0 (SLEEP): Current RUNNING thread is already connected to
- * something (mutex, cond, intr, etc.)
- * YIELD=1 (YIELD): Current RUNNING thread is active,
- * it is needed to be enqueued to READY queue.
- *
- * For Cortex-M0, this should be AAPCS-compliant function entry, so we
- * put "noinline" attribute.
- *
- * AAPCS: ARM Architecture Procedure Call Standard
- *
- * Returns:
- * 1 on wakeup by others.
- * 0 on normal wakeup.
- * -1 on cancellation.
- */
-static uintptr_t __attribute__ ((naked, noinline))
-chx_sched (uint32_t yield)
-{
- register struct chx_thread *tp asm ("r0");
-
-#if defined(__ARM_ARCH_7M__)
- asm volatile (
- "svc #0\n\t"
- "bx lr"
- : "=r" (tp) : "0" (yield): "memory");
-#else
- register uint32_t arg_yield asm ("r1");
-
- /* Build stack data as if it were an exception entry. */
- /*
- * r0: 0 scratch
- * r1: 0 scratch
- * r2: 0 scratch
- * r3: 0 scratch
- * r12: 0 scratch
- * lr as-is
- * pc: return address (= lr)
- * psr: INITIAL_XPSR scratch
- */
- asm ("mov r1, lr\n\t"
- "mov r2, r1\n\t"
- "mov r3, #128\n\t"
- "lsl r3, #17\n\t"
- "push {r1, r2, r3}\n\t"
- "mov r1, #0\n\t"
- "mov r2, r1\n\t"
- "mov r3, r1\n\t"
- "push {r1, r2, r3}\n\t"
- "push {r1, r2}"
- : /* no output*/
- : /* no input */
- : "r1", "r2", "r3", "memory");
-
- /* Save registers onto CHX_THREAD struct. */
- asm ("mov r1, r0\n\t"
- "ldr r2, =running\n\t"
- "ldr r0, [r2]\n\t"
- "add r0, #20\n\t"
- "stm r0!, {r4, r5, r6, r7}\n\t"
- "mov r2, r8\n\t"
- "mov r3, r9\n\t"
- "mov r4, r10\n\t"
- "mov r5, r11\n\t"
- "mov r6, sp\n\t"
- "stm r0!, {r2, r3, r4, r5, r6}\n\t"
- "sub r0, #56"
- : "=r" (tp), "=r" (arg_yield)
- : "0" (yield)
- : "r2", "r3", "r4", "r5", "r6", "r7", "memory");
-
- if (arg_yield)
- {
- if (tp->flag_sched_rr)
- chx_timer_dequeue (tp);
- chx_ready_enqueue (tp);
- }
-
- tp = chx_ready_pop ();
- if (tp && tp->flag_sched_rr)
- {
- chx_spin_lock (&q_timer.lock);
- tp = chx_timer_insert (tp, PREEMPTION_USEC);
- chx_spin_unlock (&q_timer.lock);
- }
-
- asm volatile (/* Now, r0 points to the thread to be switched. */
- /* Put it to *running. */
- "ldr r1, =running\n\t"
- /* Update running. */
- "str r0, [r1]\n\t"
- "cmp r0, #0\n\t"
- "bne 0f\n\t"
-
- /* Spawn an IDLE thread. */
- "ldr r1, =__main_stack_end__\n\t"
- "mov sp, r1\n\t"
- "ldr r0, =idle\n\t" /* PC = idle */
- /**/
- /* Unmask interrupts. */
- "cpsie i\n\t"
- "bx r0\n"
-
- /* Normal context switch */
- "0:\n\t"
- "add r0, #16\n\t" /* ->V */
- "ldr r1, [r0]\n\t"
- "str r1, [sp]\n\t"
- /**/
- "add r0, #4\n\t"
- "ldm r0!, {r4, r5, r6, r7}\n\t"
- "ldm r0!, {r1, r2, r3}\n\t"
- "mov r8, r1\n\t"
- "mov r9, r2\n\t"
- "mov r10, r3\n\t"
- "ldm r0!, {r1, r2}\n\t"
- "mov r11, r1\n\t"
- "mov sp, r2\n\t"
- "sub r0, #45\n\t"
- "ldrb r1, [r0]\n\t" /* ->PRIO field. */
- "cmp r1, #247\n\t"
- "bhi 1f\n\t" /* Leave interrupt disabled if >= 248 */
- /**/
- /* Unmask interrupts. */
- "cpsie i\n"
- /**/
- "1:\n\t"
- /*
- 0: r0
- 4: r1
- 8: r2
- 12: r3
- 16: r12
- 20: lr
- 24: pc
- 28: psr
- 32: possibly exists for alignment
- [28 or 32] <-- pc
- */
- "ldr r0, [sp, #28]\n\t"
- "lsl r1, r0, #23\n\t"
- "bcc 2f\n\t"
- /**/
- "ldr r2, [sp, #24]\n\t"
- "mov r1, #1\n\t"
- "orr r2, r1\n\t" /* Ensure Thumb-mode */
- "str r2, [sp, #32]\n\t"
- "msr APSR_nzcvq, r0\n\t"
- /**/
- "ldr r0, [sp, #20]\n\t"
- "mov lr, r0\n\t"
- "ldr r0, [sp, #16]\n\t"
- "mov r12, r0\n\t"
- "pop {r0, r1, r2, r3}\n\t"
- "add sp, #16\n\t"
- "pop {pc}\n"
- "2:\n\t"
- "ldr r2, [sp, #24]\n\t"
- "mov r1, #1\n\t"
- "orr r2, r1\n\t" /* Ensure Thumb-mode */
- "str r2, [sp, #28]\n\t"
- "msr APSR_nzcvq, r0\n\t"
- /**/
- "ldr r0, [sp, #20]\n\t"
- "mov lr, r0\n\t"
- "ldr r0, [sp, #16]\n\t"
- "mov r12, r0\n\t"
- "pop {r0, r1, r2, r3}\n\t"
- "add sp, #12\n\t"
- "pop {pc}"
- : "=r" (tp) /* Return value in R0 */
- : "0" (tp)
- : "memory");
-#endif
-
- return (uintptr_t)tp;
-}
-
/*
* Wakeup the thread TP. Called with schedule lock held.
@@ -1002,10 +588,6 @@ chx_mutex_unlock (chopstx_mutex_t *mutex)
#define CHOPSTX_PRIO_MASK ((1 << CHOPSTX_PRIO_BITS) - 1)
-typedef void *(voidfunc) (void *);
-
-extern void cause_link_time_error_unexpected_size_of_struct_chx_thread (void);
-
/**
* chopstx_create - Create a thread
* @flags_and_prio: Flags and priority
@@ -1022,29 +604,10 @@ chopstx_create (uint32_t flags_and_prio,
voidfunc thread_entry, void *arg)
{
struct chx_thread *tp;
- void *stack;
- struct chx_stack_regs *p;
chopstx_prio_t prio = (flags_and_prio & CHOPSTX_PRIO_MASK);
- if (CHOPSTX_THREAD_SIZE != sizeof(struct chx_thread))
- cause_link_time_error_unexpected_size_of_struct_chx_thread ();
-
- if (stack_size < sizeof (struct chx_thread) + 8 * sizeof (uint32_t))
- chx_fatal (CHOPSTX_ERR_THREAD_CREATE);
-
- stack = (void *)(stack_addr + stack_size - sizeof (struct chx_thread)
- - sizeof (struct chx_stack_regs));
- memset (stack, 0, sizeof (struct chx_stack_regs));
- tp = (struct chx_thread *)(stack + sizeof (struct chx_stack_regs));
- p = (struct chx_stack_regs *)stack;
- p->reg[REG_R0] = (uint32_t)arg;
- p->reg[REG_LR] = (uint32_t)chopstx_exit;
- p->reg[REG_PC] = (uint32_t)thread_entry;
- p->reg[REG_XPSR] = INITIAL_XPSR;
-
- memset (&tp->tc, 0, sizeof (tp->tc));
- tp->tc.reg[REG_SP] = (uint32_t)stack;
-
+ tp = chopstx_create_arch (stack_addr, stack_size, thread_entry,
+ arg);
tp->next = tp->prev = (struct chx_pq *)tp;
tp->mutex_list = NULL;
tp->clp = NULL;
@@ -1906,213 +1469,3 @@ chopstx_setpriority (chopstx_prio_t prio_new)
return prio_orig;
}
-
-/*
- * Lower layer architecture specific exception handling entries.
- *
- */
-
-void __attribute__ ((naked))
-preempt (void)
-{
- register struct chx_thread *tp asm ("r0");
- register struct chx_thread *cur asm ("r1");
-
- asm volatile (
-#if defined(__ARM_ARCH_6M__)
- "cpsid i\n\t"
-#else
- "msr BASEPRI, r0\n\t"
-#endif
- "ldr r2, =running\n\t"
- "ldr r0, [r2]\n\t"
- "mov r1, r0"
- : "=r" (tp), "=r" (cur)
- : "0" (CPU_EXCEPTION_PRIORITY_INHIBIT_SCHED)
- : "r2");
-
- if (!cur)
- /* It's idle thread. It's ok to clobber registers. */
- ;
- else
- {
- /* Save registers onto CHX_THREAD struct. */
- asm volatile (
- "add %0, #20\n\t"
- "stm %0!, {r4, r5, r6, r7}\n\t"
- "mov r2, r8\n\t"
- "mov r3, r9\n\t"
- "mov r4, r10\n\t"
- "mov r5, r11\n\t"
- "mrs r6, PSP\n\t" /* r13(=SP) in user space. */
- "stm %0!, {r2, r3, r4, r5, r6}"
- : "=r" (cur)
- : "0" (cur)
- /*
- * Memory clobber constraint here is not accurate, but this
- * works. R7 keeps its value, but having "r7" here prevents
- * use of R7 before this asm statement.
- */
- : "r2", "r3", "r4", "r5", "r6", "r7", "memory");
-
- if (tp)
- {
- if (tp->flag_sched_rr)
- {
- if (tp->state == THREAD_RUNNING)
- {
- chx_timer_dequeue (tp);
- chx_ready_enqueue (tp);
- }
- /*
- * It may be THREAD_READY after chx_timer_expired.
- * Then, do nothing.
- */
- }
- else
- chx_ready_push (tp);
- running = NULL;
- }
- }
-
- /* Registers on stack (PSP): r0, r1, r2, r3, r12, lr, pc, xpsr */
-
- tp = chx_ready_pop ();
- if (tp && tp->flag_sched_rr)
- {
- chx_spin_lock (&q_timer.lock);
- tp = chx_timer_insert (tp, PREEMPTION_USEC);
- chx_spin_unlock (&q_timer.lock);
- }
-
- asm volatile (
- ".L_CONTEXT_SWITCH:\n\t"
- /* Now, r0 points to the thread to be switched. */
- /* Put it to *running. */
- "ldr r1, =running\n\t"
- /* Update running. */
- "str r0, [r1]\n\t"
-#if defined(__ARM_ARCH_6M__)
- "cmp r0, #0\n\t"
- "beq 1f\n\t"
-#else
- "cbz r0, 1f\n\t"
-#endif
- /**/
- "add r0, #20\n\t"
- "ldm r0!, {r4, r5, r6, r7}\n\t"
-#if defined(__ARM_ARCH_6M__)
- "ldm r0!, {r1, r2, r3}\n\t"
- "mov r8, r1\n\t"
- "mov r9, r2\n\t"
- "mov r10, r3\n\t"
- "ldm r0!, {r1, r2}\n\t"
- "mov r11, r1\n\t"
- "msr PSP, r2\n\t"
-#else
- "ldr r8, [r0], #4\n\t"
- "ldr r9, [r0], #4\n\t"
- "ldr r10, [r0], #4\n\t"
- "ldr r11, [r0], #4\n\t"
- "ldr r1, [r0], #4\n\t"
- "msr PSP, r1\n\t"
-#endif
- "sub r0, #45\n\t"
- "ldrb r1, [r0]\n\t" /* ->PRIO field. */
- "mov r0, #0\n\t"
- "cmp r1, #247\n\t"
- "bhi 0f\n\t" /* Leave interrupt disabled if >= 248 */
- /**/
- /* Unmask interrupts. */
-#if defined(__ARM_ARCH_6M__)
- "cpsie i\n"
-#else
- "msr BASEPRI, r0\n"
-#endif
- /**/
- "0:\n\t"
- "sub r0, #3\n\t" /* EXC_RETURN to a thread with PSP */
- "bx r0\n"
- "1:\n\t"
- /* Spawn an IDLE thread. */
- "ldr r0, =__main_stack_end__-32\n\t"
- "msr PSP, r0\n\t"
- "mov r1, #0\n\t"
- "mov r2, #0\n\t"
- "mov r3, #0\n\t"
- "stm r0!, {r1, r2, r3}\n\t"
- "stm r0!, {r1, r2, r3}\n\t"
- "ldr r1, =idle\n\t" /* PC = idle */
- "mov r2, #0x010\n\t"
- "lsl r2, r2, #20\n\t" /* xPSR = T-flag set (Thumb) */
- "stm r0!, {r1, r2}\n\t"
- /**/
- /* Unmask interrupts. */
- "mov r0, #0\n\t"
-#if defined(__ARM_ARCH_6M__)
- "cpsie i\n\t"
-#else
- "msr BASEPRI, r0\n"
-#endif
- /**/
- "sub r0, #3\n\t" /* EXC_RETURN to a thread with PSP */
- "bx r0"
- : /* no output */ : "r" (tp) : "memory");
-}
-
-#if defined(__ARM_ARCH_7M__)
-/*
- * System call: switch to another thread.
- * There are two cases:
- * ORIG_R0=0 (SLEEP): Current RUNNING thread is already connected to
- * something (mutex, cond, intr, etc.)
- * ORIG_R0=1 (YIELD): Current RUNNING thread is active,
- * it is needed to be enqueued to READY queue.
- */
-void __attribute__ ((naked))
-svc (void)
-{
- register struct chx_thread *tp asm ("r0");
- register uint32_t orig_r0 asm ("r1");
-
- asm ("ldr r1, =running\n\t"
- "ldr r0, [r1]\n\t"
- "add r1, r0, #20\n\t"
- /* Save registers onto CHX_THREAD struct. */
- "stm r1!, {r4, r5, r6, r7}\n\t"
- "mov r2, r8\n\t"
- "mov r3, r9\n\t"
- "mov r4, r10\n\t"
- "mov r5, r11\n\t"
- "mrs r6, PSP\n\t" /* r13(=SP) in user space. */
- "stm r1!, {r2, r3, r4, r5, r6}\n\t"
- "ldr r1, [r6]"
- : "=r" (tp), "=r" (orig_r0)
- : /* no input */
- : "r2", "r3", "r4", "r5", "r6", "memory");
-
- if (orig_r0) /* yield */
- {
- if (tp->flag_sched_rr)
- chx_timer_dequeue (tp);
- chx_ready_enqueue (tp);
- running = NULL;
- }
-
- tp = chx_ready_pop ();
- if (tp && tp->flag_sched_rr)
- {
- chx_spin_lock (&q_timer.lock);
- chx_timer_insert (tp, PREEMPTION_USEC);
- chx_spin_unlock (&q_timer.lock);
- }
-
- asm volatile (
- "cbz r0, 0f\n\t"
- "ldr r1, [r0, #16]\n\t" /* ->V */
- "str r1, [sp]\n\t"
- "0:\n\t"
- "b .L_CONTEXT_SWITCH"
- : /* no output */ : "r" (tp) : "memory");
-}
-#endif
--
cgit v1.2.3