From ce2264b17ca92a8b64e206655fabe1b952d50dde Mon Sep 17 00:00:00 2001 From: gdisirio Date: Tue, 6 Aug 2013 14:58:58 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/branches/kernel_3_dev@6092 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- demos/ARMCM4-STM32F303-DISCOVERY/chconf.h | 2 +- os/kernel/ports/ARMCMx/chcore.c | 55 ++ os/kernel/ports/ARMCMx/chcore.h | 186 +++++++ os/kernel/ports/ARMCMx/chcore_v7m.h | 611 +++++++++++++++++++++++ os/kernel/ports/ARMCMx/compilers/GCC/chcoreasm.s | 74 +++ os/kernel/ports/ARMCMx/devices/STM32F30x/port.mk | 20 +- 6 files changed, 937 insertions(+), 11 deletions(-) create mode 100644 os/kernel/ports/ARMCMx/chcore_v7m.h diff --git a/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h b/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h index ad7cc685a..36fb6fe76 100644 --- a/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h +++ b/demos/ARMCM4-STM32F303-DISCOVERY/chconf.h @@ -368,7 +368,7 @@ * @note The default is @p FALSE. */ #if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__) -#define CH_DBG_SYSTEM_STATE_CHECK FALSE +#define CH_DBG_SYSTEM_STATE_CHECK TRUE #endif /** diff --git a/os/kernel/ports/ARMCMx/chcore.c b/os/kernel/ports/ARMCMx/chcore.c index e69de29bb..c66ce160c 100644 --- a/os/kernel/ports/ARMCMx/chcore.c +++ b/os/kernel/ports/ARMCMx/chcore.c @@ -0,0 +1,55 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 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 . +*/ + +/** + * @file ARMCMx/chcore.c + * @brief ARM Cortex-Mx port code. + * + * @addtogroup ARMCMx_CORE + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** @} */ diff --git a/os/kernel/ports/ARMCMx/chcore.h b/os/kernel/ports/ARMCMx/chcore.h index e69de29bb..e1b626351 100644 --- a/os/kernel/ports/ARMCMx/chcore.h +++ b/os/kernel/ports/ARMCMx/chcore.h @@ -0,0 +1,186 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 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 . +*/ + +/** + * @file ARMCMx/chcore.h + * @brief ARM Cortex-Mx port macros and structures. + * + * @addtogroup ARMCMx_CORE + * @{ + */ + +#ifndef _CHCORE_H_ +#define _CHCORE_H_ + +/*===========================================================================*/ +/* Module constants. */ +/*===========================================================================*/ + +/** + * @name Architecture and Compiler + * @{ + */ +/** + * @brief Macro defining a generic ARM architecture. + */ +#define CH_ARCHITECTURE_ARM + +/** + * @brief Name of the compiler supported by this port. + */ +#define CH_COMPILER_NAME "GCC " __VERSION__ +/** @} */ + +/** + * @name Cortex-M variants + * @{ + */ +#define CORTEX_M0 0 /**< @brief Cortex-M0 variant. */ +#define CORTEX_M0PLUS 1 /**< @brief Cortex-M0+ variant. */ +#define CORTEX_M1 2 /**< @brief Cortex-M1 variant. */ +#define CORTEX_M3 3 /**< @brief Cortex-M3 variant. */ +#define CORTEX_M4 4 /**< @brief Cortex-M4 variant. */ +/** @} */ + +/* Inclusion of the Cortex-Mx implementation specific parameters.*/ +#include "cmparams.h" + +/*===========================================================================*/ +/* Module pre-compile time settings. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Derived constants and error checks. */ +/*===========================================================================*/ + +#if !defined(_FROM_ASM_) + +/* + * Inclusion of the appropriate CMSIS header for the selected device. + */ +#if CORTEX_MODEL == CORTEX_M0 +#include "core_cm0.h" +#elif CORTEX_MODEL == CORTEX_M0PLUS +#include "core_cm0plus.h" +#elif CORTEX_MODEL == CORTEX_M3 +#include "core_cm3.h" +#elif CORTEX_MODEL == CORTEX_M4 +#include "core_cm4.h" +#else +#error "unknown or unsupported Cortex-M model" +#endif + +#endif /* !defined(_FROM_ASM_) */ + +/*===========================================================================*/ +/* Module data structures and types. */ +/*===========================================================================*/ + +#if !defined(_FROM_ASM_) + +/* The following declarations are there just for Doxygen documentation, the + real declarations are inside the sub-headers being specific for the + sub-architectures.*/ +#if defined(__DOXYGEN__) +/** + * @brief Stack and memory alignment enforcement. + * @note In this architecture the stack alignment is enforced to 64 bits, + * 32 bits alignment is supported by hardware but deprecated by ARM, + * the implementation choice is to not offer the option. + */ +typedef uint64_t stkalign_t; + +/** + * @brief Interrupt saved context. + * @details This structure represents the stack frame saved during a + * preemption-capable interrupt handler. + * @note It is implemented to match the Cortex-Mx exception context. + */ +struct extctx {}; + +/** + * @brief System saved context. + * @details This structure represents the inner stack frame during a context + * switching. + */ +struct intctx {}; +#endif /* defined(__DOXYGEN__) */ + +#endif /* !defined(_FROM_ASM_) */ + +/*===========================================================================*/ +/* Module macros. */ +/*===========================================================================*/ + +/** + * @brief Total priority levels. + */ +#define CORTEX_PRIORITY_LEVELS (1 << CORTEX_PRIORITY_BITS) + +/** + * @brief Minimum priority level. + * @details This minimum priority level is calculated from the number of + * priority bits supported by the specific Cortex-Mx implementation. + */ +#define CORTEX_MINIMUM_PRIORITY (CORTEX_PRIORITY_LEVELS - 1) + +/** + * @brief Maximum priority level. + * @details The maximum allowed priority level is always zero. + */ +#define CORTEX_MAXIMUM_PRIORITY 0 + +/** + * @brief Priority level verification macro. + */ +#define CORTEX_IS_VALID_PRIORITY(n) \ + (((n) >= 0) && ((n) < CORTEX_PRIORITY_LEVELS)) + +/** + * @brief Priority level verification macro. + */ +#define CORTEX_IS_VALID_KERNEL_PRIORITY(n) \ + (((n) >= CORTEX_MAX_KERNEL_PRIORITY) && ((n) < CORTEX_PRIORITY_LEVELS)) + +/** + * @brief Priority level to priority mask conversion macro. + */ +#define CORTEX_PRIORITY_MASK(n) \ + ((n) << (8 - CORTEX_PRIORITY_BITS)) + +/*===========================================================================*/ +/* External declarations. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module inline functions. */ +/*===========================================================================*/ + +/* Includes the sub-architecture-specific part.*/ +#if (CORTEX_MODEL == CORTEX_M0) || (CORTEX_MODEL == CORTEX_M0PLUS) || \ + (CORTEX_MODEL == CORTEX_M1) +#include "chcore_v6m.h" +#elif (CORTEX_MODEL == CORTEX_M3) || (CORTEX_MODEL == CORTEX_M4) +#include "chcore_v7m.h" +#endif + +#endif /* _CHCORE_H_ */ + +/** @} */ diff --git a/os/kernel/ports/ARMCMx/chcore_v7m.h b/os/kernel/ports/ARMCMx/chcore_v7m.h new file mode 100644 index 000000000..118638469 --- /dev/null +++ b/os/kernel/ports/ARMCMx/chcore_v7m.h @@ -0,0 +1,611 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 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 . +*/ + +/** + * @file ARMCMx/chcore_v7m.h + * @brief ARMv7-M architecture port macros and structures. + * + * @addtogroup ARMCMx_V7M_CORE + * @{ + */ + +#ifndef _CHCORE_V7M_H_ +#define _CHCORE_V7M_H_ + +/*===========================================================================*/ +/* Module constants. */ +/*===========================================================================*/ + +/** + * @name Architecture and Compiler + * @{ + */ +#if (CORTEX_MODEL == CORTEX_M3) || defined(__DOXYGEN__) +/** + * @brief Macro defining the specific ARM architecture. + */ +#define CH_ARCHITECTURE_ARM_v7M + +/** + * @brief Name of the implemented architecture. + */ +#define CH_ARCHITECTURE_NAME "ARMv7-M" + +/** + * @brief Name of the architecture variant. + */ +#define CH_CORE_VARIANT_NAME "Cortex-M3" + +#elif (CORTEX_MODEL == CORTEX_M4) +#define CH_ARCHITECTURE_ARM_v7ME +#define CH_ARCHITECTURE_NAME "ARMv7-ME" +#if CORTEX_USE_FPU +#define CH_CORE_VARIANT_NAME "Cortex-M4F" +#else +#define CH_CORE_VARIANT_NAME "Cortex-M4" +#endif +#endif + +/** + * @brief Port-specific information string. + */ +#if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__) +#define CH_PORT_INFO "Advanced kernel mode" +#else +#define CH_PORT_INFO "Compact kernel mode" +#endif +/** @} */ + +/** + * @brief This port supports a realtime counter. + */ +#define CH_PORT_SUPPORTS_RT TRUE + +/** + * @brief Disabled value for BASEPRI register. + */ +#define CORTEX_BASEPRI_DISABLED 0 + +/*===========================================================================*/ +/* Module pre-compile time settings. */ +/*===========================================================================*/ + +/** + * @brief Stack size for the system idle thread. + * @details This size depends on the idle thread implementation, usually + * the idle thread should take no more space than those reserved + * by @p PORT_INT_REQUIRED_STACK. + * @note In this port it is set to 16 because the idle thread does have + * a stack frame when compiling without optimizations. You may + * reduce this value to zero when compiling with optimizations. + */ +#if !defined(CH_PORT_IDLE_THREAD_STACK_SIZE) || defined(__DOXYGEN__) +#define CH_PORT_IDLE_THREAD_STACK_SIZE 16 +#endif + +/** + * @brief Per-thread stack overhead for interrupts servicing. + * @details This constant is used in the calculation of the correct working + * area size. + * @note In this port this value is conservatively set to 32 because the + * function @p chSchDoReschedule() can have a stack frame, especially + * with compiler optimizations disabled. The value can be reduced + * when compiler optimizations are enabled. + */ +#if !defined(CH_PORT_INT_REQUIRED_STACK) || defined(__DOXYGEN__) +#define CH_PORT_INT_REQUIRED_STACK 32 +#endif + +/** + * @brief Enables the use of the WFI instruction in the idle thread loop. + */ +#if !defined(CORTEX_ENABLE_WFI_IDLE) +#define CORTEX_ENABLE_WFI_IDLE FALSE +#endif + +/** + * @brief SYSTICK handler priority. + * @note The default SYSTICK handler priority is calculated as the priority + * level in the middle of the numeric priorities range. + */ +#if !defined(CORTEX_PRIORITY_SYSTICK) +#define CORTEX_PRIORITY_SYSTICK (CORTEX_PRIORITY_LEVELS >> 1) +#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SYSTICK) +/* If it is externally redefined then better perform a validity check on it.*/ +#error "invalid priority level specified for CORTEX_PRIORITY_SYSTICK" +#endif + +/** + * @brief FPU support in context switch. + * @details Activating this option activates the FPU support in the kernel. + */ +#if !defined(CORTEX_USE_FPU) +#define CORTEX_USE_FPU CORTEX_HAS_FPU +#elif CORTEX_USE_FPU && !CORTEX_HAS_FPU +/* This setting requires an FPU presence check in case it is externally + redefined.*/ +#error "the selected core does not have an FPU" +#endif + +/** + * @brief Simplified priority handling flag. + * @details Activating this option makes the Kernel work in compact mode. + */ +#if !defined(CORTEX_SIMPLIFIED_PRIORITY) +#define CORTEX_SIMPLIFIED_PRIORITY FALSE +#endif + +/** + * @brief SVCALL handler priority. + * @note The default SVCALL handler priority is defaulted to + * @p CORTEX_MAXIMUM_PRIORITY+1, this reserves the + * @p CORTEX_MAXIMUM_PRIORITY priority level as fast interrupts + * priority level. + */ +#if !defined(CORTEX_PRIORITY_SVCALL) +#define CORTEX_PRIORITY_SVCALL (CORTEX_MAXIMUM_PRIORITY + 1) +#elif !CORTEX_IS_VALID_PRIORITY(CORTEX_PRIORITY_SVCALL) +/* If it is externally redefined then better perform a validity check on it.*/ +#error "invalid priority level specified for CORTEX_PRIORITY_SVCALL" +#endif + +/** + * @brief NVIC VTOR initialization expression. + */ +#if !defined(CORTEX_VTOR_INIT) || defined(__DOXYGEN__) +#define CORTEX_VTOR_INIT 0x00000000 +#endif + +/** + * @brief NVIC PRIGROUP initialization expression. + * @details The default assigns all available priority bits as preemption + * priority with no sub-priority. + */ +#if !defined(CORTEX_PRIGROUP_INIT) || defined(__DOXYGEN__) +#define CORTEX_PRIGROUP_INIT (7 - CORTEX_PRIORITY_BITS) +#endif + +/*===========================================================================*/ +/* Derived constants and error checks. */ +/*===========================================================================*/ + +#if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__) +/** + * @brief Maximum usable priority for normal ISRs. + */ +#define CORTEX_MAX_KERNEL_PRIORITY (CORTEX_PRIORITY_SVCALL + 1) + +/** + * @brief BASEPRI level within kernel lock. + * @note In compact kernel mode this constant value is enforced to zero. + */ +#define CORTEX_BASEPRI_KERNEL \ + CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY) +#else + +#define CORTEX_MAX_KERNEL_PRIORITY 1 +#define CORTEX_BASEPRI_KERNEL 0 +#endif + +/** + * @brief PendSV priority level. + * @note This priority is enforced to be equal to @p CORTEX_BASEPRI_KERNEL, + * this handler always have the highest priority that cannot preempt + * the kernel. + */ +#define CORTEX_PRIORITY_PENDSV CORTEX_BASEPRI_KERNEL + +/*===========================================================================*/ +/* Module data structures and types. */ +/*===========================================================================*/ + +#if !defined(_FROM_ASM_) + +/** + * @brief Generic ARM register. + */ +typedef void *regarm_t; + +/* The documentation of the following declarations is in chconf.h in order + to not have duplicated structure names into the documentation.*/ +#if !defined(__DOXYGEN__) + +typedef uint64_t stkalign_t; + +struct extctx { + regarm_t r0; + regarm_t r1; + regarm_t r2; + regarm_t r3; + regarm_t r12; + regarm_t lr_thd; + regarm_t pc; + regarm_t xpsr; +#if CORTEX_USE_FPU + regarm_t s0; + regarm_t s1; + regarm_t s2; + regarm_t s3; + regarm_t s4; + regarm_t s5; + regarm_t s6; + regarm_t s7; + regarm_t s8; + regarm_t s9; + regarm_t s10; + regarm_t s11; + regarm_t s12; + regarm_t s13; + regarm_t s14; + regarm_t s15; + regarm_t fpscr; + regarm_t fpccr; +#endif /* CORTEX_USE_FPU */ +}; + +struct intctx { +#if CORTEX_USE_FPU + regarm_t s16; + regarm_t s17; + regarm_t s18; + regarm_t s19; + regarm_t s20; + regarm_t s21; + regarm_t s22; + regarm_t s23; + regarm_t s24; + regarm_t s25; + regarm_t s26; + regarm_t s27; + regarm_t s28; + regarm_t s29; + regarm_t s30; + regarm_t s31; +#endif /* CORTEX_USE_FPU */ + regarm_t r4; + regarm_t r5; + regarm_t r6; + regarm_t r7; + regarm_t r8; + regarm_t r9; + regarm_t r10; + regarm_t r11; + regarm_t lr; +}; + +#endif /* !defined(__DOXYGEN__) */ + +/** + * @brief Platform dependent part of the @p thread_t structure. + * @details In this port the structure just holds a pointer to the @p intctx + * structure representing the stack pointer at context switch time. + */ +struct context { + struct intctx *r13; +}; + +/*===========================================================================*/ +/* Module macros. */ +/*===========================================================================*/ + +/** + * @brief Platform dependent part of the @p chThdCreateI() API. + * @details This code usually setup the context switching frame represented + * by an @p intctx structure. + */ +#define SETUP_CONTEXT(workspace, wsize, pf, arg) { \ + tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace + \ + wsize - \ + sizeof(struct intctx)); \ + tp->p_ctx.r13->r4 = (void *)(pf); \ + tp->p_ctx.r13->r5 = (void *)(arg); \ + tp->p_ctx.r13->lr = (void *)(_port_thread_start); \ +} + +/** + * @brief Enforces a correct alignment for a stack area size value. + */ +#define STACK_ALIGN(n) ((((n) - 1) | (sizeof(stkalign_t) - 1)) + 1) + +/** + * @brief Computes the thread working area global size. + */ +#define THD_WA_SIZE(n) STACK_ALIGN(sizeof(thread_t) + \ + sizeof(struct intctx) + \ + sizeof(struct extctx) + \ + (n) + (CH_PORT_INT_REQUIRED_STACK)) + +/** + * @brief Static working area allocation. + * @details This macro is used to allocate a static thread working area + * aligned as both position and size. + */ +#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)] + +/** + * @brief IRQ prologue code. + * @details This macro must be inserted at the start of all IRQ handlers + * enabled to invoke system APIs. + */ +#define PORT_IRQ_PROLOGUE() + +/** + * @brief IRQ epilogue code. + * @details This macro must be inserted at the end of all IRQ handlers + * enabled to invoke system APIs. + */ +#define PORT_IRQ_EPILOGUE() _port_irq_epilogue() + +/** + * @brief IRQ handler function declaration. + * @note @p id can be a function name or a vector number depending on the + * port implementation. + */ +#define PORT_IRQ_HANDLER(id) void id(void) + +/** + * @brief Fast IRQ handler function declaration. + * @note @p id can be a function name or a vector number depending on the + * port implementation. + */ +#define PORT_FAST_IRQ_HANDLER(id) void id(void) + +/** + * @brief Performs a context switch between two threads. + * @details This is the most critical code in any port, this function + * is responsible for the context switch between 2 threads. + * @note The implementation of this code affects directly the context + * switch performance so optimize here as much as you can. + * + * @param[in] ntp the thread to be switched in + * @param[in] otp the thread to be switched out + */ +#if !CH_DBG_ENABLE_STACK_CHECK || defined(__DOXYGEN__) +#define port_switch(ntp, otp) _port_switch(ntp, otp) +#else +#define port_switch(ntp, otp) { \ + register struct intctx *r13 asm ("r13"); \ + if ((stkalign_t *)(r13 - 1) < otp->p_stklimit) \ + chDbgPanic("stack overflow"); \ + _port_switch(ntp, otp); \ +} +#endif + +/*===========================================================================*/ +/* External declarations. */ +/*===========================================================================*/ + +#ifdef __cplusplus +extern "C" { +#endif + void _port_irq_epilogue(void); + void _port_switch_from_isr(void); + void _port_exit_from_isr(void); + void _port_switch(thread_t *ntp, thread_t *otp); + void _port_thread_start(void); +#ifdef __cplusplus +} +#endif + +#if CH_CFG_TIMEDELTA > 0 +#include "systick.h" +#endif + +/*===========================================================================*/ +/* Module inline functions. */ +/*===========================================================================*/ + +/** + * @brief Port-related initialization code. + */ +static inline void port_init(void) { + + /* Initialization of the vector table and priority related settings.*/ + SCB_VTOR = CORTEX_VTOR_INIT; + SCB_AIRCR = AIRCR_VECTKEY | AIRCR_PRIGROUP(CORTEX_PRIGROUP_INIT); + + /* DWT cycle counter enable.*/ + SCS_DEMCR |= SCS_DEMCR_TRCENA; + DWT_CTRL |= DWT_CTRL_CYCCNTENA; + + /* Initialization of the system vectors used by the port.*/ + nvicSetSystemHandlerPriority(HANDLER_SVCALL, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SVCALL)); + nvicSetSystemHandlerPriority(HANDLER_PENDSV, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_PENDSV)); +#if CH_CFG_TIMEDELTA == 0 + nvicSetSystemHandlerPriority(HANDLER_SYSTICK, + CORTEX_PRIORITY_MASK(CORTEX_PRIORITY_SYSTICK)); +#else + port_timer_init(); +#endif +} + +/** + * @brief Returns a word encoding the current interrupts status. + * + * @return The interrupts status. + */ +static inline syssts_t port_get_irq_status(void) { + register uint32_t sts; + +#if !CORTEX_SIMPLIFIED_PRIORITY + asm volatile ("mrs %0, BASEPRI" : "=r" (sts) : : "memory"); +#else /* CORTEX_SIMPLIFIED_PRIORITY */ + asm volatile ("mrs %0, PRIMASK" : "=r" (sts) : : "memory"); +#endif /* CORTEX_SIMPLIFIED_PRIORITY */ + return sts; +} + +/** + * @brief Checks the interrupt status. + * + * @param[in] sts the interrupt status word + * + * @return The interrupt status. + * @retvel false the word specified a disabled interrupts status. + * @retvel true the word specified an enabled interrupts status. + */ +static inline bool port_irq_enabled(syssts_t sts) { + +#if !CORTEX_SIMPLIFIED_PRIORITY + return sts >= CORTEX_BASEPRI_KERNEL; +#else /* CORTEX_SIMPLIFIED_PRIORITY */ + return (sts & 1) == 0; +#endif /* CORTEX_SIMPLIFIED_PRIORITY */ +} + +/** + * @brief Determines the current execution context. + * + * @return The execution context. + * @retval false not running in ISR mode. + * @retval true running in ISR mode. + */ +static inline bool port_is_isr_context(void) { + uint32_t ipsr; + + asm volatile ("MRS %0, ipsr" : "=r" (ipsr)); + return (bool)((ipsr & 0x1FF) != 0); +} + +/** + * @brief Kernel-lock action. + * @details Usually this function just disables interrupts but may perform + * more actions. + * @note In this port this it raises the base priority to kernel level. + */ +static inline void port_lock(void) { + +#if !CORTEX_SIMPLIFIED_PRIORITY + register uint32_t basepri = CORTEX_BASEPRI_KERNEL; + asm volatile ("msr BASEPRI, %0" : : "r" (basepri) : "memory"); +#else /* CORTEX_SIMPLIFIED_PRIORITY */ + asm volatile ("cpsid i" : : : "memory"); +#endif /* CORTEX_SIMPLIFIED_PRIORITY */ +} + +/** + * @brief Kernel-unlock action. + * @details Usually this function just enables interrupts but may perform + * more actions. + * @note In this port this it lowers the base priority to user level. + */ +static inline void port_unlock(void) { + +#if !CORTEX_SIMPLIFIED_PRIORITY + register uint32_t basepri = CORTEX_BASEPRI_DISABLED; + asm volatile ("msr BASEPRI, %0" : : "r" (basepri) : "memory"); +#else /* CORTEX_SIMPLIFIED_PRIORITY */ + asm volatile ("cpsie i" : : : "memory"); +#endif /* CORTEX_SIMPLIFIED_PRIORITY */ +} + +/** + * @brief Kernel-lock action from an interrupt handler. + * @details This function is invoked before invoking I-class APIs from + * interrupt handlers. The implementation is architecture dependent, + * in its simplest form it is void. + * @note Same as @p port_lock() in this port. + */ +static inline void port_lock_from_isr(void) { + + port_lock(); +} + +/** + * @brief Kernel-unlock action from an interrupt handler. + * @details This function is invoked after invoking I-class APIs from interrupt + * handlers. The implementation is architecture dependent, in its + * simplest form it is void. + * @note Same as @p port_unlock() in this port. + */ +static inline void port_unlock_from_isr(void) { + + port_unlock(); +} + +/** + * @brief Disables all the interrupt sources. + * @note Of course non-maskable interrupt sources are not included. + * @note In this port it disables all the interrupt sources by raising + * the priority mask to level 0. + */ +static inline void port_disable(void) { + + asm volatile ("cpsid i" : : : "memory"); +} + +/** + * @brief Disables the interrupt sources below kernel-level priority. + * @note Interrupt sources above kernel level remains enabled. + * @note In this port it raises/lowers the base priority to kernel level. + */ +static inline void port_suspend(void) { + +#if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__) + register uint32_t basepri = CORTEX_BASEPRI_KERNEL; + asm volatile ("msr BASEPRI, %0 \n\t" + "cpsie i" : : "r" (basepri) : "memory"); +#else + asm volatile ("cpsid i" : : : "memory"); +#endif +} + +/** + * @brief Enables all the interrupt sources. + * @note In this port it lowers the base priority to user level. + */ +static inline void port_enable(void) { + +#if !CORTEX_SIMPLIFIED_PRIORITY || defined(__DOXYGEN__) + register uint32_t basepri = CORTEX_BASEPRI_DISABLED; + asm volatile ("msr BASEPRI, %0 \n\t" + "cpsie i" : : "r" (basepri) : "memory"); +#else + asm volatile ("cpsie i" : : : "memory"); +#endif +} + +/** + * @brief Enters an architecture-dependent IRQ-waiting mode. + * @details The function is meant to return when an interrupt becomes pending. + * The simplest implementation is an empty function or macro but this + * would not take advantage of architecture-specific power saving + * modes. + * @note Implemented as an inlined @p WFI instruction. + */ +static inline void port_wait_for_interrupt(void) { + +#if CORTEX_ENABLE_WFI_IDLE + asm volatile ("wfi" : : : "memory"); +#endif +} + +static inline rtcnt_t port_rt_get_counter_value(void) { + + return DWT_CYCCNT; +} + +#endif /* !defined(_FROM_ASM_) */ + +#endif /* _CHCORE_V7M_H_ */ + +/** @} */ diff --git a/os/kernel/ports/ARMCMx/compilers/GCC/chcoreasm.s b/os/kernel/ports/ARMCMx/compilers/GCC/chcoreasm.s index e69de29bb..924a0b6d0 100644 --- a/os/kernel/ports/ARMCMx/compilers/GCC/chcoreasm.s +++ b/os/kernel/ports/ARMCMx/compilers/GCC/chcoreasm.s @@ -0,0 +1,74 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 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 . +*/ + +/** + * @file ARM/chcoreasm.s + * @brief ARM7/9 architecture port low level code. + * + * @addtogroup ARM_CORE + * @{ + */ + +#include "chconf.h" +#include "chcore.h" + +#if !defined(FALSE) || defined(__DOXYGEN__) +#define FALSE 0 +#endif + +#if !defined(TRUE) || defined(__DOXYGEN__) +#define TRUE 1 +#endif + +#if !defined(__DOXYGEN__) + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + + .thumb + .text + +/* + * Thread trampoline code. + * + * Threads execution starts here, the code leaves the system critical zone + * and then jumps into the thread function passed in register R4. The + * register R5 contains the thread parameter. The function chThdExit() is + * called on thread function return. + */ + .thumb_func + .globl _port_thread_start +_port_thread_start: +#if CH_DBG_SYSTEM_STATE_CHECK + bl dbg_check_unlock +#endif +#if CH_DBG_STATISTICS + bl _stats_stop_measure_crit_thd +#endif + movs r3, #0 + msr BASEPRI, r3 + mov r0, r5 + blx r4 + bl chThdExit + +#endif /* !defined(__DOXYGEN__) */ + +/** @} */ diff --git a/os/kernel/ports/ARMCMx/devices/STM32F30x/port.mk b/os/kernel/ports/ARMCMx/devices/STM32F30x/port.mk index 29cf0b7eb..a4a320bf8 100644 --- a/os/kernel/ports/ARMCMx/devices/STM32F30x/port.mk +++ b/os/kernel/ports/ARMCMx/devices/STM32F30x/port.mk @@ -1,14 +1,14 @@ -# List of the ChibiOS/RT Cortex-M4 STM32 port files. -PORTSRC = ${CHIBIOS}/os/portsnew/ARMCMx/chcore.c \ - ${CHIBIOS}/os/portsnew/ARMCMx/chcore_v7m.c \ - $(CHIBIOS)/os/portsnew/ARMCMx/GCC/crt0.c \ - $(CHIBIOS)/os/portsnew/ARMCMx/GCC/vectors.c \ +# List of the ChibiOS/RT Cortex-M4 STM32F30x port files. +PORTSRC = ${CHIBIOS}/os/kernel/ports/ARMCMx/chcore.c \ + ${CHIBIOS}/os/kernel/ports/ARMCMx/chcore_v7m.c \ + $(CHIBIOS)/os/kernel/ports/ARMCMx/GCC/crt0.c \ + $(CHIBIOS)/os/kernel/ports/ARMCMx/GCC/vectors.c \ -PORTASM = $(CHIBIOS)/os/portsnew/ARMCMx/GCC/chcoreasm.s +PORTASM = $(CHIBIOS)/os/kernel/ports/ARMCMx/GCC/chcoreasm.s -PORTINC = ${CHIBIOS}/os/portsnew/ARMCMx \ - ${CHIBIOS}/os/portsnew/ARMCMx/GCC \ - ${CHIBIOS}/os/portsnew/ARMCMx/common/CMSIS/include \ - ${CHIBIOS}/os/portsnew/ARMCMx/GCC/STM32F30x +PORTINC = ${CHIBIOS}/os/kernel/ports/ARMCMx \ + ${CHIBIOS}/os/kernel/ports/ARMCMx/GCC \ + ${CHIBIOS}/os/kernel/ports/ARMCMx/GCC/STM32F30x \ + ${CHIBIOS}/os/kernel/ports/ARMCMx/common/CMSIS/include PORTLD = ${CHIBIOS}/os/ports/GCC/ld -- cgit v1.2.3