Factor out arch dependent code.
This commit is contained in:
NIIBE Yutaka
13
ChangeLog
13
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.
|
||||
|
||||
707
chopstx-cortex-m.c
Normal file
707
chopstx-cortex-m.c
Normal file
@@ -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
|
||||
12
chopstx-cortex-m.h
Normal file
12
chopstx-cortex-m.h
Normal file
@@ -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;
|
||||
699
chopstx.c
699
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
|
||||
|
||||
Reference in New Issue
Block a user