From 48026bb824fd2d9cfb00ecd040db6ef3a416bae9 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Fri, 22 Jan 2021 21:43:36 -0500 Subject: upload initial port --- ChibiOS_20.3.2/os/rt/src/chcond.c | 321 ++++++++++++ ChibiOS_20.3.2/os/rt/src/chdebug.c | 257 ++++++++++ ChibiOS_20.3.2/os/rt/src/chdynamic.c | 185 +++++++ ChibiOS_20.3.2/os/rt/src/chevents.c | 603 ++++++++++++++++++++++ ChibiOS_20.3.2/os/rt/src/chmsg.c | 221 +++++++++ ChibiOS_20.3.2/os/rt/src/chmtx.c | 537 ++++++++++++++++++++ ChibiOS_20.3.2/os/rt/src/chregistry.c | 268 ++++++++++ ChibiOS_20.3.2/os/rt/src/chschd.c | 610 +++++++++++++++++++++++ ChibiOS_20.3.2/os/rt/src/chsem.c | 405 +++++++++++++++ ChibiOS_20.3.2/os/rt/src/chstats.c | 126 +++++ ChibiOS_20.3.2/os/rt/src/chsys.c | 445 +++++++++++++++++ ChibiOS_20.3.2/os/rt/src/chthreads.c | 906 ++++++++++++++++++++++++++++++++++ ChibiOS_20.3.2/os/rt/src/chtm.c | 168 +++++++ ChibiOS_20.3.2/os/rt/src/chtrace.c | 265 ++++++++++ ChibiOS_20.3.2/os/rt/src/chvt.c | 477 ++++++++++++++++++ 15 files changed, 5794 insertions(+) create mode 100644 ChibiOS_20.3.2/os/rt/src/chcond.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chdebug.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chdynamic.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chevents.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chmsg.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chmtx.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chregistry.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chschd.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chsem.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chstats.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chsys.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chthreads.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chtm.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chtrace.c create mode 100644 ChibiOS_20.3.2/os/rt/src/chvt.c (limited to 'ChibiOS_20.3.2/os/rt/src') diff --git a/ChibiOS_20.3.2/os/rt/src/chcond.c b/ChibiOS_20.3.2/os/rt/src/chcond.c new file mode 100644 index 0000000..bd653c3 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chcond.c @@ -0,0 +1,321 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 . +*/ +/* + Concepts and parts of this file have been contributed by Leon Woestenberg. + */ + +/** + * @file rt/src/chcond.c + * @brief Condition Variables code. + * + * @addtogroup condvars + * @details This module implements the Condition Variables mechanism. Condition + * variables are an extensions to the mutex subsystem and cannot + * work alone. + *

Operation mode

+ * The condition variable is a synchronization object meant to be + * used inside a zone protected by a mutex. Mutexes and condition + * variables together can implement a Monitor construct. + * @pre In order to use the condition variable APIs the @p CH_CFG_USE_CONDVARS + * option must be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_CONDVARS == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes s @p condition_variable_t structure. + * + * @param[out] cp pointer to a @p condition_variable_t structure + * + * @init + */ +void chCondObjectInit(condition_variable_t *cp) { + + chDbgCheck(cp != NULL); + + queue_init(&cp->queue); +} + +/** + * @brief Signals one thread that is waiting on the condition variable. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * + * @api + */ +void chCondSignal(condition_variable_t *cp) { + + chDbgCheck(cp != NULL); + + chSysLock(); + if (queue_notempty(&cp->queue)) { + chSchWakeupS(queue_fifo_remove(&cp->queue), MSG_OK); + } + chSysUnlock(); +} + +/** + * @brief Signals one thread that is waiting on the condition variable. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * + * @iclass + */ +void chCondSignalI(condition_variable_t *cp) { + + chDbgCheckClassI(); + chDbgCheck(cp != NULL); + + if (queue_notempty(&cp->queue)) { + thread_t *tp = queue_fifo_remove(&cp->queue); + tp->u.rdymsg = MSG_OK; + (void) chSchReadyI(tp); + } +} + +/** + * @brief Signals all threads that are waiting on the condition variable. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * + * @api + */ +void chCondBroadcast(condition_variable_t *cp) { + + chSysLock(); + chCondBroadcastI(cp); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Signals all threads that are waiting on the condition variable. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * + * @iclass + */ +void chCondBroadcastI(condition_variable_t *cp) { + + chDbgCheckClassI(); + chDbgCheck(cp != NULL); + + /* Empties the condition variable queue and inserts all the threads into the + ready list in FIFO order. The wakeup message is set to @p MSG_RESET in + order to make a chCondBroadcast() detectable from a chCondSignal().*/ + while (queue_notempty(&cp->queue)) { + chSchReadyI(queue_fifo_remove(&cp->queue))->u.rdymsg = MSG_RESET; + } +} + +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the currently owned mutex, waits on the condition + * variable, and finally acquires the mutex again. All the sequence + * is performed atomically. + * @pre The invoking thread must have at least one owned mutex. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * @return A message specifying how the invoking thread has been + * released from the condition variable. + * @retval MSG_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval MSG_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * + * @api + */ +msg_t chCondWait(condition_variable_t *cp) { + msg_t msg; + + chSysLock(); + msg = chCondWaitS(cp); + chSysUnlock(); + return msg; +} + +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the currently owned mutex, waits on the condition + * variable, and finally acquires the mutex again. All the sequence + * is performed atomically. + * @pre The invoking thread must have at least one owned mutex. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * @return A message specifying how the invoking thread has been + * released from the condition variable. + * @retval MSG_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval MSG_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * + * @sclass + */ +msg_t chCondWaitS(condition_variable_t *cp) { + thread_t *ctp = currp; + mutex_t *mp = chMtxGetNextMutexX(); + msg_t msg; + + chDbgCheckClassS(); + chDbgCheck(cp != NULL); + chDbgAssert(mp != NULL, "not owning a mutex"); + + /* Releasing "current" mutex.*/ + chMtxUnlockS(mp); + + /* Start waiting on the condition variable, on exit the mutex is taken + again.*/ + ctp->u.wtobjp = cp; + queue_prio_insert(ctp, &cp->queue); + chSchGoSleepS(CH_STATE_WTCOND); + msg = ctp->u.rdymsg; + chMtxLockS(mp); + + return msg; +} + +#if (CH_CFG_USE_CONDVARS_TIMEOUT == TRUE) || defined(__DOXYGEN__) +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the currently owned mutex, waits on the condition + * variable, and finally acquires the mutex again. All the sequence + * is performed atomically. + * @pre The invoking thread must have at least one owned mutex. + * @pre The configuration option @p CH_CFG_USE_CONDVARS_TIMEOUT must be enabled + * in order to use this function. + * @post Exiting the function because a timeout does not re-acquire the + * mutex, the mutex ownership is lost. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * @param[in] timeout the number of ticks before the operation timeouts, the + * special values are handled as follow: + * - @a TIME_INFINITE no timeout. + * - @a TIME_IMMEDIATE this value is not allowed. + * . + * @return A message specifying how the invoking thread has been + * released from the condition variable. + * @retval MSG_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval MSG_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * @retval MSG_TIMEOUT if the condition variable has not been signaled within + * the specified timeout. + * + * @api + */ +msg_t chCondWaitTimeout(condition_variable_t *cp, sysinterval_t timeout) { + msg_t msg; + + chSysLock(); + msg = chCondWaitTimeoutS(cp, timeout); + chSysUnlock(); + + return msg; +} + +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the currently owned mutex, waits on the condition + * variable, and finally acquires the mutex again. All the sequence + * is performed atomically. + * @pre The invoking thread must have at least one owned mutex. + * @pre The configuration option @p CH_CFG_USE_CONDVARS_TIMEOUT must be enabled + * in order to use this function. + * @post Exiting the function because a timeout does not re-acquire the + * mutex, the mutex ownership is lost. + * + * @param[in] cp pointer to the @p condition_variable_t structure + * @param[in] timeout the number of ticks before the operation timeouts, the + * special values are handled as follow: + * - @a TIME_INFINITE no timeout. + * - @a TIME_IMMEDIATE this value is not allowed. + * . + * @return A message specifying how the invoking thread has been + * released from the condition variable. + * @retval MSG_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval MSG_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * @retval MSG_TIMEOUT if the condition variable has not been signaled within + * the specified timeout. + * + * @sclass + */ +msg_t chCondWaitTimeoutS(condition_variable_t *cp, sysinterval_t timeout) { + mutex_t *mp = chMtxGetNextMutexX(); + msg_t msg; + + chDbgCheckClassS(); + chDbgCheck((cp != NULL) && (timeout != TIME_IMMEDIATE)); + chDbgAssert(mp != NULL, "not owning a mutex"); + + /* Releasing "current" mutex.*/ + chMtxUnlockS(mp); + + /* Start waiting on the condition variable, on exit the mutex is taken + again.*/ + currp->u.wtobjp = cp; + queue_prio_insert(currp, &cp->queue); + msg = chSchGoSleepTimeoutS(CH_STATE_WTCOND, timeout); + if (msg != MSG_TIMEOUT) { + chMtxLockS(mp); + } + + return msg; +} +#endif /* CH_CFG_USE_CONDVARS_TIMEOUT == TRUE */ + +#endif /* CH_CFG_USE_CONDVARS == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chdebug.c b/ChibiOS_20.3.2/os/rt/src/chdebug.c new file mode 100644 index 0000000..d73950c --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chdebug.c @@ -0,0 +1,257 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chdebug.c + * @brief Debug support code. + * + * @addtogroup checks_assertions + * @details Debug APIs and services: + * - Runtime system state and call protocol check. The following + * panic messages can be generated: + * - SV#1, misplaced @p chSysDisable(). + * - Called from an ISR. + * - Called from a critical zone. + * . + * - SV#2, misplaced @p chSysSuspend() + * - Called from an ISR. + * - Called from a critical zone. + * . + * - SV#3, misplaced @p chSysEnable(). + * - Called from an ISR. + * - Called from a critical zone. + * . + * - SV#4, misplaced @p chSysLock(). + * - Called from an ISR. + * - Called from a critical zone. + * . + * - SV#5, misplaced @p chSysUnlock(). + * - Called from an ISR. + * - Not called from a critical zone. + * . + * - SV#6, misplaced @p chSysLockFromISR(). + * - Not called from an ISR. + * - Called from a critical zone. + * . + * - SV#7, misplaced @p chSysUnlockFromISR(). + * - Not called from an ISR. + * - Not called from a critical zone. + * . + * - SV#8, misplaced @p CH_IRQ_PROLOGUE(). + * - Not called at ISR begin. + * - Called from a critical zone. + * . + * - SV#9, misplaced @p CH_IRQ_EPILOGUE(). + * - @p CH_IRQ_PROLOGUE() missing. + * - Not called at ISR end. + * - Called from a critical zone. + * . + * - SV#10, misplaced I-class function. + * - I-class function not called from within a critical zone. + * . + * - SV#11, misplaced S-class function. + * - S-class function not called from within a critical zone. + * - Called from an ISR. + * . + * - Parameters check. + * - Kernel assertions. + * . + * @note Stack checks are not implemented in this module but in the port + * layer in an architecture-dependent way. + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +#if (CH_DBG_SYSTEM_STATE_CHECK == TRUE) || defined(__DOXYGEN__) +/** + * @brief Guard code for @p chSysDisable(). + * + * @notapi + */ +void _dbg_check_disable(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#1"); + } +} + +/** + * @brief Guard code for @p chSysSuspend(). + * + * @notapi + */ +void _dbg_check_suspend(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#2"); + } +} + +/** + * @brief Guard code for @p chSysEnable(). + * + * @notapi + */ +void _dbg_check_enable(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#3"); + } +} + +/** + * @brief Guard code for @p chSysLock(). + * + * @notapi + */ +void _dbg_check_lock(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#4"); + } + _dbg_enter_lock(); +} + +/** + * @brief Guard code for @p chSysUnlock(). + * + * @notapi + */ +void _dbg_check_unlock(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) { + chSysHalt("SV#5"); + } + _dbg_leave_lock(); +} + +/** + * @brief Guard code for @p chSysLockFromIsr(). + * + * @notapi + */ +void _dbg_check_lock_from_isr(void) { + + if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#6"); + } + _dbg_enter_lock(); +} + +/** + * @brief Guard code for @p chSysUnlockFromIsr(). + * + * @notapi + */ +void _dbg_check_unlock_from_isr(void) { + + if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) { + chSysHalt("SV#7"); + } + _dbg_leave_lock(); +} + +/** + * @brief Guard code for @p CH_IRQ_PROLOGUE(). + * + * @notapi + */ +void _dbg_check_enter_isr(void) { + + port_lock_from_isr(); + if ((ch.dbg.isr_cnt < (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#8"); + } + ch.dbg.isr_cnt++; + port_unlock_from_isr(); +} + +/** + * @brief Guard code for @p CH_IRQ_EPILOGUE(). + * + * @notapi + */ +void _dbg_check_leave_isr(void) { + + port_lock_from_isr(); + if ((ch.dbg.isr_cnt <= (cnt_t)0) || (ch.dbg.lock_cnt != (cnt_t)0)) { + chSysHalt("SV#9"); + } + ch.dbg.isr_cnt--; + port_unlock_from_isr(); +} + +/** + * @brief I-class functions context check. + * @details Verifies that the system is in an appropriate state for invoking + * an I-class API function. A panic is generated if the state is + * not compatible. + * + * @api + */ +void chDbgCheckClassI(void) { + + if ((ch.dbg.isr_cnt < (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) { + chSysHalt("SV#10"); + } +} + +/** + * @brief S-class functions context check. + * @details Verifies that the system is in an appropriate state for invoking + * an S-class API function. A panic is generated if the state is + * not compatible. + * + * @api + */ +void chDbgCheckClassS(void) { + + if ((ch.dbg.isr_cnt != (cnt_t)0) || (ch.dbg.lock_cnt <= (cnt_t)0)) { + chSysHalt("SV#11"); + } +} + +#endif /* CH_DBG_SYSTEM_STATE_CHECK == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chdynamic.c b/ChibiOS_20.3.2/os/rt/src/chdynamic.c new file mode 100644 index 0000000..a11c219 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chdynamic.c @@ -0,0 +1,185 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chdynamic.c + * @brief Dynamic threads code. + * + * @addtogroup dynamic_threads + * @details Dynamic threads related APIs and services. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_DYNAMIC == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +#if (CH_CFG_USE_HEAP == TRUE) || defined(__DOXYGEN__) +/** + * @brief Creates a new thread allocating the memory from the heap. + * @pre The configuration options @p CH_CFG_USE_DYNAMIC and + * @p CH_CFG_USE_HEAP must be enabled in order to use this function. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * @note The memory allocated for the thread is not released automatically, + * it is responsibility of the creator thread to call @p chThdWait() + * and then release the allocated memory. + * + * @param[in] heapp heap from which allocate the memory or @p NULL for the + * default heap + * @param[in] size size of the working area to be allocated + * @param[in] name thread name + * @param[in] prio the priority level for the new thread + * @param[in] pf the thread function + * @param[in] arg an argument passed to the thread function. It can be + * @p NULL. + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * @retval NULL if the memory cannot be allocated. + * + * @api + */ +thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size, + const char *name, tprio_t prio, + tfunc_t pf, void *arg) { + thread_t *tp; + void *wsp; + + wsp = chHeapAllocAligned(heapp, size, PORT_WORKING_AREA_ALIGN); + if (wsp == NULL) { + return NULL; + } + + thread_descriptor_t td = { + name, + wsp, + (stkalign_t *)((uint8_t *)wsp + size), + prio, + pf, + arg + }; + +#if CH_DBG_FILL_THREADS == TRUE + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + size, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateSuspendedI(&td); + tp->flags = CH_FLAG_MODE_HEAP; + chSchWakeupS(tp, MSG_OK); + chSysUnlock(); + + return tp; +} +#endif /* CH_CFG_USE_HEAP == TRUE */ + +#if (CH_CFG_USE_MEMPOOLS == TRUE) || defined(__DOXYGEN__) +/** + * @brief Creates a new thread allocating the memory from the specified + * memory pool. + * @pre The configuration options @p CH_CFG_USE_DYNAMIC and + * @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this + * function. + * @pre The pool must be initialized to contain only objects with + * alignment @p PORT_WORKING_AREA_ALIGN. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * @note The memory allocated for the thread is not released automatically, + * it is responsibility of the creator thread to call @p chThdWait() + * and then release the allocated memory. + * + * @param[in] mp pointer to the memory pool object + * @param[in] name thread name + * @param[in] prio the priority level for the new thread + * @param[in] pf the thread function + * @param[in] arg an argument passed to the thread function. It can be + * @p NULL. + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * @retval NULL if the memory pool is empty. + * + * @api + */ +thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, const char *name, + tprio_t prio, tfunc_t pf, void *arg) { + thread_t *tp; + void *wsp; + + chDbgCheck(mp != NULL); + + wsp = chPoolAlloc(mp); + if (wsp == NULL) { + return NULL; + } + + thread_descriptor_t td = { + name, + wsp, + (stkalign_t *)((uint8_t *)wsp + mp->object_size), + prio, + pf, + arg + }; + +#if CH_DBG_FILL_THREADS == TRUE + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + mp->object_size, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateSuspendedI(&td); + tp->flags = CH_FLAG_MODE_MPOOL; + tp->mpool = mp; + chSchWakeupS(tp, MSG_OK); + chSysUnlock(); + + return tp; +} +#endif /* CH_CFG_USE_MEMPOOLS == TRUE */ + +#endif /* CH_CFG_USE_DYNAMIC == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chevents.c b/ChibiOS_20.3.2/os/rt/src/chevents.c new file mode 100644 index 0000000..9b84ce2 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chevents.c @@ -0,0 +1,603 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 . +*/ +/* + Concepts and parts of this file have been contributed by Scott (skute). + */ + +/** + * @file rt/src/chevents.c + * @brief Events code. + * + * @addtogroup events + * @details Event Flags, Event Sources and Event Listeners. + *

Operation mode

+ * Each thread has a mask of pending events inside its + * @p thread_t structure. + * Operations defined for events: + * - Wait, the invoking thread goes to sleep until a certain + * AND/OR combination of events are signaled. + * - Clear, a mask of events is cleared from the pending + * events, the cleared events mask is returned (only the + * events that were actually pending and then cleared). + * - Signal, an events mask is directly ORed to the mask of + * the signaled thread. + * - Broadcast, each thread registered on an Event Source is + * signaled with the events specified in its Event Listener. + * - Dispatch, an events mask is scanned and for each bit set + * to one an associated handler function is invoked. Bit masks are + * scanned from bit zero upward. + * . + * An Event Source is a special object that can be "broadcasted" by + * a thread or an interrupt service routine. Broadcasting an Event + * Source has the effect that all the threads registered on the + * Event Source will be signaled with an events mask.
+ * An unlimited number of Event Sources can exists in a system and + * each thread can be listening on an unlimited number of + * them. + * @pre In order to use the Events APIs the @p CH_CFG_USE_EVENTS option + * must be enabled in @p chconf.h. + * @post Enabling events requires 1-4 (depending on the architecture) + * extra bytes in the @p thread_t structure. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_EVENTS == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Registers an Event Listener on an Event Source. + * @details Once a thread has registered as listener on an event source it + * will be notified of all events broadcasted there. + * @note Multiple Event Listeners can specify the same bits to be ORed to + * different threads. + * + * @param[in] esp pointer to the @p event_source_t structure + * @param[in] elp pointer to the @p event_listener_t structure + * @param[in] events events to be ORed to the thread when + * the event source is broadcasted + * @param[in] wflags mask of flags the listening thread is interested in + * + * @api + */ +void chEvtRegisterMaskWithFlags(event_source_t *esp, + event_listener_t *elp, + eventmask_t events, + eventflags_t wflags) { + + chDbgCheck((esp != NULL) && (elp != NULL)); + + chSysLock(); + elp->next = esp->next; + esp->next = elp; + elp->listener = currp; + elp->events = events; + elp->flags = (eventflags_t)0; + elp->wflags = wflags; + chSysUnlock(); +} + +/** + * @brief Unregisters an Event Listener from its Event Source. + * @note If the event listener is not registered on the specified event + * source then the function does nothing. + * @note For optimal performance it is better to perform the unregister + * operations in inverse order of the register operations (elements + * are found on top of the list). + * + * @param[in] esp pointer to the @p event_source_t structure + * @param[in] elp pointer to the @p event_listener_t structure + * + * @api + */ +void chEvtUnregister(event_source_t *esp, event_listener_t *elp) { + event_listener_t *p; + + chDbgCheck((esp != NULL) && (elp != NULL)); + + /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/ + p = (event_listener_t *)esp; + /*lint -restore*/ + chSysLock(); + /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/ + while (p->next != (event_listener_t *)esp) { + /*lint -restore*/ + if (p->next == elp) { + p->next = elp->next; + break; + } + p = p->next; + } + chSysUnlock(); +} + +/** + * @brief Clears the pending events specified in the events mask. + * + * @param[in] events the events to be cleared + * @return The mask of pending events that were cleared. + * + * @iclass + */ +eventmask_t chEvtGetAndClearEventsI(eventmask_t events) { + eventmask_t m; + + m = currp->epending & events; + currp->epending &= ~events; + + return m; +} + +/** + * @brief Clears the pending events specified in the events mask. + * + * @param[in] events the events to be cleared + * @return The mask of pending events that were cleared. + * + * @api + */ +eventmask_t chEvtGetAndClearEvents(eventmask_t events) { + eventmask_t m; + + chSysLock(); + m = chEvtGetAndClearEventsI(events); + chSysUnlock(); + + return m; +} + +/** + * @brief Adds (OR) a set of events to the current thread, this is + * @b much faster than using @p chEvtBroadcast() or @p chEvtSignal(). + * + * @param[in] events the events to be added + * @return The mask of currently pending events. + * + * @api + */ +eventmask_t chEvtAddEvents(eventmask_t events) { + eventmask_t newevt; + + chSysLock(); + newevt = chEvtAddEventsI(events); + chSysUnlock(); + + return newevt; +} + +/** + * @brief Signals all the Event Listeners registered on the specified Event + * Source. + * @details This function variants ORs the specified event flags to all the + * threads registered on the @p event_source_t in addition to the + * event flags specified by the threads themselves in the + * @p event_listener_t objects. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] esp pointer to the @p event_source_t structure + * @param[in] flags the flags set to be added to the listener flags mask + * + * @iclass + */ +void chEvtBroadcastFlagsI(event_source_t *esp, eventflags_t flags) { + event_listener_t *elp; + + chDbgCheckClassI(); + chDbgCheck(esp != NULL); + + elp = esp->next; + /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/ + while (elp != (event_listener_t *)esp) { + /*lint -restore*/ + elp->flags |= flags; + /* When flags == 0 the thread will always be signaled because the + source does not emit any flag.*/ + if ((flags == (eventflags_t)0) || + ((flags & elp->wflags) != (eventflags_t)0)) { + chEvtSignalI(elp->listener, elp->events); + } + elp = elp->next; + } +} + +/** + * @brief Returns the flags associated to an @p event_listener_t. + * @details The flags are returned and the @p event_listener_t flags mask is + * cleared. + * + * @param[in] elp pointer to the @p event_listener_t structure + * @return The flags added to the listener by the associated + * event source. + * + * @api + */ +eventflags_t chEvtGetAndClearFlags(event_listener_t *elp) { + eventflags_t flags; + + chSysLock(); + flags = elp->flags; + elp->flags = (eventflags_t)0; + chSysUnlock(); + + return flags & elp->wflags; +} + +/** + * @brief Adds a set of event flags directly to the specified @p thread_t. + * + * @param[in] tp the thread to be signaled + * @param[in] events the events set to be ORed + * + * @api + */ +void chEvtSignal(thread_t *tp, eventmask_t events) { + + chDbgCheck(tp != NULL); + + chSysLock(); + chEvtSignalI(tp, events); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Adds a set of event flags directly to the specified @p thread_t. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] tp the thread to be signaled + * @param[in] events the events set to be ORed + * + * @iclass + */ +void chEvtSignalI(thread_t *tp, eventmask_t events) { + + chDbgCheckClassI(); + chDbgCheck(tp != NULL); + + tp->epending |= events; + /* Test on the AND/OR conditions wait states.*/ + if (((tp->state == CH_STATE_WTOREVT) && + ((tp->epending & tp->u.ewmask) != (eventmask_t)0)) || + ((tp->state == CH_STATE_WTANDEVT) && + ((tp->epending & tp->u.ewmask) == tp->u.ewmask))) { + tp->u.rdymsg = MSG_OK; + (void) chSchReadyI(tp); + } +} + +/** + * @brief Signals all the Event Listeners registered on the specified Event + * Source. + * @details This function variants ORs the specified event flags to all the + * threads registered on the @p event_source_t in addition to the + * event flags specified by the threads themselves in the + * @p event_listener_t objects. + * + * @param[in] esp pointer to the @p event_source_t structure + * @param[in] flags the flags set to be added to the listener flags mask + * + * @api + */ +void chEvtBroadcastFlags(event_source_t *esp, eventflags_t flags) { + + chSysLock(); + chEvtBroadcastFlagsI(esp, flags); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Returns the unmasked flags associated to an @p event_listener_t. + * @details The flags are returned and the @p event_listener_t flags mask is + * cleared. + * + * @param[in] elp pointer to the @p event_listener_t structure + * @return The flags added to the listener by the associated + * event source. + * + * @iclass + */ +eventflags_t chEvtGetAndClearFlagsI(event_listener_t *elp) { + eventflags_t flags; + + flags = elp->flags; + elp->flags = (eventflags_t)0; + + return flags & elp->wflags; +} + +/** + * @brief Invokes the event handlers associated to an event flags mask. + * + * @param[in] events mask of events to be dispatched + * @param[in] handlers an array of @p evhandler_t. The array must have size + * equal to the number of bits in eventmask_t. + * + * @api + */ +void chEvtDispatch(const evhandler_t *handlers, eventmask_t events) { + eventid_t eid; + + chDbgCheck(handlers != NULL); + + eid = (eventid_t)0; + while (events != (eventmask_t)0) { + if ((events & EVENT_MASK(eid)) != (eventmask_t)0) { + chDbgAssert(handlers[eid] != NULL, "null handler"); + events &= ~EVENT_MASK(eid); + handlers[eid](eid); + } + eid++; + } +} + +#if (CH_CFG_OPTIMIZE_SPEED == TRUE) || \ + (CH_CFG_USE_EVENTS_TIMEOUT == FALSE) || \ + defined(__DOXYGEN__) +/** + * @brief Waits for exactly one of the specified events. + * @details The function waits for one event among those specified in + * @p events to become pending then the event is cleared and returned. + * @note One and only one event is served in the function, the one with the + * lowest event id. The function is meant to be invoked into a loop in + * order to serve all the pending events.
+ * This means that Event Listeners with a lower event identifier have + * an higher priority. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS enables all the events + * @return The mask of the lowest event id served and cleared. + * + * @api + */ +eventmask_t chEvtWaitOne(eventmask_t events) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + m = ctp->epending & events; + if (m == (eventmask_t)0) { + ctp->u.ewmask = events; + chSchGoSleepS(CH_STATE_WTOREVT); + m = ctp->epending & events; + } + m ^= m & (m - (eventmask_t)1); + ctp->epending &= ~m; + chSysUnlock(); + + return m; +} + +/** + * @brief Waits for any of the specified events. + * @details The function waits for any event among those specified in + * @p events to become pending then the events are cleared and + * returned. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS enables all the events + * @return The mask of the served and cleared events. + * + * @api + */ +eventmask_t chEvtWaitAny(eventmask_t events) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + m = ctp->epending & events; + if (m == (eventmask_t)0) { + ctp->u.ewmask = events; + chSchGoSleepS(CH_STATE_WTOREVT); + m = ctp->epending & events; + } + ctp->epending &= ~m; + chSysUnlock(); + + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p events to + * become pending then the events are cleared and returned. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS requires all the events + * @return The mask of the served and cleared events. + * + * @api + */ +eventmask_t chEvtWaitAll(eventmask_t events) { + thread_t *ctp = currp; + + chSysLock(); + if ((ctp->epending & events) != events) { + ctp->u.ewmask = events; + chSchGoSleepS(CH_STATE_WTANDEVT); + } + ctp->epending &= ~events; + chSysUnlock(); + + return events; +} +#endif /* CH_CFG_OPTIMIZE_SPEED || !CH_CFG_USE_EVENTS_TIMEOUT */ + +#if (CH_CFG_USE_EVENTS_TIMEOUT == TRUE) || defined(__DOXYGEN__) +/** + * @brief Waits for exactly one of the specified events. + * @details The function waits for one event among those specified in + * @p events to become pending then the event is cleared and returned. + * @note One and only one event is served in the function, the one with the + * lowest event id. The function is meant to be invoked into a loop + * in order to serve all the pending events.
+ * This means that Event Listeners with a lower event identifier have + * an higher priority. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS enables all the events + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return The mask of the lowest event id served and cleared. + * @retval 0 if the operation has timed out. + * + * @api + */ +eventmask_t chEvtWaitOneTimeout(eventmask_t events, sysinterval_t timeout) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + m = ctp->epending & events; + if (m == (eventmask_t)0) { + if (TIME_IMMEDIATE == timeout) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->u.ewmask = events; + if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, timeout) < MSG_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + m = ctp->epending & events; + } + m ^= m & (m - (eventmask_t)1); + ctp->epending &= ~m; + chSysUnlock(); + + return m; +} + +/** + * @brief Waits for any of the specified events. + * @details The function waits for any event among those specified in + * @p events to become pending then the events are cleared and + * returned. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS enables all the events + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return The mask of the served and cleared events. + * @retval 0 if the operation has timed out. + * + * @api + */ +eventmask_t chEvtWaitAnyTimeout(eventmask_t events, sysinterval_t timeout) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + m = ctp->epending & events; + if (m == (eventmask_t)0) { + if (TIME_IMMEDIATE == timeout) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->u.ewmask = events; + if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, timeout) < MSG_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + m = ctp->epending & events; + } + ctp->epending &= ~m; + chSysUnlock(); + + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p events to + * become pending then the events are cleared and returned. + * + * @param[in] events events that the function should wait + * for, @p ALL_EVENTS requires all the events + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return The mask of the served and cleared events. + * @retval 0 if the operation has timed out. + * + * @api + */ +eventmask_t chEvtWaitAllTimeout(eventmask_t events, sysinterval_t timeout) { + thread_t *ctp = currp; + + chSysLock(); + if ((ctp->epending & events) != events) { + if (TIME_IMMEDIATE == timeout) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->u.ewmask = events; + if (chSchGoSleepTimeoutS(CH_STATE_WTANDEVT, timeout) < MSG_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + } + ctp->epending &= ~events; + chSysUnlock(); + + return events; +} +#endif /* CH_CFG_USE_EVENTS_TIMEOUT == TRUE */ + +#endif /* CH_CFG_USE_EVENTS == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chmsg.c b/ChibiOS_20.3.2/os/rt/src/chmsg.c new file mode 100644 index 0000000..2003dd0 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chmsg.c @@ -0,0 +1,221 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chmsg.c + * @brief Messages code. + * + * @addtogroup messages + * @details Synchronous inter-thread messages APIs and services. + *

Operation Mode

+ * Synchronous messages are an easy to use and fast IPC mechanism, + * threads can both act as message servers and/or message clients, + * the mechanism allows data to be carried in both directions. Note + * that messages are not copied between the client and server threads + * but just a pointer passed so the exchange is very time + * efficient.
+ * Messages are scalar data types of type @p msg_t that are guaranteed + * to be size compatible with data pointers. Note that on some + * architectures function pointers can be larger that @p msg_t.
+ * Messages are usually processed in FIFO order but it is possible to + * process them in priority order by enabling the + * @p CH_CFG_USE_MESSAGES_PRIORITY option in @p chconf.h.
+ * @pre In order to use the message APIs the @p CH_CFG_USE_MESSAGES option + * must be enabled in @p chconf.h. + * @post Enabling messages requires 6-12 (depending on the architecture) + * extra bytes in the @p thread_t structure. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_MESSAGES == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if CH_CFG_USE_MESSAGES_PRIORITY == TRUE +#define msg_insert(tp, qp) queue_prio_insert(tp, qp) +#else +#define msg_insert(tp, qp) queue_insert(tp, qp) +#endif + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Sends a message to the specified thread. + * @details The sender is stopped until the receiver executes a + * @p chMsgRelease()after receiving the message. + * + * @param[in] tp the pointer to the thread + * @param[in] msg the message + * @return The answer message from @p chMsgRelease(). + * + * @api + */ +msg_t chMsgSend(thread_t *tp, msg_t msg) { + thread_t *ctp = currp; + + chDbgCheck(tp != NULL); + + chSysLock(); + ctp->u.sentmsg = msg; + msg_insert(ctp, &tp->msgqueue); + if (tp->state == CH_STATE_WTMSG) { + (void) chSchReadyI(tp); + } + chSchGoSleepS(CH_STATE_SNDMSGQ); + msg = ctp->u.rdymsg; + chSysUnlock(); + + return msg; +} + +/** + * @brief Suspends the thread and waits for an incoming message. + * @post After receiving a message the function @p chMsgGet() must be + * called in order to retrieve the message and then @p chMsgRelease() + * must be invoked in order to acknowledge the reception and send + * the answer. + * @note If the message is a pointer then you can assume that the data + * pointed by the message is stable until you invoke @p chMsgRelease() + * because the sending thread is suspended until then. + * @note The reference counter of the sender thread is not increased, the + * returned pointer is a temporary reference. + * + * @return A pointer to the thread carrying the message. + * + * @sclass + */ +thread_t *chMsgWaitS(void) { + thread_t *tp; + + chDbgCheckClassS(); + + if (!chMsgIsPendingI(currp)) { + chSchGoSleepS(CH_STATE_WTMSG); + } + tp = queue_fifo_remove(&currp->msgqueue); + tp->state = CH_STATE_SNDMSG; + + return tp; +} + +/** + * @brief Suspends the thread and waits for an incoming message or a + * timeout to occur. + * @post After receiving a message the function @p chMsgGet() must be + * called in order to retrieve the message and then @p chMsgRelease() + * must be invoked in order to acknowledge the reception and send + * the answer. + * @note If the message is a pointer then you can assume that the data + * pointed by the message is stable until you invoke @p chMsgRelease() + * because the sending thread is suspended until then. + * @note The reference counter of the sender thread is not increased, the + * returned pointer is a temporary reference. + * + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_INFINITE no timeout. + * . + * @return A pointer to the thread carrying the message. + * @retval NULL if a timeout occurred. + * + * @sclass + */ +thread_t *chMsgWaitTimeoutS(sysinterval_t timeout) { + thread_t *tp; + + chDbgCheckClassS(); + + if (!chMsgIsPendingI(currp)) { + if (chSchGoSleepTimeoutS(CH_STATE_WTMSG, timeout) != MSG_OK) { + return NULL; + } + } + tp = queue_fifo_remove(&currp->msgqueue); + tp->state = CH_STATE_SNDMSG; + + return tp; +} + +/** + * @brief Poll to check for an incoming message. + * @post If a message is available the function @p chMsgGet() must be + * called in order to retrieve the message and then @p chMsgRelease() + * must be invoked in order to acknowledge the reception and send + * the answer. + * @note If the message is a pointer then you can assume that the data + * pointed by the message is stable until you invoke @p chMsgRelease() + * because the sending thread is suspended until then. + * @note The reference counter of the sender thread is not increased, the + * returned pointer is a temporary reference. + * + * @return Result of the poll. + * @retval NULL if no incoming message waiting. + * + * @sclass + */ +thread_t *chMsgPollS(void) { + thread_t *tp = NULL; + + if (chMsgIsPendingI(currp)) { + tp = queue_fifo_remove(&currp->msgqueue); + tp->state = CH_STATE_SNDMSG; + } + + return tp; +} + +/** + * @brief Releases a sender thread specifying a response message. + * @pre Invoke this function only after a message has been received + * using @p chMsgWait(). + * + * @param[in] tp pointer to the thread + * @param[in] msg message to be returned to the sender + * + * @api + */ +void chMsgRelease(thread_t *tp, msg_t msg) { + + chSysLock(); + chDbgAssert(tp->state == CH_STATE_SNDMSG, "invalid state"); + chMsgReleaseS(tp, msg); + chSysUnlock(); +} + +#endif /* CH_CFG_USE_MESSAGES == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chmtx.c b/ChibiOS_20.3.2/os/rt/src/chmtx.c new file mode 100644 index 0000000..e7744f1 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chmtx.c @@ -0,0 +1,537 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chmtx.c + * @brief Mutexes code. + * + * @addtogroup mutexes + * @details Mutexes related APIs and services. + *

Operation mode

+ * A mutex is a threads synchronization object that can be in two + * distinct states: + * - Not owned (unlocked). + * - Owned by a thread (locked). + * . + * Operations defined for mutexes: + * - Lock: The mutex is checked, if the mutex is not owned by + * some other thread then it is associated to the locking thread + * else the thread is queued on the mutex in a list ordered by + * priority. + * - Unlock: The mutex is released by the owner and the highest + * priority thread waiting in the queue, if any, is resumed and made + * owner of the mutex. + * . + *

Constraints

+ * In ChibiOS/RT the Unlock operations must always be performed + * in lock-reverse order. This restriction both improves the + * performance and is required for an efficient implementation + * of the priority inheritance mechanism.
+ * Operating under this restriction also ensures that deadlocks + * are no possible. + * + *

Recursive mode

+ * By default mutexes are not recursive, this mean that it is not + * possible to take a mutex already owned by the same thread. + * It is possible to enable the recursive behavior by enabling the + * option @p CH_CFG_USE_MUTEXES_RECURSIVE. + * + *

The priority inversion problem

+ * The mutexes in ChibiOS/RT implements the full priority + * inheritance mechanism in order handle the priority inversion + * problem.
+ * When a thread is queued on a mutex, any thread, directly or + * indirectly, holding the mutex gains the same priority of the + * waiting thread (if their priority was not already equal or higher). + * The mechanism works with any number of nested mutexes and any + * number of involved threads. The algorithm complexity (worst case) + * is N with N equal to the number of nested mutexes. + * @pre In order to use the mutex APIs the @p CH_CFG_USE_MUTEXES option + * must be enabled in @p chconf.h. + * @post Enabling mutexes requires 5-12 (depending on the architecture) + * extra bytes in the @p thread_t structure. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes s @p mutex_t structure. + * + * @param[out] mp pointer to a @p mutex_t structure + * + * @init + */ +void chMtxObjectInit(mutex_t *mp) { + + chDbgCheck(mp != NULL); + + queue_init(&mp->queue); + mp->owner = NULL; +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + mp->cnt = (cnt_t)0; +#endif +} + +/** + * @brief Locks the specified mutex. + * @post The mutex is locked and inserted in the per-thread stack of owned + * mutexes. + * + * @param[in] mp pointer to the @p mutex_t structure + * + * @api + */ +void chMtxLock(mutex_t *mp) { + + chSysLock(); + chMtxLockS(mp); + chSysUnlock(); +} + +/** + * @brief Locks the specified mutex. + * @post The mutex is locked and inserted in the per-thread stack of owned + * mutexes. + * + * @param[in] mp pointer to the @p mutex_t structure + * + * @sclass + */ +void chMtxLockS(mutex_t *mp) { + thread_t *ctp = currp; + + chDbgCheckClassS(); + chDbgCheck(mp != NULL); + + /* Is the mutex already locked? */ + if (mp->owner != NULL) { +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + + chDbgAssert(mp->cnt >= (cnt_t)1, "counter is not positive"); + + /* If the mutex is already owned by this thread, the counter is increased + and there is no need of more actions.*/ + if (mp->owner == ctp) { + mp->cnt++; + } + else { +#endif + /* Priority inheritance protocol; explores the thread-mutex dependencies + boosting the priority of all the affected threads to equal the + priority of the running thread requesting the mutex.*/ + thread_t *tp = mp->owner; + + /* Does the running thread have higher priority than the mutex + owning thread? */ + while (tp->prio < ctp->prio) { + /* Make priority of thread tp match the running thread's priority.*/ + tp->prio = ctp->prio; + + /* The following states need priority queues reordering.*/ + switch (tp->state) { + case CH_STATE_WTMTX: + /* Re-enqueues the mutex owner with its new priority.*/ + queue_prio_insert(queue_dequeue(tp), &tp->u.wtmtxp->queue); + tp = tp->u.wtmtxp->owner; + /*lint -e{9042} [16.1] Continues the while.*/ + continue; +#if (CH_CFG_USE_CONDVARS == TRUE) || \ + ((CH_CFG_USE_SEMAPHORES == TRUE) && \ + (CH_CFG_USE_SEMAPHORES_PRIORITY == TRUE)) || \ + ((CH_CFG_USE_MESSAGES == TRUE) && \ + (CH_CFG_USE_MESSAGES_PRIORITY == TRUE)) +#if CH_CFG_USE_CONDVARS == TRUE + case CH_STATE_WTCOND: +#endif +#if (CH_CFG_USE_SEMAPHORES == TRUE) && \ + (CH_CFG_USE_SEMAPHORES_PRIORITY == TRUE) + case CH_STATE_WTSEM: +#endif +#if (CH_CFG_USE_MESSAGES == TRUE) && (CH_CFG_USE_MESSAGES_PRIORITY == TRUE) + case CH_STATE_SNDMSGQ: +#endif + /* Re-enqueues tp with its new priority on the queue.*/ + queue_prio_insert(queue_dequeue(tp), &tp->u.wtmtxp->queue); + break; +#endif + case CH_STATE_READY: +#if CH_DBG_ENABLE_ASSERTS == TRUE + /* Prevents an assertion in chSchReadyI().*/ + tp->state = CH_STATE_CURRENT; +#endif + /* Re-enqueues tp with its new priority on the ready list.*/ + (void) chSchReadyI(queue_dequeue(tp)); + break; + default: + /* Nothing to do for other states.*/ + break; + } + break; + } + + /* Sleep on the mutex.*/ + queue_prio_insert(ctp, &mp->queue); + ctp->u.wtmtxp = mp; + chSchGoSleepS(CH_STATE_WTMTX); + + /* It is assumed that the thread performing the unlock operation assigns + the mutex to this thread.*/ + chDbgAssert(mp->owner == ctp, "not owner"); + chDbgAssert(ctp->mtxlist == mp, "not owned"); +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + chDbgAssert(mp->cnt == (cnt_t)1, "counter is not one"); + } +#endif + } + else { +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + chDbgAssert(mp->cnt == (cnt_t)0, "counter is not zero"); + + mp->cnt++; +#endif + /* It was not owned, inserted in the owned mutexes list.*/ + mp->owner = ctp; + mp->next = ctp->mtxlist; + ctp->mtxlist = mp; + } +} + +/** + * @brief Tries to lock a mutex. + * @details This function attempts to lock a mutex, if the mutex is already + * locked by another thread then the function exits without waiting. + * @post The mutex is locked and inserted in the per-thread stack of owned + * mutexes. + * @note This function does not have any overhead related to the + * priority inheritance mechanism because it does not try to + * enter a sleep state. + * + * @param[in] mp pointer to the @p mutex_t structure + * @return The operation status. + * @retval true if the mutex has been successfully acquired + * @retval false if the lock attempt failed. + * + * @api + */ +bool chMtxTryLock(mutex_t *mp) { + bool b; + + chSysLock(); + b = chMtxTryLockS(mp); + chSysUnlock(); + + return b; +} + +/** + * @brief Tries to lock a mutex. + * @details This function attempts to lock a mutex, if the mutex is already + * taken by another thread then the function exits without waiting. + * @post The mutex is locked and inserted in the per-thread stack of owned + * mutexes. + * @note This function does not have any overhead related to the + * priority inheritance mechanism because it does not try to + * enter a sleep state. + * + * @param[in] mp pointer to the @p mutex_t structure + * @return The operation status. + * @retval true if the mutex has been successfully acquired + * @retval false if the lock attempt failed. + * + * @sclass + */ +bool chMtxTryLockS(mutex_t *mp) { + + chDbgCheckClassS(); + chDbgCheck(mp != NULL); + + if (mp->owner != NULL) { +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + + chDbgAssert(mp->cnt >= (cnt_t)1, "counter is not positive"); + + if (mp->owner == currp) { + mp->cnt++; + return true; + } +#endif + return false; + } +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + + chDbgAssert(mp->cnt == (cnt_t)0, "counter is not zero"); + + mp->cnt++; +#endif + mp->owner = currp; + mp->next = currp->mtxlist; + currp->mtxlist = mp; + return true; +} + +/** + * @brief Unlocks the specified mutex. + * @note Mutexes must be unlocked in reverse lock order. Violating this + * rules will result in a panic if assertions are enabled. + * @pre The invoking thread must have at least one owned mutex. + * @post The mutex is unlocked and removed from the per-thread stack of + * owned mutexes. + * + * @param[in] mp pointer to the @p mutex_t structure + * + * @api + */ +void chMtxUnlock(mutex_t *mp) { + thread_t *ctp = currp; + mutex_t *lmp; + + chDbgCheck(mp != NULL); + + chSysLock(); + + chDbgAssert(ctp->mtxlist != NULL, "owned mutexes list empty"); + chDbgAssert(ctp->mtxlist->owner == ctp, "ownership failure"); +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + chDbgAssert(mp->cnt >= (cnt_t)1, "counter is not positive"); + + if (--mp->cnt == (cnt_t)0) { +#endif + + chDbgAssert(ctp->mtxlist == mp, "not next in list"); + + /* Removes the top mutex from the thread's owned mutexes list and marks + it as not owned. Note, it is assumed to be the same mutex passed as + parameter of this function.*/ + ctp->mtxlist = mp->next; + + /* If a thread is waiting on the mutex then the fun part begins.*/ + if (chMtxQueueNotEmptyS(mp)) { + thread_t *tp; + + /* Recalculates the optimal thread priority by scanning the owned + mutexes list.*/ + tprio_t newprio = ctp->realprio; + lmp = ctp->mtxlist; + while (lmp != NULL) { + /* If the highest priority thread waiting in the mutexes list has a + greater priority than the current thread base priority then the + final priority will have at least that priority.*/ + if (chMtxQueueNotEmptyS(lmp) && + (lmp->queue.next->prio > newprio)) { + newprio = lmp->queue.next->prio; + } + lmp = lmp->next; + } + + /* Assigns to the current thread the highest priority among all the + waiting threads.*/ + ctp->prio = newprio; + + /* Awakens the highest priority thread waiting for the unlocked mutex and + assigns the mutex to it.*/ +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + mp->cnt = (cnt_t)1; +#endif + tp = queue_fifo_remove(&mp->queue); + mp->owner = tp; + mp->next = tp->mtxlist; + tp->mtxlist = mp; + + /* Note, not using chSchWakeupS() because that function expects the + current thread to have the higher or equal priority than the ones + in the ready list. This is not necessarily true here because we + just changed priority.*/ + (void) chSchReadyI(tp); + chSchRescheduleS(); + } + else { + mp->owner = NULL; + } +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + } +#endif + + chSysUnlock(); +} + +/** + * @brief Unlocks the specified mutex. + * @note Mutexes must be unlocked in reverse lock order. Violating this + * rules will result in a panic if assertions are enabled. + * @pre The invoking thread must have at least one owned mutex. + * @post The mutex is unlocked and removed from the per-thread stack of + * owned mutexes. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. + * + * @param[in] mp pointer to the @p mutex_t structure + * + * @sclass + */ +void chMtxUnlockS(mutex_t *mp) { + thread_t *ctp = currp; + mutex_t *lmp; + + chDbgCheckClassS(); + chDbgCheck(mp != NULL); + + chDbgAssert(ctp->mtxlist != NULL, "owned mutexes list empty"); + chDbgAssert(ctp->mtxlist->owner == ctp, "ownership failure"); +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + chDbgAssert(mp->cnt >= (cnt_t)1, "counter is not positive"); + + if (--mp->cnt == (cnt_t)0) { +#endif + + chDbgAssert(ctp->mtxlist == mp, "not next in list"); + + /* Removes the top mutex from the thread's owned mutexes list and marks + it as not owned. Note, it is assumed to be the same mutex passed as + parameter of this function.*/ + ctp->mtxlist = mp->next; + + /* If a thread is waiting on the mutex then the fun part begins.*/ + if (chMtxQueueNotEmptyS(mp)) { + thread_t *tp; + + /* Recalculates the optimal thread priority by scanning the owned + mutexes list.*/ + tprio_t newprio = ctp->realprio; + lmp = ctp->mtxlist; + while (lmp != NULL) { + /* If the highest priority thread waiting in the mutexes list has a + greater priority than the current thread base priority then the + final priority will have at least that priority.*/ + if (chMtxQueueNotEmptyS(lmp) && + (lmp->queue.next->prio > newprio)) { + newprio = lmp->queue.next->prio; + } + lmp = lmp->next; + } + + /* Assigns to the current thread the highest priority among all the + waiting threads.*/ + ctp->prio = newprio; + + /* Awakens the highest priority thread waiting for the unlocked mutex and + assigns the mutex to it.*/ +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + mp->cnt = (cnt_t)1; +#endif + tp = queue_fifo_remove(&mp->queue); + mp->owner = tp; + mp->next = tp->mtxlist; + tp->mtxlist = mp; + (void) chSchReadyI(tp); + } + else { + mp->owner = NULL; + } +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + } +#endif +} + +/** + * @brief Unlocks all mutexes owned by the invoking thread. + * @post The stack of owned mutexes is emptied and all the found + * mutexes are unlocked. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. + * @note This function is MUCH MORE efficient than releasing the + * mutexes one by one and not just because the call overhead, + * this function does not have any overhead related to the priority + * inheritance mechanism. + * + * @sclass + */ +void chMtxUnlockAllS(void) { + thread_t *ctp = currp; + + if (ctp->mtxlist != NULL) { + do { + mutex_t *mp = ctp->mtxlist; + ctp->mtxlist = mp->next; + if (chMtxQueueNotEmptyS(mp)) { + thread_t *tp; +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + mp->cnt = (cnt_t)1; +#endif + tp = queue_fifo_remove(&mp->queue); + mp->owner = tp; + mp->next = tp->mtxlist; + tp->mtxlist = mp; + (void) chSchReadyI(tp); + } + else { +#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE + mp->cnt = (cnt_t)0; +#endif + mp->owner = NULL; + } + } while (ctp->mtxlist != NULL); + ctp->prio = ctp->realprio; + chSchRescheduleS(); + } +} + +/** + * @brief Unlocks all mutexes owned by the invoking thread. + * @post The stack of owned mutexes is emptied and all the found + * mutexes are unlocked. + * @note This function is MUCH MORE efficient than releasing the + * mutexes one by one and not just because the call overhead, + * this function does not have any overhead related to the priority + * inheritance mechanism. + * + * @api + */ +void chMtxUnlockAll(void) { + + chSysLock(); + chMtxUnlockAllS(); + chSysUnlock(); +} + +#endif /* CH_CFG_USE_MUTEXES == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chregistry.c b/ChibiOS_20.3.2/os/rt/src/chregistry.c new file mode 100644 index 0000000..69be197 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chregistry.c @@ -0,0 +1,268 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chregistry.c + * @brief Threads registry code. + * + * @addtogroup registry + * @details Threads Registry related APIs and services. + *

Operation mode

+ * The Threads Registry is a double linked list that holds all the + * active threads in the system.
+ * Operations defined for the registry: + * - First, returns the first, in creation order, active thread + * in the system. + * - Next, returns the next, in creation order, active thread + * in the system. + * . + * The registry is meant to be mainly a debug feature, for example, + * using the registry a debugger can enumerate the active threads + * in any given moment or the shell can print the active threads + * and their state.
+ * Another possible use is for centralized threads memory management, + * terminating threads can pulse an event source and an event handler + * can perform a scansion of the registry in order to recover the + * memory. + * @pre In order to use the threads registry the @p CH_CFG_USE_REGISTRY + * option must be enabled in @p chconf.h. + * @{ + */ + +#include + +#include "ch.h" + +#if (CH_CFG_USE_REGISTRY == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#define _offsetof(st, m) \ + /*lint -save -e9005 -e9033 -e413 [11.8, 10.8 1.3] Normal pointers + arithmetic, it is safe.*/ \ + ((size_t)((char *)&((st *)0)->m - (char *)0)) \ + /*lint -restore*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/* + * OS signature in ROM plus debug-related information. + */ +ROMCONST chdebug_t ch_debug = { + {'m', 'a', 'i', 'n'}, + (uint8_t)0, + (uint8_t)sizeof (chdebug_t), + (uint16_t)(((unsigned)CH_KERNEL_MAJOR << 11U) | + ((unsigned)CH_KERNEL_MINOR << 6U) | + ((unsigned)CH_KERNEL_PATCH << 0U)), + (uint8_t)sizeof (void *), + (uint8_t)sizeof (systime_t), + (uint8_t)sizeof (thread_t), + (uint8_t)_offsetof(thread_t, prio), + (uint8_t)_offsetof(thread_t, ctx), + (uint8_t)_offsetof(thread_t, newer), + (uint8_t)_offsetof(thread_t, older), + (uint8_t)_offsetof(thread_t, name), +#if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE) + (uint8_t)_offsetof(thread_t, wabase), +#else + (uint8_t)0, +#endif + (uint8_t)_offsetof(thread_t, state), + (uint8_t)_offsetof(thread_t, flags), +#if CH_CFG_USE_DYNAMIC == TRUE + (uint8_t)_offsetof(thread_t, refs), +#else + (uint8_t)0, +#endif +#if CH_CFG_TIME_QUANTUM > 0 + (uint8_t)_offsetof(thread_t, ticks), +#else + (uint8_t)0, +#endif +#if CH_DBG_THREADS_PROFILING == TRUE + (uint8_t)_offsetof(thread_t, time) +#else + (uint8_t)0 +#endif +}; + +/** + * @brief Returns the first thread in the system. + * @details Returns the most ancient thread in the system, usually this is + * the main thread unless it terminated. A reference is added to the + * returned thread in order to make sure its status is not lost. + * @note This function cannot return @p NULL because there is always at + * least one thread in the system. + * + * @return A reference to the most ancient thread. + * + * @api + */ +thread_t *chRegFirstThread(void) { + thread_t *tp; + + chSysLock(); + tp = ch.rlist.newer; +#if CH_CFG_USE_DYNAMIC == TRUE + tp->refs++; +#endif + chSysUnlock(); + + return tp; +} + +/** + * @brief Returns the thread next to the specified one. + * @details The reference counter of the specified thread is decremented and + * the reference counter of the returned thread is incremented. + * + * @param[in] tp pointer to the thread + * @return A reference to the next thread. + * @retval NULL if there is no next thread. + * + * @api + */ +thread_t *chRegNextThread(thread_t *tp) { + thread_t *ntp; + + chSysLock(); + ntp = tp->newer; + /*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/ + if (ntp == (thread_t *)&ch.rlist) { + /*lint -restore*/ + ntp = NULL; + } +#if CH_CFG_USE_DYNAMIC == TRUE + else { + chDbgAssert(ntp->refs < (trefs_t)255, "too many references"); + ntp->refs++; + } +#endif + chSysUnlock(); +#if CH_CFG_USE_DYNAMIC == TRUE + chThdRelease(tp); +#endif + + return ntp; +} + +/** + * @brief Retrieves a thread pointer by name. + * @note The reference counter of the found thread is increased by one so + * it cannot be disposed incidentally after the pointer has been + * returned. + * + * @param[in] name the thread name + * @return A pointer to the found thread. + * @retval NULL if a matching thread has not been found. + * + * @api + */ +thread_t *chRegFindThreadByName(const char *name) { + thread_t *ctp; + + /* Scanning registry.*/ + ctp = chRegFirstThread(); + do { + if (strcmp(chRegGetThreadNameX(ctp), name) == 0) { + return ctp; + } + ctp = chRegNextThread(ctp); + } while (ctp != NULL); + + return NULL; +} + +/** + * @brief Confirms that a pointer is a valid thread pointer. + * @note The reference counter of the found thread is increased by one so + * it cannot be disposed incidentally after the pointer has been + * returned. + * + * @param[in] tp pointer to the thread + * @return A pointer to the found thread. + * @retval NULL if a matching thread has not been found. + * + * @api + */ +thread_t *chRegFindThreadByPointer(thread_t *tp) { + thread_t *ctp; + + /* Scanning registry.*/ + ctp = chRegFirstThread(); + do { + if (ctp == tp) { + return ctp; + } + ctp = chRegNextThread(ctp); + } while (ctp != NULL); + + return NULL; +} + +#if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE) || \ + defined(__DOXYGEN__) +/** + * @brief Confirms that a working area is being used by some active thread. + * @note The reference counter of the found thread is increased by one so + * it cannot be disposed incidentally after the pointer has been + * returned. + * + * @param[in] wa pointer to a static working area + * @return A pointer to the found thread. + * @retval NULL if a matching thread has not been found. + * + * @api + */ +thread_t *chRegFindThreadByWorkingArea(stkalign_t *wa) { + thread_t *ctp; + + /* Scanning registry.*/ + ctp = chRegFirstThread(); + do { + if (chThdGetWorkingAreaX(ctp) == wa) { + return ctp; + } + ctp = chRegNextThread(ctp); + } while (ctp != NULL); + + return NULL; +} +#endif + +#endif /* CH_CFG_USE_REGISTRY == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chschd.c b/ChibiOS_20.3.2/os/rt/src/chschd.c new file mode 100644 index 0000000..540b1c2 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chschd.c @@ -0,0 +1,610 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chschd.c + * @brief Scheduler code. + * + * @addtogroup scheduler + * @details This module provides the default portable scheduler code. + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/** + * @brief System data structures. + */ +ch_system_t ch; + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Scheduler initialization. + * + * @notapi + */ +void _scheduler_init(void) { + + queue_init(&ch.rlist.queue); + ch.rlist.prio = NOPRIO; +#if CH_CFG_USE_REGISTRY == TRUE + ch.rlist.newer = (thread_t *)&ch.rlist; + ch.rlist.older = (thread_t *)&ch.rlist; +#endif +} + +#if (CH_CFG_OPTIMIZE_SPEED == FALSE) || defined(__DOXYGEN__) +/** + * @brief Inserts a thread into a priority ordered queue. + * @note The insertion is done by scanning the list from the highest + * priority toward the lowest. + * + * @param[in] tp the pointer to the thread to be inserted in the list + * @param[in] tqp the pointer to the threads list header + * + * @notapi + */ +void queue_prio_insert(thread_t *tp, threads_queue_t *tqp) { + + thread_t *cp = (thread_t *)tqp; + do { + cp = cp->queue.next; + } while ((cp != (thread_t *)tqp) && (cp->prio >= tp->prio)); + tp->queue.next = cp; + tp->queue.prev = cp->queue.prev; + tp->queue.prev->queue.next = tp; + cp->queue.prev = tp; +} + +/** + * @brief Inserts a thread into a queue. + * + * @param[in] tp the pointer to the thread to be inserted in the list + * @param[in] tqp the pointer to the threads list header + * + * @notapi + */ +void queue_insert(thread_t *tp, threads_queue_t *tqp) { + + tp->queue.next = (thread_t *)tqp; + tp->queue.prev = tqp->prev; + tp->queue.prev->queue.next = tp; + tqp->prev = tp; +} + +/** + * @brief Removes the first-out thread from a queue and returns it. + * @note If the queue is priority ordered then this function returns the + * thread with the highest priority. + * + * @param[in] tqp the pointer to the threads list header + * @return The removed thread pointer. + * + * @notapi + */ +thread_t *queue_fifo_remove(threads_queue_t *tqp) { + thread_t *tp = tqp->next; + + tqp->next = tp->queue.next; + tqp->next->queue.prev = (thread_t *)tqp; + + return tp; +} + +/** + * @brief Removes the last-out thread from a queue and returns it. + * @note If the queue is priority ordered then this function returns the + * thread with the lowest priority. + * + * @param[in] tqp the pointer to the threads list header + * @return The removed thread pointer. + * + * @notapi + */ +thread_t *queue_lifo_remove(threads_queue_t *tqp) { + thread_t *tp = tqp->prev; + + tqp->prev = tp->queue.prev; + tqp->prev->queue.next = (thread_t *)tqp; + + return tp; +} + +/** + * @brief Removes a thread from a queue and returns it. + * @details The thread is removed from the queue regardless of its relative + * position and regardless the used insertion method. + * + * @param[in] tp the pointer to the thread to be removed from the queue + * @return The removed thread pointer. + * + * @notapi + */ +thread_t *queue_dequeue(thread_t *tp) { + + tp->queue.prev->queue.next = tp->queue.next; + tp->queue.next->queue.prev = tp->queue.prev; + + return tp; +} + +/** + * @brief Pushes a thread_t on top of a stack list. + * + * @param[in] tp the pointer to the thread to be inserted in the list + * @param[in] tlp the pointer to the threads list header + * + * @notapi + */ +void list_insert(thread_t *tp, threads_list_t *tlp) { + + tp->queue.next = tlp->next; + tlp->next = tp; +} + +/** + * @brief Pops a thread from the top of a stack list and returns it. + * @pre The list must be non-empty before calling this function. + * + * @param[in] tlp the pointer to the threads list header + * @return The removed thread pointer. + * + * @notapi + */ +thread_t *list_remove(threads_list_t *tlp) { + + thread_t *tp = tlp->next; + tlp->next = tp->queue.next; + + return tp; +} +#endif /* CH_CFG_OPTIMIZE_SPEED */ + +/** + * @brief Inserts a thread in the Ready List placing it behind its peers. + * @details The thread is positioned behind all threads with higher or equal + * priority. + * @pre The thread must not be already inserted in any list through its + * @p next and @p prev or list corruption would occur. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] tp the thread to be made ready + * @return The thread pointer. + * + * @iclass + */ +thread_t *chSchReadyI(thread_t *tp) { + thread_t *cp; + + chDbgCheckClassI(); + chDbgCheck(tp != NULL); + chDbgAssert((tp->state != CH_STATE_READY) && + (tp->state != CH_STATE_FINAL), + "invalid state"); + + tp->state = CH_STATE_READY; + cp = (thread_t *)&ch.rlist.queue; + do { + cp = cp->queue.next; + } while (cp->prio >= tp->prio); + /* Insertion on prev.*/ + tp->queue.next = cp; + tp->queue.prev = cp->queue.prev; + tp->queue.prev->queue.next = tp; + cp->queue.prev = tp; + + return tp; +} + +/** + * @brief Inserts a thread in the Ready List placing it ahead its peers. + * @details The thread is positioned ahead all threads with higher or equal + * priority. + * @pre The thread must not be already inserted in any list through its + * @p next and @p prev or list corruption would occur. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] tp the thread to be made ready + * @return The thread pointer. + * + * @iclass + */ +thread_t *chSchReadyAheadI(thread_t *tp) { + thread_t *cp; + + chDbgCheckClassI(); + chDbgCheck(tp != NULL); + chDbgAssert((tp->state != CH_STATE_READY) && + (tp->state != CH_STATE_FINAL), + "invalid state"); + + tp->state = CH_STATE_READY; + cp = (thread_t *)&ch.rlist.queue; + do { + cp = cp->queue.next; + } while (cp->prio > tp->prio); + /* Insertion on prev.*/ + tp->queue.next = cp; + tp->queue.prev = cp->queue.prev; + tp->queue.prev->queue.next = tp; + cp->queue.prev = tp; + + return tp; +} + +/** + * @brief Puts the current thread to sleep into the specified state. + * @details The thread goes into a sleeping state. The possible + * @ref thread_states are defined into @p threads.h. + * + * @param[in] newstate the new thread state + * + * @sclass + */ +void chSchGoSleepS(tstate_t newstate) { + thread_t *otp = currp; + + chDbgCheckClassS(); + + /* New state.*/ + otp->state = newstate; + +#if CH_CFG_TIME_QUANTUM > 0 + /* The thread is renouncing its remaining time slices so it will have a new + time quantum when it will wakeup.*/ + otp->ticks = (tslices_t)CH_CFG_TIME_QUANTUM; +#endif + + /* Next thread in ready list becomes current.*/ + currp = queue_fifo_remove(&ch.rlist.queue); + currp->state = CH_STATE_CURRENT; + + /* Handling idle-enter hook.*/ + if (currp->prio == IDLEPRIO) { + CH_CFG_IDLE_ENTER_HOOK(); + } + + /* Swap operation as tail call.*/ + chSysSwitch(currp, otp); +} + +/* + * Timeout wakeup callback. + */ +static void wakeup(void *p) { + thread_t *tp = (thread_t *)p; + + chSysLockFromISR(); + switch (tp->state) { + case CH_STATE_READY: + /* Handling the special case where the thread has been made ready by + another thread with higher priority.*/ + chSysUnlockFromISR(); + return; + case CH_STATE_SUSPENDED: + *tp->u.wttrp = NULL; + break; +#if CH_CFG_USE_SEMAPHORES == TRUE + case CH_STATE_WTSEM: + chSemFastSignalI(tp->u.wtsemp); +#endif + /* Falls through.*/ + case CH_STATE_QUEUED: + /* Falls through.*/ +#if (CH_CFG_USE_CONDVARS == TRUE) && (CH_CFG_USE_CONDVARS_TIMEOUT == TRUE) + case CH_STATE_WTCOND: +#endif + /* States requiring dequeuing.*/ + (void) queue_dequeue(tp); + break; + default: + /* Any other state, nothing to do.*/ + break; + } + tp->u.rdymsg = MSG_TIMEOUT; + (void) chSchReadyI(tp); + chSysUnlockFromISR(); +} + +/** + * @brief Puts the current thread to sleep into the specified state with + * timeout specification. + * @details The thread goes into a sleeping state, if it is not awakened + * explicitly within the specified timeout then it is forcibly + * awakened with a @p MSG_TIMEOUT low level message. The possible + * @ref thread_states are defined into @p threads.h. + * + * @param[in] newstate the new thread state + * @param[in] timeout the number of ticks before the operation timeouts, the + * special values are handled as follow: + * - @a TIME_INFINITE the thread enters an infinite sleep + * state, this is equivalent to invoking + * @p chSchGoSleepS() but, of course, less efficient. + * - @a TIME_IMMEDIATE this value is not allowed. + * . + * @return The wakeup message. + * @retval MSG_TIMEOUT if a timeout occurs. + * + * @sclass + */ +msg_t chSchGoSleepTimeoutS(tstate_t newstate, sysinterval_t timeout) { + + chDbgCheckClassS(); + + if (TIME_INFINITE != timeout) { + virtual_timer_t vt; + + chVTDoSetI(&vt, timeout, wakeup, currp); + chSchGoSleepS(newstate); + if (chVTIsArmedI(&vt)) { + chVTDoResetI(&vt); + } + } + else { + chSchGoSleepS(newstate); + } + + return currp->u.rdymsg; +} + +/** + * @brief Wakes up a thread. + * @details The thread is inserted into the ready list or immediately made + * running depending on its relative priority compared to the current + * thread. + * @pre The thread must not be already inserted in any list through its + * @p next and @p prev or list corruption would occur. + * @note It is equivalent to a @p chSchReadyI() followed by a + * @p chSchRescheduleS() but much more efficient. + * @note The function assumes that the current thread has the highest + * priority. + * + * @param[in] ntp the thread to be made ready + * @param[in] msg the wakeup message + * + * @sclass + */ +void chSchWakeupS(thread_t *ntp, msg_t msg) { + thread_t *otp = currp; + + chDbgCheckClassS(); + + chDbgAssert((ch.rlist.queue.next == (thread_t *)&ch.rlist.queue) || + (ch.rlist.current->prio >= ch.rlist.queue.next->prio), + "priority order violation"); + + /* Storing the message to be retrieved by the target thread when it will + restart execution.*/ + ntp->u.rdymsg = msg; + + /* If the waken thread has a not-greater priority than the current + one then it is just inserted in the ready list else it made + running immediately and the invoking thread goes in the ready + list instead.*/ + if (ntp->prio <= otp->prio) { + (void) chSchReadyI(ntp); + } + else { + otp = chSchReadyAheadI(otp); + + /* Handling idle-leave hook.*/ + if (otp->prio == IDLEPRIO) { + CH_CFG_IDLE_LEAVE_HOOK(); + } + + /* The extracted thread is marked as current.*/ + currp = ntp; + ntp->state = CH_STATE_CURRENT; + + /* Swap operation as tail call.*/ + chSysSwitch(ntp, otp); + } +} + +/** + * @brief Performs a reschedule if a higher priority thread is runnable. + * @details If a thread with a higher priority than the current thread is in + * the ready list then make the higher priority thread running. + * + * @sclass + */ +void chSchRescheduleS(void) { + + chDbgCheckClassS(); + + if (chSchIsRescRequiredI()) { + chSchDoRescheduleAhead(); + } +} + +#if !defined(CH_SCH_IS_PREEMPTION_REQUIRED_HOOKED) +/** + * @brief Evaluates if preemption is required. + * @details The decision is taken by comparing the relative priorities and + * depending on the state of the round robin timeout counter. + * @note Not a user function, it is meant to be invoked by the scheduler + * itself or from within the port layer. + * + * @retval true if there is a thread that must go in running state + * immediately. + * @retval false if preemption is not required. + * + * @special + */ +bool chSchIsPreemptionRequired(void) { + tprio_t p1 = firstprio(&ch.rlist.queue); + tprio_t p2 = currp->prio; + +#if CH_CFG_TIME_QUANTUM > 0 + /* If the running thread has not reached its time quantum, reschedule only + if the first thread on the ready queue has a higher priority. + Otherwise, if the running thread has used up its time quantum, reschedule + if the first thread on the ready queue has equal or higher priority.*/ + return (currp->ticks > (tslices_t)0) ? (p1 > p2) : (p1 >= p2); +#else + /* If the round robin preemption feature is not enabled then performs a + simpler comparison.*/ + return p1 > p2; +#endif +} +#endif /* !defined(CH_SCH_IS_PREEMPTION_REQUIRED_HOOKED) */ + +/** + * @brief Switches to the first thread on the runnable queue. + * @details The current thread is positioned in the ready list behind all + * threads having the same priority. The thread regains its time + * quantum. + * @note Not a user function, it is meant to be invoked by the scheduler + * itself. + * + * @special + */ +void chSchDoRescheduleBehind(void) { + thread_t *otp = currp; + + /* Picks the first thread from the ready queue and makes it current.*/ + currp = queue_fifo_remove(&ch.rlist.queue); + currp->state = CH_STATE_CURRENT; + + /* Handling idle-leave hook.*/ + if (otp->prio == IDLEPRIO) { + CH_CFG_IDLE_LEAVE_HOOK(); + } + +#if CH_CFG_TIME_QUANTUM > 0 + /* It went behind peers so it gets a new time quantum.*/ + otp->ticks = (tslices_t)CH_CFG_TIME_QUANTUM; +#endif + + /* Placing in ready list behind peers.*/ + otp = chSchReadyI(otp); + + /* Swap operation as tail call.*/ + chSysSwitch(currp, otp); +} + +/** + * @brief Switches to the first thread on the runnable queue. + * @details The current thread is positioned in the ready list ahead of all + * threads having the same priority. + * @note Not a user function, it is meant to be invoked by the scheduler + * itself. + * + * @special + */ +void chSchDoRescheduleAhead(void) { + thread_t *otp = currp; + + /* Picks the first thread from the ready queue and makes it current.*/ + currp = queue_fifo_remove(&ch.rlist.queue); + currp->state = CH_STATE_CURRENT; + + /* Handling idle-leave hook.*/ + if (otp->prio == IDLEPRIO) { + CH_CFG_IDLE_LEAVE_HOOK(); + } + + /* Placing in ready list ahead of peers.*/ + otp = chSchReadyAheadI(otp); + + /* Swap operation as tail call.*/ + chSysSwitch(currp, otp); +} + +#if !defined(CH_SCH_DO_RESCHEDULE_HOOKED) +/** + * @brief Switches to the first thread on the runnable queue. + * @details The current thread is positioned in the ready list behind or + * ahead of all threads having the same priority depending on + * if it used its whole time slice. + * @note Not a user function, it is meant to be invoked by the scheduler + * itself or from within the port layer. + * + * @special + */ +void chSchDoReschedule(void) { + thread_t *otp = currp; + + /* Picks the first thread from the ready queue and makes it current.*/ + currp = queue_fifo_remove(&ch.rlist.queue); + currp->state = CH_STATE_CURRENT; + + /* Handling idle-leave hook.*/ + if (otp->prio == IDLEPRIO) { + CH_CFG_IDLE_LEAVE_HOOK(); + } + +#if CH_CFG_TIME_QUANTUM > 0 + /* If CH_CFG_TIME_QUANTUM is enabled then there are two different scenarios + to handle on preemption: time quantum elapsed or not.*/ + if (otp->ticks == (tslices_t)0) { + + /* The thread consumed its time quantum so it is enqueued behind threads + with same priority level, however, it acquires a new time quantum.*/ + otp = chSchReadyI(otp); + + /* The thread being swapped out receives a new time quantum.*/ + otp->ticks = (tslices_t)CH_CFG_TIME_QUANTUM; + } + else { + /* The thread didn't consume all its time quantum so it is put ahead of + threads with equal priority and does not acquire a new time quantum.*/ + otp = chSchReadyAheadI(otp); + } +#else /* !(CH_CFG_TIME_QUANTUM > 0) */ + /* If the round-robin mechanism is disabled then the thread goes always + ahead of its peers.*/ + otp = chSchReadyAheadI(otp); +#endif /* !(CH_CFG_TIME_QUANTUM > 0) */ + + /* Swap operation as tail call.*/ + chSysSwitch(currp, otp); +} +#endif /* !defined(CH_SCH_DO_RESCHEDULE_HOOKED) */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chsem.c b/ChibiOS_20.3.2/os/rt/src/chsem.c new file mode 100644 index 0000000..7671aec --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chsem.c @@ -0,0 +1,405 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chsem.c + * @brief Semaphores code. + * + * @addtogroup semaphores + * @details Semaphores related APIs and services. + *

Operation mode

+ * Semaphores are a flexible synchronization primitive, ChibiOS/RT + * implements semaphores in their "counting semaphores" variant as + * defined by Edsger Dijkstra plus several enhancements like: + * - Wait operation with timeout. + * - Reset operation. + * - Atomic wait+signal operation. + * - Return message from the wait operation (OK, RESET, TIMEOUT). + * . + * The binary semaphores variant can be easily implemented using + * counting semaphores.
+ * Operations defined for semaphores: + * - Signal: The semaphore counter is increased and if the + * result is non-positive then a waiting thread is removed from + * the semaphore queue and made ready for execution. + * - Wait: The semaphore counter is decreased and if the result + * becomes negative the thread is queued in the semaphore and + * suspended. + * - Reset: The semaphore counter is reset to a non-negative + * value and all the threads in the queue are released. + * . + * Semaphores can be used as guards for mutual exclusion zones + * (note that mutexes are recommended for this kind of use) but + * also have other uses, queues guards and counters for example.
+ * Semaphores usually use a FIFO queuing strategy but it is possible + * to make them order threads by priority by enabling + * @p CH_CFG_USE_SEMAPHORES_PRIORITY in @p chconf.h. + * @pre In order to use the semaphore APIs the @p CH_CFG_USE_SEMAPHORES + * option must be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_SEMAPHORES == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if CH_CFG_USE_SEMAPHORES_PRIORITY == TRUE +#define sem_insert(tp, qp) queue_prio_insert(tp, qp) +#else +#define sem_insert(tp, qp) queue_insert(tp, qp) +#endif + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes a semaphore with the specified counter value. + * + * @param[out] sp pointer to a @p semaphore_t structure + * @param[in] n initial value of the semaphore counter. Must be + * non-negative. + * + * @init + */ +void chSemObjectInit(semaphore_t *sp, cnt_t n) { + + chDbgCheck((sp != NULL) && (n >= (cnt_t)0)); + + queue_init(&sp->queue); + sp->cnt = n; +} + +/** + * @brief Performs a reset operation on the semaphore. + * @post After invoking this function all the threads waiting on the + * semaphore, if any, are released and the semaphore counter is set + * to the specified, non negative, value. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] n the new value of the semaphore counter. The value must + * be non-negative. + * @param[in] msg message to be sent + * + * @api + */ +void chSemResetWithMessage(semaphore_t *sp, cnt_t n, msg_t msg) { + + chSysLock(); + chSemResetWithMessageI(sp, n, msg); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Performs a reset operation on the semaphore. + * @post After invoking this function all the threads waiting on the + * semaphore, if any, are released and the semaphore counter is set + * to the specified, non negative, value. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] n the new value of the semaphore counter. The value must + * be non-negative. + * @param[in] msg message to be sent + * + * @iclass + */ +void chSemResetWithMessageI(semaphore_t *sp, cnt_t n, msg_t msg) { + + chDbgCheckClassI(); + chDbgCheck((sp != NULL) && (n >= (cnt_t)0)); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + + sp->cnt = n; + while (queue_notempty(&sp->queue)) { + chSchReadyI(queue_lifo_remove(&sp->queue))->u.rdymsg = msg; + } +} + +/** + * @brief Performs a wait operation on a semaphore. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @return A message specifying how the invoking thread has been + * released from the semaphore. + * @retval MSG_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). + * + * @api + */ +msg_t chSemWait(semaphore_t *sp) { + msg_t msg; + + chSysLock(); + msg = chSemWaitS(sp); + chSysUnlock(); + + return msg; +} + +/** + * @brief Performs a wait operation on a semaphore. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @return A message specifying how the invoking thread has been + * released from the semaphore. + * @retval MSG_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). + * + * @sclass + */ +msg_t chSemWaitS(semaphore_t *sp) { + + chDbgCheckClassS(); + chDbgCheck(sp != NULL); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + + if (--sp->cnt < (cnt_t)0) { + currp->u.wtsemp = sp; + sem_insert(currp, &sp->queue); + chSchGoSleepS(CH_STATE_WTSEM); + + return currp->u.rdymsg; + } + + return MSG_OK; +} + +/** + * @brief Performs a wait operation on a semaphore with timeout specification. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return A message specifying how the invoking thread has been + * released from the semaphore. + * @retval MSG_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). + * @retval MSG_TIMEOUT if the semaphore has not been signaled or reset within + * the specified timeout. + * + * @api + */ +msg_t chSemWaitTimeout(semaphore_t *sp, sysinterval_t timeout) { + msg_t msg; + + chSysLock(); + msg = chSemWaitTimeoutS(sp, timeout); + chSysUnlock(); + + return msg; +} + +/** + * @brief Performs a wait operation on a semaphore with timeout specification. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return A message specifying how the invoking thread has been + * released from the semaphore. + * @retval MSG_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). + * @retval MSG_TIMEOUT if the semaphore has not been signaled or reset within + * the specified timeout. + * + * @sclass + */ +msg_t chSemWaitTimeoutS(semaphore_t *sp, sysinterval_t timeout) { + + chDbgCheckClassS(); + chDbgCheck(sp != NULL); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + + if (--sp->cnt < (cnt_t)0) { + if (TIME_IMMEDIATE == timeout) { + sp->cnt++; + + return MSG_TIMEOUT; + } + currp->u.wtsemp = sp; + sem_insert(currp, &sp->queue); + + return chSchGoSleepTimeoutS(CH_STATE_WTSEM, timeout); + } + + return MSG_OK; +} + +/** + * @brief Performs a signal operation on a semaphore. + * + * @param[in] sp pointer to a @p semaphore_t structure + * + * @api + */ +void chSemSignal(semaphore_t *sp) { + + chDbgCheck(sp != NULL); + + chSysLock(); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + if (++sp->cnt <= (cnt_t)0) { + chSchWakeupS(queue_fifo_remove(&sp->queue), MSG_OK); + } + chSysUnlock(); +} + +/** + * @brief Performs a signal operation on a semaphore. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] sp pointer to a @p semaphore_t structure + * + * @iclass + */ +void chSemSignalI(semaphore_t *sp) { + + chDbgCheckClassI(); + chDbgCheck(sp != NULL); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + + if (++sp->cnt <= (cnt_t)0) { + /* Note, it is done this way in order to allow a tail call on + chSchReadyI().*/ + thread_t *tp = queue_fifo_remove(&sp->queue); + tp->u.rdymsg = MSG_OK; + (void) chSchReadyI(tp); + } +} + +/** + * @brief Adds the specified value to the semaphore counter. + * @post This function does not reschedule so a call to a rescheduling + * function must be performed before unlocking the kernel. Note that + * interrupt handlers always reschedule on exit so an explicit + * reschedule must not be performed in ISRs. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] n value to be added to the semaphore counter. The value + * must be positive. + * + * @iclass + */ +void chSemAddCounterI(semaphore_t *sp, cnt_t n) { + + chDbgCheckClassI(); + chDbgCheck((sp != NULL) && (n > (cnt_t)0)); + chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) || + ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)), + "inconsistent semaphore"); + + while (n > (cnt_t)0) { + if (++sp->cnt <= (cnt_t)0) { + chSchReadyI(queue_fifo_remove(&sp->queue))->u.rdymsg = MSG_OK; + } + n--; + } +} + +/** + * @brief Performs atomic signal and wait operations on two semaphores. + * + * @param[in] sps pointer to a @p semaphore_t structure to be signaled + * @param[in] spw pointer to a @p semaphore_t structure to wait on + * @return A message specifying how the invoking thread has been + * released from the semaphore. + * @retval MSG_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval MSG_RESET if the semaphore has been reset using @p chSemReset(). + * + * @api + */ +msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) { + msg_t msg; + + chDbgCheck((sps != NULL) && (spw != NULL)); + + chSysLock(); + chDbgAssert(((sps->cnt >= (cnt_t)0) && queue_isempty(&sps->queue)) || + ((sps->cnt < (cnt_t)0) && queue_notempty(&sps->queue)), + "inconsistent semaphore"); + chDbgAssert(((spw->cnt >= (cnt_t)0) && queue_isempty(&spw->queue)) || + ((spw->cnt < (cnt_t)0) && queue_notempty(&spw->queue)), + "inconsistent semaphore"); + if (++sps->cnt <= (cnt_t)0) { + chSchReadyI(queue_fifo_remove(&sps->queue))->u.rdymsg = MSG_OK; + } + if (--spw->cnt < (cnt_t)0) { + thread_t *ctp = currp; + sem_insert(ctp, &spw->queue); + ctp->u.wtsemp = spw; + chSchGoSleepS(CH_STATE_WTSEM); + msg = ctp->u.rdymsg; + } + else { + chSchRescheduleS(); + msg = MSG_OK; + } + chSysUnlock(); + + return msg; +} + +#endif /* CH_CFG_USE_SEMAPHORES == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chstats.c b/ChibiOS_20.3.2/os/rt/src/chstats.c new file mode 100644 index 0000000..5d3c0fc --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chstats.c @@ -0,0 +1,126 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chstats.c + * @brief Statistics module code. + * + * @addtogroup statistics + * @details Statistics services. + * @{ + */ + +#include "ch.h" + +#if (CH_DBG_STATISTICS == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes the statistics module. + * + * @init + */ +void _stats_init(void) { + + ch.kernel_stats.n_irq = (ucnt_t)0; + ch.kernel_stats.n_ctxswc = (ucnt_t)0; + chTMObjectInit(&ch.kernel_stats.m_crit_thd); + chTMObjectInit(&ch.kernel_stats.m_crit_isr); +} + +/** + * @brief Increases the IRQ counter. + */ +void _stats_increase_irq(void) { + + port_lock_from_isr(); + ch.kernel_stats.n_irq++; + port_unlock_from_isr(); +} + +/** + * @brief Updates context switch related statistics. + * + * @param[in] ntp the thread to be switched in + * @param[in] otp the thread to be switched out + */ +void _stats_ctxswc(thread_t *ntp, thread_t *otp) { + + ch.kernel_stats.n_ctxswc++; + chTMChainMeasurementToX(&otp->stats, &ntp->stats); +} + +/** + * @brief Starts the measurement of a thread critical zone. + */ +void _stats_start_measure_crit_thd(void) { + + chTMStartMeasurementX(&ch.kernel_stats.m_crit_thd); +} + +/** + * @brief Stops the measurement of a thread critical zone. + */ +void _stats_stop_measure_crit_thd(void) { + + chTMStopMeasurementX(&ch.kernel_stats.m_crit_thd); +} + +/** + * @brief Starts the measurement of an ISR critical zone. + */ +void _stats_start_measure_crit_isr(void) { + + chTMStartMeasurementX(&ch.kernel_stats.m_crit_isr); +} + +/** + * @brief Stops the measurement of an ISR critical zone. + */ +void _stats_stop_measure_crit_isr(void) { + + chTMStopMeasurementX(&ch.kernel_stats.m_crit_isr); +} + +#endif /* CH_DBG_STATISTICS == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chsys.c b/ChibiOS_20.3.2/os/rt/src/chsys.c new file mode 100644 index 0000000..cfe9321 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chsys.c @@ -0,0 +1,445 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chsys.c + * @brief System related code. + * + * @addtogroup system + * @details System related APIs and services: + * - Initialization. + * - Locks. + * - Interrupt Handling. + * - Power Management. + * - Abnormal Termination. + * - Realtime counter. + * . + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +#if (CH_CFG_NO_IDLE_THREAD == FALSE) || defined(__DOXYGEN__) +/** + * @brief Idle thread working area. + */ +THD_WORKING_AREA(ch_idle_thread_wa, PORT_IDLE_THREAD_STACK_SIZE); +#endif + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if (CH_CFG_NO_IDLE_THREAD == FALSE) || defined(__DOXYGEN__) +/** + * @brief This function implements the idle thread infinite loop. + * @details The function puts the processor in the lowest power mode capable + * to serve interrupts.
+ * The priority is internally set to the minimum system value so + * that this thread is executed only if there are no other ready + * threads in the system. + * + * @param[in] p the thread parameter, unused in this scenario + */ +static void _idle_thread(void *p) { + + (void)p; + + while (true) { + /*lint -save -e522 [2.2] Apparently no side effects because it contains + an asm instruction.*/ + port_wait_for_interrupt(); + /*lint -restore*/ + CH_CFG_IDLE_LOOP_HOOK(); + } +} +#endif /* CH_CFG_NO_IDLE_THREAD == FALSE */ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief ChibiOS/RT initialization. + * @details After executing this function the current instructions stream + * becomes the main thread. + * @pre Interrupts must disabled before invoking this function. + * @post The main thread is created with priority @p NORMALPRIO and + * interrupts are enabled. + * + * @special + */ +void chSysInit(void) { + + _scheduler_init(); + _vt_init(); + _trace_init(); + _oslib_init(); + +#if CH_DBG_SYSTEM_STATE_CHECK == TRUE + ch.dbg.isr_cnt = (cnt_t)0; + ch.dbg.lock_cnt = (cnt_t)0; +#endif +#if CH_CFG_USE_TM == TRUE + _tm_init(); +#endif +#if CH_DBG_STATISTICS == TRUE + _stats_init(); +#endif + +#if CH_CFG_NO_IDLE_THREAD == FALSE + /* Now this instructions flow becomes the main thread.*/ +#if CH_CFG_USE_REGISTRY == TRUE + currp = _thread_init(&ch.mainthread, (const char *)&ch_debug, NORMALPRIO); +#else + currp = _thread_init(&ch.mainthread, "main", NORMALPRIO); +#endif +#else + /* Now this instructions flow becomes the idle thread.*/ + currp = _thread_init(&ch.mainthread, "idle", IDLEPRIO); +#endif + +#if CH_DBG_ENABLE_STACK_CHECK == TRUE + { + /* Setting up the base address of the static main thread stack, the + symbol must be provided externally.*/ + extern stkalign_t __main_thread_stack_base__; + currp->wabase = &__main_thread_stack_base__; + } +#elif CH_CFG_USE_DYNAMIC == TRUE + currp->wabase = NULL; +#endif + + /* Setting up the caller as current thread.*/ + currp->state = CH_STATE_CURRENT; + + /* Port layer initialization last because it depend on some of the + initializations performed before.*/ + port_init(); + +#if CH_DBG_STATISTICS == TRUE + /* Starting measurement for this thread.*/ + chTMStartMeasurementX(&currp->stats); +#endif + + /* Initialization hook.*/ + CH_CFG_SYSTEM_INIT_HOOK(); + + /* It is alive now.*/ + chSysEnable(); + +#if CH_CFG_NO_IDLE_THREAD == FALSE + { + static const thread_descriptor_t idle_descriptor = { + "idle", + THD_WORKING_AREA_BASE(ch_idle_thread_wa), + THD_WORKING_AREA_END(ch_idle_thread_wa), + IDLEPRIO, + _idle_thread, + NULL + }; + + /* This thread has the lowest priority in the system, its role is just to + serve interrupts in its context while keeping the lowest energy saving + mode compatible with the system status.*/ + (void) chThdCreate(&idle_descriptor); + } +#endif +} + +/** + * @brief Halts the system. + * @details This function is invoked by the operating system when an + * unrecoverable error is detected, for example because a programming + * error in the application code that triggers an assertion while + * in debug mode. + * @note Can be invoked from any system state. + * + * @param[in] reason pointer to an error string + * + * @special + */ +void chSysHalt(const char *reason) { + + port_disable(); + + /* Logging the event.*/ + _trace_halt(reason); + + /* Pointing to the passed message.*/ + ch.dbg.panic_msg = reason; + + /* Halt hook code, usually empty.*/ + CH_CFG_SYSTEM_HALT_HOOK(reason); + + /* Harmless infinite loop.*/ + while (true) { + } +} + +/** + * @brief System integrity check. + * @details Performs an integrity check of the important ChibiOS/RT data + * structures. + * @note The appropriate action in case of failure is to halt the system + * before releasing the critical zone. + * @note If the system is corrupted then one possible outcome of this + * function is an exception caused by @p NULL or corrupted pointers + * in list elements. Exception vectors must be monitored as well. + * @note This function is not used internally, it is up to the + * application to define if and where to perform system + * checking. + * @note Performing all tests at once can be a slow operation and can + * degrade the system response time. It is suggested to execute + * one test at time and release the critical zone in between tests. + * + * @param[in] testmask Each bit in this mask is associated to a test to be + * performed. + * @return The test result. + * @retval false The test succeeded. + * @retval true Test failed. + * + * @iclass + */ +bool chSysIntegrityCheckI(unsigned testmask) { + cnt_t n; + + chDbgCheckClassI(); + + /* Ready List integrity check.*/ + if ((testmask & CH_INTEGRITY_RLIST) != 0U) { + thread_t *tp; + + /* Scanning the ready list forward.*/ + n = (cnt_t)0; + tp = ch.rlist.queue.next; + while (tp != (thread_t *)&ch.rlist.queue) { + n++; + tp = tp->queue.next; + } + + /* Scanning the ready list backward.*/ + tp = ch.rlist.queue.prev; + while (tp != (thread_t *)&ch.rlist.queue) { + n--; + tp = tp->queue.prev; + } + + /* The number of elements must match.*/ + if (n != (cnt_t)0) { + return true; + } + } + + /* Timers list integrity check.*/ + if ((testmask & CH_INTEGRITY_VTLIST) != 0U) { + virtual_timer_t * vtp; + + /* Scanning the timers list forward.*/ + n = (cnt_t)0; + vtp = ch.vtlist.next; + while (vtp != (virtual_timer_t *)&ch.vtlist) { + n++; + vtp = vtp->next; + } + + /* Scanning the timers list backward.*/ + vtp = ch.vtlist.prev; + while (vtp != (virtual_timer_t *)&ch.vtlist) { + n--; + vtp = vtp->prev; + } + + /* The number of elements must match.*/ + if (n != (cnt_t)0) { + return true; + } + } + +#if CH_CFG_USE_REGISTRY == TRUE + if ((testmask & CH_INTEGRITY_REGISTRY) != 0U) { + thread_t *tp; + + /* Scanning the ready list forward.*/ + n = (cnt_t)0; + tp = ch.rlist.newer; + while (tp != (thread_t *)&ch.rlist) { + n++; + tp = tp->newer; + } + + /* Scanning the ready list backward.*/ + tp = ch.rlist.older; + while (tp != (thread_t *)&ch.rlist) { + n--; + tp = tp->older; + } + + /* The number of elements must match.*/ + if (n != (cnt_t)0) { + return true; + } + } +#endif /* CH_CFG_USE_REGISTRY == TRUE */ + +#if defined(PORT_INTEGRITY_CHECK) + if ((testmask & CH_INTEGRITY_PORT) != 0U) { + PORT_INTEGRITY_CHECK(); + } +#endif + + return false; +} + +/** + * @brief Handles time ticks for round robin preemption and timer increments. + * @details Decrements the remaining time quantum of the running thread + * and preempts it when the quantum is used up. Increments system + * time and manages the timers. + * @note The frequency of the timer determines the system tick granularity + * and, together with the @p CH_CFG_TIME_QUANTUM macro, the round robin + * interval. + * + * @iclass + */ +void chSysTimerHandlerI(void) { + + chDbgCheckClassI(); + +#if CH_CFG_TIME_QUANTUM > 0 + /* Running thread has not used up quantum yet? */ + if (currp->ticks > (tslices_t)0) { + /* Decrement remaining quantum.*/ + currp->ticks--; + } +#endif +#if CH_DBG_THREADS_PROFILING == TRUE + currp->time++; +#endif + chVTDoTickI(); + CH_CFG_SYSTEM_TICK_HOOK(); +} + +/** + * @brief Returns the execution status and enters a critical zone. + * @details This functions enters into a critical zone and can be called + * from any context. Because its flexibility it is less efficient + * than @p chSysLock() which is preferable when the calling context + * is known. + * @post The system is in a critical zone. + * + * @return The previous system status, the encoding of this + * status word is architecture-dependent and opaque. + * + * @xclass + */ +syssts_t chSysGetStatusAndLockX(void) { + + syssts_t sts = port_get_irq_status(); + if (port_irq_enabled(sts)) { + if (port_is_isr_context()) { + chSysLockFromISR(); + } + else { + chSysLock(); + } + } + return sts; +} + +/** + * @brief Restores the specified execution status and leaves a critical zone. + * @note A call to @p chSchRescheduleS() is automatically performed + * if exiting the critical zone and if not in ISR context. + * + * @param[in] sts the system status to be restored. + * + * @xclass + */ +void chSysRestoreStatusX(syssts_t sts) { + + if (port_irq_enabled(sts)) { + if (port_is_isr_context()) { + chSysUnlockFromISR(); + } + else { + chSchRescheduleS(); + chSysUnlock(); + } + } +} + +#if (PORT_SUPPORTS_RT == TRUE) || defined(__DOXYGEN__) +/** + * @brief Realtime window test. + * @details This function verifies if the current realtime counter value + * lies within the specified range or not. The test takes care + * of the realtime counter wrapping to zero on overflow. + * @note When start==end then the function returns always false because a + * null time range is specified. + * @note This function is only available if the port layer supports the + * option @p PORT_SUPPORTS_RT. + * + * @param[in] cnt the counter value to be tested + * @param[in] start the start of the time window (inclusive) + * @param[in] end the end of the time window (non inclusive) + * @retval true current time within the specified time window. + * @retval false current time not within the specified time window. + * + * @xclass + */ +bool chSysIsCounterWithinX(rtcnt_t cnt, rtcnt_t start, rtcnt_t end) { + + return (bool)(((rtcnt_t)cnt - (rtcnt_t)start) < + ((rtcnt_t)end - (rtcnt_t)start)); +} + +/** + * @brief Polled delay. + * @note The real delay is always few cycles in excess of the specified + * value. + * @note This function is only available if the port layer supports the + * option @p PORT_SUPPORTS_RT. + * + * @param[in] cycles number of cycles + * + * @xclass + */ +void chSysPolledDelayX(rtcnt_t cycles) { + rtcnt_t start = chSysGetRealtimeCounterX(); + rtcnt_t end = start + cycles; + + while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end)) { + } +} +#endif /* PORT_SUPPORTS_RT == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chthreads.c b/ChibiOS_20.3.2/os/rt/src/chthreads.c new file mode 100644 index 0000000..caa846b --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chthreads.c @@ -0,0 +1,906 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chthreads.c + * @brief Threads code. + * + * @addtogroup threads + * @details Threads related APIs and services. + *

Operation mode

+ * A thread is an abstraction of an independent instructions flow. + * In ChibiOS/RT a thread is represented by a "C" function owning + * a processor context, state informations and a dedicated stack + * area. In this scenario static variables are shared among all + * threads while automatic variables are local to the thread.
+ * Operations defined for threads: + * - Create, a thread is started on the specified thread + * function. This operation is available in multiple variants, + * both static and dynamic. + * - Exit, a thread terminates by returning from its top + * level function or invoking a specific API, the thread can + * return a value that can be retrieved by other threads. + * - Wait, a thread waits for the termination of another + * thread and retrieves its return value. + * - Resume, a thread created in suspended state is started. + * - Sleep, the execution of a thread is suspended for the + * specified amount of time or the specified future absolute time + * is reached. + * - SetPriority, a thread changes its own priority level. + * - Yield, a thread voluntarily renounces to its time slot. + * . + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes a thread structure. + * @note This is an internal functions, do not use it in application code. + * + * @param[in] tp pointer to the thread + * @param[in] name thread name + * @param[in] prio the priority level for the new thread + * @return The same thread pointer passed as parameter. + * + * @notapi + */ +thread_t *_thread_init(thread_t *tp, const char *name, tprio_t prio) { + + tp->prio = prio; + tp->state = CH_STATE_WTSTART; + tp->flags = CH_FLAG_MODE_STATIC; +#if CH_CFG_TIME_QUANTUM > 0 + tp->ticks = (tslices_t)CH_CFG_TIME_QUANTUM; +#endif +#if CH_CFG_USE_MUTEXES == TRUE + tp->realprio = prio; + tp->mtxlist = NULL; +#endif +#if CH_CFG_USE_EVENTS == TRUE + tp->epending = (eventmask_t)0; +#endif +#if CH_DBG_THREADS_PROFILING == TRUE + tp->time = (systime_t)0; +#endif +#if CH_CFG_USE_REGISTRY == TRUE + tp->refs = (trefs_t)1; + tp->name = name; + REG_INSERT(tp); +#else + (void)name; +#endif +#if CH_CFG_USE_WAITEXIT == TRUE + list_init(&tp->waiting); +#endif +#if CH_CFG_USE_MESSAGES == TRUE + queue_init(&tp->msgqueue); +#endif +#if CH_DBG_STATISTICS == TRUE + chTMObjectInit(&tp->stats); +#endif + CH_CFG_THREAD_INIT_HOOK(tp); + return tp; +} + +#if (CH_DBG_FILL_THREADS == TRUE) || defined(__DOXYGEN__) +/** + * @brief Memory fill utility. + * + * @param[in] startp first address to fill + * @param[in] endp last address to fill +1 + * @param[in] v filler value + * + * @notapi + */ +void _thread_memfill(uint8_t *startp, uint8_t *endp, uint8_t v) { + + while (startp < endp) { + *startp++ = v; + } +} +#endif /* CH_DBG_FILL_THREADS */ + +/** + * @brief Creates a new thread into a static memory area. + * @details The new thread is initialized but not inserted in the ready list, + * the initial state is @p CH_STATE_WTSTART. + * @post The created thread has a reference counter set to one, it is + * caller responsibility to call @p chThdRelease() or @p chthdWait() + * in order to release the reference. The thread persists in the + * registry until its reference counter reaches zero. + * @post The initialized thread can be subsequently started by invoking + * @p chThdStart(), @p chThdStartI() or @p chSchWakeupS() + * depending on the execution context. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * @note Threads created using this function do not obey to the + * @p CH_DBG_FILL_THREADS debug option because it would keep + * the kernel locked for too much time. + * + * @param[out] tdp pointer to the thread descriptor + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @iclass + */ +thread_t *chThdCreateSuspendedI(const thread_descriptor_t *tdp) { + thread_t *tp; + + chDbgCheckClassI(); + chDbgCheck(tdp != NULL); + chDbgCheck(MEM_IS_ALIGNED(tdp->wbase, PORT_WORKING_AREA_ALIGN) && + MEM_IS_ALIGNED(tdp->wend, PORT_STACK_ALIGN) && + (tdp->wend > tdp->wbase) && + (((size_t)tdp->wend - (size_t)tdp->wbase) >= THD_WORKING_AREA_SIZE(0))); + chDbgCheck((tdp->prio <= HIGHPRIO) && (tdp->funcp != NULL)); + + /* The thread structure is laid out in the upper part of the thread + workspace. The thread position structure is aligned to the required + stack alignment because it represents the stack top.*/ + tp = (thread_t *)((uint8_t *)tdp->wend - + MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN)); + +#if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE) + /* Stack boundary.*/ + tp->wabase = tdp->wbase; +#endif + + /* Setting up the port-dependent part of the working area.*/ + PORT_SETUP_CONTEXT(tp, tdp->wbase, tp, tdp->funcp, tdp->arg); + + /* The driver object is initialized but not started.*/ + return _thread_init(tp, tdp->name, tdp->prio); +} + +/** + * @brief Creates a new thread into a static memory area. + * @details The new thread is initialized but not inserted in the ready list, + * the initial state is @p CH_STATE_WTSTART. + * @post The created thread has a reference counter set to one, it is + * caller responsibility to call @p chThdRelease() or @p chthdWait() + * in order to release the reference. The thread persists in the + * registry until its reference counter reaches zero. + * @post The initialized thread can be subsequently started by invoking + * @p chThdStart(), @p chThdStartI() or @p chSchWakeupS() + * depending on the execution context. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * + * @param[out] tdp pointer to the thread descriptor + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @api + */ +thread_t *chThdCreateSuspended(const thread_descriptor_t *tdp) { + thread_t *tp; + +#if CH_CFG_USE_REGISTRY == TRUE + chDbgAssert(chRegFindThreadByWorkingArea(tdp->wbase) == NULL, + "working area in use"); +#endif + +#if CH_DBG_FILL_THREADS == TRUE + _thread_memfill((uint8_t *)tdp->wbase, + (uint8_t *)tdp->wend, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateSuspendedI(tdp); + chSysUnlock(); + + return tp; +} + +/** + * @brief Creates a new thread into a static memory area. + * @details The new thread is initialized and make ready to execute. + * @post The created thread has a reference counter set to one, it is + * caller responsibility to call @p chThdRelease() or @p chthdWait() + * in order to release the reference. The thread persists in the + * registry until its reference counter reaches zero. + * @post The initialized thread can be subsequently started by invoking + * @p chThdStart(), @p chThdStartI() or @p chSchWakeupS() + * depending on the execution context. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * @note Threads created using this function do not obey to the + * @p CH_DBG_FILL_THREADS debug option because it would keep + * the kernel locked for too much time. + * + * @param[out] tdp pointer to the thread descriptor + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @iclass + */ +thread_t *chThdCreateI(const thread_descriptor_t *tdp) { + + return chSchReadyI(chThdCreateSuspendedI(tdp)); +} + +/** + * @brief Creates a new thread into a static memory area. + * @details The new thread is initialized and make ready to execute. + * @post The created thread has a reference counter set to one, it is + * caller responsibility to call @p chThdRelease() or @p chthdWait() + * in order to release the reference. The thread persists in the + * registry until its reference counter reaches zero. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * + * @param[out] tdp pointer to the thread descriptor + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @iclass + */ +thread_t *chThdCreate(const thread_descriptor_t *tdp) { + thread_t *tp; + +#if (CH_CFG_USE_REGISTRY == TRUE) && \ + ((CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE)) + chDbgAssert(chRegFindThreadByWorkingArea(tdp->wbase) == NULL, + "working area in use"); +#endif + +#if CH_DBG_FILL_THREADS == TRUE + _thread_memfill((uint8_t *)tdp->wbase, + (uint8_t *)tdp->wend, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateSuspendedI(tdp); + chSchWakeupS(tp, MSG_OK); + chSysUnlock(); + + return tp; +} + +/** + * @brief Creates a new thread into a static memory area. + * @post The created thread has a reference counter set to one, it is + * caller responsibility to call @p chThdRelease() or @p chThdWait() + * in order to release the reference. The thread persists in the + * registry until its reference counter reaches zero. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * + * @param[out] wsp pointer to a working area dedicated to the thread stack + * @param[in] size size of the working area + * @param[in] prio the priority level for the new thread + * @param[in] pf the thread function + * @param[in] arg an argument passed to the thread function. It can be + * @p NULL. + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @api + */ +thread_t *chThdCreateStatic(void *wsp, size_t size, + tprio_t prio, tfunc_t pf, void *arg) { + thread_t *tp; + + chDbgCheck((wsp != NULL) && + MEM_IS_ALIGNED(wsp, PORT_WORKING_AREA_ALIGN) && + (size >= THD_WORKING_AREA_SIZE(0)) && + MEM_IS_ALIGNED(size, PORT_STACK_ALIGN) && + (prio <= HIGHPRIO) && (pf != NULL)); + +#if (CH_CFG_USE_REGISTRY == TRUE) && \ + ((CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE)) + chDbgAssert(chRegFindThreadByWorkingArea(wsp) == NULL, + "working area in use"); +#endif + +#if CH_DBG_FILL_THREADS == TRUE + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + size, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + + /* The thread structure is laid out in the upper part of the thread + workspace. The thread position structure is aligned to the required + stack alignment because it represents the stack top.*/ + tp = (thread_t *)((uint8_t *)wsp + size - + MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN)); + +#if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE) + /* Stack boundary.*/ + tp->wabase = (stkalign_t *)wsp; +#endif + + /* Setting up the port-dependent part of the working area.*/ + PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg); + + tp = _thread_init(tp, "noname", prio); + + /* Starting the thread immediately.*/ + chSchWakeupS(tp, MSG_OK); + chSysUnlock(); + + return tp; +} + +/** + * @brief Resumes a thread created with @p chThdCreateI(). + * + * @param[in] tp pointer to the thread + * @return The pointer to the @p thread_t structure allocated for + * the thread into the working space area. + * + * @api + */ +thread_t *chThdStart(thread_t *tp) { + + chSysLock(); + chDbgAssert(tp->state == CH_STATE_WTSTART, "wrong state"); + chSchWakeupS(tp, MSG_OK); + chSysUnlock(); + + return tp; +} + +#if (CH_CFG_USE_REGISTRY == TRUE) || defined(__DOXYGEN__) +/** + * @brief Adds a reference to a thread object. + * @pre The configuration option @p CH_CFG_USE_REGISTRY must be enabled in + * order to use this function. + * + * @param[in] tp pointer to the thread + * @return The same thread pointer passed as parameter + * representing the new reference. + * + * @api + */ +thread_t *chThdAddRef(thread_t *tp) { + + chSysLock(); + chDbgAssert(tp->refs < (trefs_t)255, "too many references"); + tp->refs++; + chSysUnlock(); + + return tp; +} + +/** + * @brief Releases a reference to a thread object. + * @details If the references counter reaches zero and the thread + * is in the @p CH_STATE_FINAL state then the thread's memory is + * returned to the proper allocator and the thread is removed + * from the registry.
+ * Threads whose counter reaches zero and are still active become + * "detached" and will be removed from registry on termination. + * @pre The configuration option @p CH_CFG_USE_REGISTRY must be enabled in + * order to use this function. + * @note Static threads are not affected. + * + * @param[in] tp pointer to the thread + * + * @api + */ +void chThdRelease(thread_t *tp) { + + chSysLock(); + chDbgAssert(tp->refs > (trefs_t)0, "not referenced"); + tp->refs--; + + /* If the references counter reaches zero and the thread is in its + terminated state then the memory can be returned to the proper + allocator.*/ + if ((tp->refs == (trefs_t)0) && (tp->state == CH_STATE_FINAL)) { + REG_REMOVE(tp); + chSysUnlock(); + +#if CH_CFG_USE_DYNAMIC == TRUE + switch (tp->flags & CH_FLAG_MODE_MASK) { +#if CH_CFG_USE_HEAP == TRUE + case CH_FLAG_MODE_HEAP: + chHeapFree(chThdGetWorkingAreaX(tp)); + break; +#endif +#if CH_CFG_USE_MEMPOOLS == TRUE + case CH_FLAG_MODE_MPOOL: + chPoolFree(tp->mpool, chThdGetWorkingAreaX(tp)); + break; +#endif + default: + /* Nothing else to do for static threads.*/ + break; + } +#endif /* CH_CFG_USE_DYNAMIC == TRUE */ + return; + } + chSysUnlock(); +} +#endif /* CH_CFG_USE_REGISTRY == TRUE */ + +/** + * @brief Terminates the current thread. + * @details The thread goes in the @p CH_STATE_FINAL state holding the + * specified exit status code, other threads can retrieve the + * exit status code by invoking the function @p chThdWait(). + * @post Eventual code after this function will never be executed, + * this function never returns. The compiler has no way to + * know this so do not assume that the compiler would remove + * the dead code. + * + * @param[in] msg thread exit code + * + * @api + */ +void chThdExit(msg_t msg) { + + chSysLock(); + chThdExitS(msg); + /* The thread never returns here.*/ +} + +/** + * @brief Terminates the current thread. + * @details The thread goes in the @p CH_STATE_FINAL state holding the + * specified exit status code, other threads can retrieve the + * exit status code by invoking the function @p chThdWait(). + * @post Exiting a non-static thread that does not have references + * (detached) causes the thread to remain in the registry. + * It can only be removed by performing a registry scan operation. + * @post Eventual code after this function will never be executed, + * this function never returns. The compiler has no way to + * know this so do not assume that the compiler would remove + * the dead code. + * + * @param[in] msg thread exit code + * + * @sclass + */ +void chThdExitS(msg_t msg) { + thread_t *tp = currp; + + /* Storing exit message.*/ + tp->u.exitcode = msg; + + /* Exit handler hook.*/ + CH_CFG_THREAD_EXIT_HOOK(tp); + +#if CH_CFG_USE_WAITEXIT == TRUE + /* Waking up any waiting thread.*/ + while (list_notempty(&tp->waiting)) { + (void) chSchReadyI(list_remove(&tp->waiting)); + } +#endif + +#if CH_CFG_USE_REGISTRY == TRUE + /* Static threads with no references are immediately removed from the + registry because there is no memory to recover.*/ +#if CH_CFG_USE_DYNAMIC == TRUE + if ((tp->refs == (trefs_t)0) && + ((tp->flags & CH_FLAG_MODE_MASK) == CH_FLAG_MODE_STATIC)) { + REG_REMOVE(tp); + } +#else + if (tp->refs == (trefs_t)0) { + REG_REMOVE(tp); + } +#endif +#endif + + /* Going into final state.*/ + chSchGoSleepS(CH_STATE_FINAL); + + /* The thread never returns here.*/ + chDbgAssert(false, "zombies apocalypse"); +} + +#if (CH_CFG_USE_WAITEXIT == TRUE) || defined(__DOXYGEN__) +/** + * @brief Blocks the execution of the invoking thread until the specified + * thread terminates then the exit code is returned. + * @details This function waits for the specified thread to terminate then + * decrements its reference counter, if the counter reaches zero then + * the thread working area is returned to the proper allocator and + * the thread is removed from registry. + * @pre The configuration option @p CH_CFG_USE_WAITEXIT must be enabled in + * order to use this function. + * @post Enabling @p chThdWait() requires 2-4 (depending on the + * architecture) extra bytes in the @p thread_t structure. + * @note If @p CH_CFG_USE_DYNAMIC is not specified this function just waits + * for the thread termination, no memory allocators are involved. + * + * @param[in] tp pointer to the thread + * @return The exit code from the terminated thread. + * + * @api + */ +msg_t chThdWait(thread_t *tp) { + msg_t msg; + + chDbgCheck(tp != NULL); + + chSysLock(); + chDbgAssert(tp != currp, "waiting self"); +#if CH_CFG_USE_REGISTRY == TRUE + chDbgAssert(tp->refs > (trefs_t)0, "no references"); +#endif + + if (tp->state != CH_STATE_FINAL) { + list_insert(currp, &tp->waiting); + chSchGoSleepS(CH_STATE_WTEXIT); + } + msg = tp->u.exitcode; + chSysUnlock(); + +#if CH_CFG_USE_REGISTRY == TRUE + /* Releasing a reference to the thread.*/ + chThdRelease(tp); +#endif + + return msg; +} +#endif /* CH_CFG_USE_WAITEXIT */ + +/** + * @brief Changes the running thread priority level then reschedules if + * necessary. + * @note The function returns the real thread priority regardless of the + * current priority that could be higher than the real priority + * because the priority inheritance mechanism. + * + * @param[in] newprio the new priority level of the running thread + * @return The old priority level. + * + * @api + */ +tprio_t chThdSetPriority(tprio_t newprio) { + tprio_t oldprio; + + chDbgCheck(newprio <= HIGHPRIO); + + chSysLock(); +#if CH_CFG_USE_MUTEXES == TRUE + oldprio = currp->realprio; + if ((currp->prio == currp->realprio) || (newprio > currp->prio)) { + currp->prio = newprio; + } + currp->realprio = newprio; +#else + oldprio = currp->prio; + currp->prio = newprio; +#endif + chSchRescheduleS(); + chSysUnlock(); + + return oldprio; +} + +/** + * @brief Requests a thread termination. + * @pre The target thread must be written to invoke periodically + * @p chThdShouldTerminate() and terminate cleanly if it returns + * @p true. + * @post The specified thread will terminate after detecting the termination + * condition. + * + * @param[in] tp pointer to the thread + * + * @api + */ +void chThdTerminate(thread_t *tp) { + + chSysLock(); + tp->flags |= CH_FLAG_TERMINATE; + chSysUnlock(); +} + +/** + * @brief Suspends the invoking thread for the specified time. + * + * @param[in] time the delay in system ticks, the special values are + * handled as follow: + * - @a TIME_INFINITE the thread enters an infinite sleep + * state. + * - @a TIME_IMMEDIATE this value is not allowed. + * . + * + * @api + */ +void chThdSleep(sysinterval_t time) { + + chSysLock(); + chThdSleepS(time); + chSysUnlock(); +} + +/** + * @brief Suspends the invoking thread until the system time arrives to the + * specified value. + * @note The function has no concept of "past", all specifiable times + * are in the future, this means that if you call this function + * exceeding your calculated intervals then the function will + * return in a far future time, not immediately. + * @see chThdSleepUntilWindowed() + * + * @param[in] time absolute system time + * + * @api + */ +void chThdSleepUntil(systime_t time) { + sysinterval_t interval; + + chSysLock(); + interval = chTimeDiffX(chVTGetSystemTimeX(), time); + if (interval > (sysinterval_t)0) { + chThdSleepS(interval); + } + chSysUnlock(); +} + +/** + * @brief Suspends the invoking thread until the system time arrives to the + * specified value. + * @note The system time is assumed to be between @p prev and @p next + * else the call is assumed to have been called outside the + * allowed time interval, in this case no sleep is performed. + * @see chThdSleepUntil() + * + * @param[in] prev absolute system time of the previous deadline + * @param[in] next absolute system time of the next deadline + * @return the @p next parameter + * + * @api + */ +systime_t chThdSleepUntilWindowed(systime_t prev, systime_t next) { + systime_t time; + + chSysLock(); + time = chVTGetSystemTimeX(); + if (chTimeIsInRangeX(time, prev, next)) { + chThdSleepS(chTimeDiffX(time, next)); + } + chSysUnlock(); + + return next; +} + +/** + * @brief Yields the time slot. + * @details Yields the CPU control to the next thread in the ready list with + * equal priority, if any. + * + * @api + */ +void chThdYield(void) { + + chSysLock(); + chSchDoYieldS(); + chSysUnlock(); +} + +/** + * @brief Sends the current thread sleeping and sets a reference variable. + * @note This function must reschedule, it can only be called from thread + * context. + * + * @param[in] trp a pointer to a thread reference object + * @return The wake up message. + * + * @sclass + */ +msg_t chThdSuspendS(thread_reference_t *trp) { + thread_t *tp = chThdGetSelfX(); + + chDbgAssert(*trp == NULL, "not NULL"); + + *trp = tp; + tp->u.wttrp = trp; + chSchGoSleepS(CH_STATE_SUSPENDED); + + return chThdGetSelfX()->u.rdymsg; +} + +/** + * @brief Sends the current thread sleeping and sets a reference variable. + * @note This function must reschedule, it can only be called from thread + * context. + * + * @param[in] trp a pointer to a thread reference object + * @param[in] timeout the timeout in system ticks, the special values are + * handled as follow: + * - @a TIME_INFINITE the thread enters an infinite sleep + * state. + * - @a TIME_IMMEDIATE the thread is not enqueued and + * the function returns @p MSG_TIMEOUT as if a timeout + * occurred. + * . + * @return The wake up message. + * @retval MSG_TIMEOUT if the operation timed out. + * + * @sclass + */ +msg_t chThdSuspendTimeoutS(thread_reference_t *trp, sysinterval_t timeout) { + thread_t *tp = chThdGetSelfX(); + + chDbgAssert(*trp == NULL, "not NULL"); + + if (TIME_IMMEDIATE == timeout) { + return MSG_TIMEOUT; + } + + *trp = tp; + tp->u.wttrp = trp; + + return chSchGoSleepTimeoutS(CH_STATE_SUSPENDED, timeout); +} + +/** + * @brief Wakes up a thread waiting on a thread reference object. + * @note This function must not reschedule because it can be called from + * ISR context. + * + * @param[in] trp a pointer to a thread reference object + * @param[in] msg the message code + * + * @iclass + */ +void chThdResumeI(thread_reference_t *trp, msg_t msg) { + + if (*trp != NULL) { + thread_t *tp = *trp; + + chDbgAssert(tp->state == CH_STATE_SUSPENDED, "not CH_STATE_SUSPENDED"); + + *trp = NULL; + tp->u.rdymsg = msg; + (void) chSchReadyI(tp); + } +} + +/** + * @brief Wakes up a thread waiting on a thread reference object. + * @note This function must reschedule, it can only be called from thread + * context. + * + * @param[in] trp a pointer to a thread reference object + * @param[in] msg the message code + * + * @iclass + */ +void chThdResumeS(thread_reference_t *trp, msg_t msg) { + + if (*trp != NULL) { + thread_t *tp = *trp; + + chDbgAssert(tp->state == CH_STATE_SUSPENDED, "not CH_STATE_SUSPENDED"); + + *trp = NULL; + chSchWakeupS(tp, msg); + } +} + +/** + * @brief Wakes up a thread waiting on a thread reference object. + * @note This function must reschedule, it can only be called from thread + * context. + * + * @param[in] trp a pointer to a thread reference object + * @param[in] msg the message code + * + * @api + */ +void chThdResume(thread_reference_t *trp, msg_t msg) { + + chSysLock(); + chThdResumeS(trp, msg); + chSysUnlock(); +} + +/** + * @brief Enqueues the caller thread on a threads queue object. + * @details The caller thread is enqueued and put to sleep until it is + * dequeued or the specified timeouts expires. + * + * @param[in] tqp pointer to the threads queue object + * @param[in] timeout the timeout in system ticks, the special values are + * handled as follow: + * - @a TIME_INFINITE the thread enters an infinite sleep + * state. + * - @a TIME_IMMEDIATE the thread is not enqueued and + * the function returns @p MSG_TIMEOUT as if a timeout + * occurred. + * . + * @return The message from @p osalQueueWakeupOneI() or + * @p osalQueueWakeupAllI() functions. + * @retval MSG_TIMEOUT if the thread has not been dequeued within the + * specified timeout or if the function has been + * invoked with @p TIME_IMMEDIATE as timeout + * specification. + * + * @sclass + */ +msg_t chThdEnqueueTimeoutS(threads_queue_t *tqp, sysinterval_t timeout) { + + if (TIME_IMMEDIATE == timeout) { + return MSG_TIMEOUT; + } + + queue_insert(currp, tqp); + + return chSchGoSleepTimeoutS(CH_STATE_QUEUED, timeout); +} + +/** + * @brief Dequeues and wakes up one thread from the threads queue object, + * if any. + * + * @param[in] tqp pointer to the threads queue object + * @param[in] msg the message code + * + * @iclass + */ +void chThdDequeueNextI(threads_queue_t *tqp, msg_t msg) { + + if (queue_notempty(tqp)) { + chThdDoDequeueNextI(tqp, msg); + } +} + +/** + * @brief Dequeues and wakes up all threads from the threads queue object. + * + * @param[in] tqp pointer to the threads queue object + * @param[in] msg the message code + * + * @iclass + */ +void chThdDequeueAllI(threads_queue_t *tqp, msg_t msg) { + + while (queue_notempty(tqp)) { + chThdDoDequeueNextI(tqp, msg); + } +} + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chtm.c b/ChibiOS_20.3.2/os/rt/src/chtm.c new file mode 100644 index 0000000..5b3758c --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chtm.c @@ -0,0 +1,168 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chtm.c + * @brief Time Measurement module code. + * + * @addtogroup time_measurement + * @details Time Measurement APIs and services. + * @{ + */ + +#include "ch.h" + +#if (CH_CFG_USE_TM == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/** + * @brief Number of iterations in the calibration loop. + * @note This is required in order to assess the best result in + * architectures with instruction cache. + */ +#define TM_CALIBRATION_LOOP 4U + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +static inline void tm_stop(time_measurement_t *tmp, + rtcnt_t now, + rtcnt_t offset) { + + tmp->n++; + tmp->last = (now - tmp->last) - offset; + tmp->cumulative += (rttime_t)tmp->last; + if (tmp->last > tmp->worst) { + tmp->worst = tmp->last; + } + if (tmp->last < tmp->best) { + tmp->best = tmp->last; + } +} + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes the time measurement unit. + * + * @init + */ +void _tm_init(void) { + time_measurement_t tm; + unsigned i; + + /* Time Measurement subsystem calibration, it does a null measurement + and calculates the call overhead which is subtracted to real + measurements.*/ + ch.tm.offset = (rtcnt_t)0; + chTMObjectInit(&tm); + i = TM_CALIBRATION_LOOP; + do { + chTMStartMeasurementX(&tm); + chTMStopMeasurementX(&tm); + i--; + } while (i > 0U); + ch.tm.offset = tm.best; +} + +/** + * @brief Initializes a @p TimeMeasurement object. + * + * @param[out] tmp pointer to a @p TimeMeasurement structure + * + * @init + */ +void chTMObjectInit(time_measurement_t *tmp) { + + tmp->best = (rtcnt_t)-1; + tmp->worst = (rtcnt_t)0; + tmp->last = (rtcnt_t)0; + tmp->n = (ucnt_t)0; + tmp->cumulative = (rttime_t)0; +} + +/** + * @brief Starts a measurement. + * @pre The @p time_measurement_t structure must be initialized. + * + * @param[in,out] tmp pointer to a @p TimeMeasurement structure + * + * @xclass + */ +NOINLINE void chTMStartMeasurementX(time_measurement_t *tmp) { + + tmp->last = chSysGetRealtimeCounterX(); +} + +/** + * @brief Stops a measurement. + * @pre The @p time_measurement_t structure must be initialized. + * + * @param[in,out] tmp pointer to a @p time_measurement_t structure + * + * @xclass + */ +NOINLINE void chTMStopMeasurementX(time_measurement_t *tmp) { + + tm_stop(tmp, chSysGetRealtimeCounterX(), ch.tm.offset); +} + +/** + * @brief Stops a measurement and chains to the next one using the same time + * stamp. + * + * @param[in,out] tmp1 pointer to the @p time_measurement_t structure to be + * stopped + * @param[in,out] tmp2 pointer to the @p time_measurement_t structure to be + * started + * + * + * @xclass + */ +NOINLINE void chTMChainMeasurementToX(time_measurement_t *tmp1, + time_measurement_t *tmp2) { + + /* Starts new measurement.*/ + tmp2->last = chSysGetRealtimeCounterX(); + + /* Stops previous measurement using the same time stamp.*/ + tm_stop(tmp1, tmp2->last, (rtcnt_t)0); +} + +#endif /* CH_CFG_USE_TM == TRUE */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chtrace.c b/ChibiOS_20.3.2/os/rt/src/chtrace.c new file mode 100644 index 0000000..ecc2367 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chtrace.c @@ -0,0 +1,265 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chtrace.c + * @brief Tracer code. + * + * @addtogroup trace + * @details System events tracing service. + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if (CH_DBG_TRACE_MASK != CH_DBG_TRACE_MASK_DISABLED) || defined(__DOXYGEN__) +/** + * @brief Writes a time stamp and increases the trace buffer pointer. + * + * @notapi + */ +NOINLINE static void trace_next(void) { + + ch.dbg.trace_buffer.ptr->time = chVTGetSystemTimeX(); +#if PORT_SUPPORTS_RT == TRUE + ch.dbg.trace_buffer.ptr->rtstamp = chSysGetRealtimeCounterX(); +#else + ch.dbg.trace_buffer.ptr->rtstamp = (rtcnt_t)0; +#endif + + /* Trace hook, useful in order to interface debug tools.*/ + CH_CFG_TRACE_HOOK(ch.dbg.trace_buffer.ptr); + + if (++ch.dbg.trace_buffer.ptr >= + &ch.dbg.trace_buffer.buffer[CH_DBG_TRACE_BUFFER_SIZE]) { + ch.dbg.trace_buffer.ptr = &ch.dbg.trace_buffer.buffer[0]; + } +} +#endif + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +#if (CH_DBG_TRACE_MASK != CH_DBG_TRACE_MASK_DISABLED) || defined(__DOXYGEN__) +/** + * @brief Trace circular buffer subsystem initialization. + * @note Internal use only. + */ +void _trace_init(void) { + unsigned i; + + ch.dbg.trace_buffer.suspended = (uint16_t)~CH_DBG_TRACE_MASK; + ch.dbg.trace_buffer.size = CH_DBG_TRACE_BUFFER_SIZE; + ch.dbg.trace_buffer.ptr = &ch.dbg.trace_buffer.buffer[0]; + for (i = 0U; i < (unsigned)CH_DBG_TRACE_BUFFER_SIZE; i++) { + ch.dbg.trace_buffer.buffer[i].type = CH_TRACE_TYPE_UNUSED; + } +} + +/** + * @brief Inserts in the circular debug trace buffer a context switch record. + * + * @param[in] ntp the thread being switched in + * @param[in] otp the thread being switched out + * + * @notapi + */ +void _trace_switch(thread_t *ntp, thread_t *otp) { + + (void)ntp; + + if ((ch.dbg.trace_buffer.suspended & CH_DBG_TRACE_MASK_SWITCH) == 0U) { + ch.dbg.trace_buffer.ptr->type = CH_TRACE_TYPE_SWITCH; + ch.dbg.trace_buffer.ptr->state = (uint8_t)otp->state; + ch.dbg.trace_buffer.ptr->u.sw.ntp = currp; + ch.dbg.trace_buffer.ptr->u.sw.wtobjp = otp->u.wtobjp; + trace_next(); + } +} + +/** + * @brief Inserts in the circular debug trace buffer an ISR-enter record. + * + * @param[in] isr name of the isr + * + * @notapi + */ +void _trace_isr_enter(const char *isr) { + + if ((ch.dbg.trace_buffer.suspended & CH_DBG_TRACE_MASK_ISR) == 0U) { + port_lock_from_isr(); + ch.dbg.trace_buffer.ptr->type = CH_TRACE_TYPE_ISR_ENTER; + ch.dbg.trace_buffer.ptr->state = 0U; + ch.dbg.trace_buffer.ptr->u.isr.name = isr; + trace_next(); + port_unlock_from_isr(); + } +} + +/** + * @brief Inserts in the circular debug trace buffer an ISR-leave record. + * + * @param[in] isr name of the isr + * + * @notapi + */ +void _trace_isr_leave(const char *isr) { + + if ((ch.dbg.trace_buffer.suspended & CH_DBG_TRACE_MASK_ISR) == 0U) { + port_lock_from_isr(); + ch.dbg.trace_buffer.ptr->type = CH_TRACE_TYPE_ISR_LEAVE; + ch.dbg.trace_buffer.ptr->state = 0U; + ch.dbg.trace_buffer.ptr->u.isr.name = isr; + trace_next(); + port_unlock_from_isr(); + } +} + +/** + * @brief Inserts in the circular debug trace buffer an halt record. + * + * @param[in] reason the halt error string + * + * @notapi + */ +void _trace_halt(const char *reason) { + + if ((ch.dbg.trace_buffer.suspended & CH_DBG_TRACE_MASK_HALT) == 0U) { + ch.dbg.trace_buffer.ptr->type = CH_TRACE_TYPE_HALT; + ch.dbg.trace_buffer.ptr->state = 0; + ch.dbg.trace_buffer.ptr->u.halt.reason = reason; + trace_next(); + } +} + +/** + * @brief Adds an user trace record to the trace buffer. + * + * @param[in] up1 user parameter 1 + * @param[in] up2 user parameter 2 + * + * @iclass + */ +void chDbgWriteTraceI(void *up1, void *up2) { + + chDbgCheckClassI(); + + if ((ch.dbg.trace_buffer.suspended & CH_DBG_TRACE_MASK_USER) == 0U) { + ch.dbg.trace_buffer.ptr->type = CH_TRACE_TYPE_USER; + ch.dbg.trace_buffer.ptr->state = 0; + ch.dbg.trace_buffer.ptr->u.user.up1 = up1; + ch.dbg.trace_buffer.ptr->u.user.up2 = up2; + trace_next(); + } +} + +/** + * @brief Adds an user trace record to the trace buffer. + * + * @param[in] up1 user parameter 1 + * @param[in] up2 user parameter 2 + * + * @api + */ +void chDbgWriteTrace(void *up1, void *up2) { + + chSysLock(); + chDbgWriteTraceI(up1, up2); + chSysUnlock(); +} + +/** + * @brief Suspends one or more trace events. + * + * @param[in] mask mask of the trace events to be suspended + * + * @iclass + */ +void chDbgSuspendTraceI(uint16_t mask) { + + chDbgCheckClassI(); + + ch.dbg.trace_buffer.suspended |= mask; +} + +/** + * @brief Suspends one or more trace events. + * + * @param[in] mask mask of the trace events to be suspended + * + * @api + */ +void chDbgSuspendTrace(uint16_t mask) { + + chSysLock(); + chDbgSuspendTraceI(mask); + chSysUnlock(); +} + +/** + * @brief Resumes one or more trace events. + * + * @param[in] mask mask of the trace events to be resumed + * + * @iclass + */ +void chDbgResumeTraceI(uint16_t mask) { + + chDbgCheckClassI(); + + ch.dbg.trace_buffer.suspended &= ~mask; +} + +/** + * @brief Resumes one or more trace events. + * + * @param[in] mask mask of the trace events to be resumed + * + * @api + */ +void chDbgResumeTrace(uint16_t mask) { + + chSysLock(); + chDbgResumeTraceI(mask); + chSysUnlock(); +} +#endif /* CH_DBG_TRACE_MASK != CH_DBG_TRACE_MASK_DISABLED */ + +/** @} */ diff --git a/ChibiOS_20.3.2/os/rt/src/chvt.c b/ChibiOS_20.3.2/os/rt/src/chvt.c new file mode 100644 index 0000000..c138796 --- /dev/null +++ b/ChibiOS_20.3.2/os/rt/src/chvt.c @@ -0,0 +1,477 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS 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 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 rt/src/chvt.c + * @brief Time and Virtual Timers module code. + * + * @addtogroup time + * @details Time and Virtual Timers related APIs and services. + * @{ + */ + +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/** + * @brief List empty check. + * + * @param[in] vtlp pointer to the list header + * + * @notapi + */ +#define is_vtlist_empty(vtlp) ((vtlp) == (virtual_timers_list_t *)(vtlp)->next) + +/** + * @brief Last timer in the list check. + * + * @param[in] vtlp pointer to the list header + * @param[in] vtp pointer to the timer header + * + * @notapi + */ +#define is_last_timer(vtlp, vtp) ((vtp)->next == (virtual_timer_t *)(vtlp)) + +/** + * @brief Fist timer in the list check. + * + * @param[in] vtlp pointer to the list header + * @param[in] vtp pointer to the timer header + * + * @notapi + */ +#define is_first_timer(vtlp, vtp) ((vtlp)->next == (vtp)) + +/** + * @brief Timer check. + * + * @param[in] vtlp pointer to the list header + * @param[in] vtp pointer to the timer header + * + * @notapi + */ +#define is_timer(vtlp, vtp) ((vtp) != (virtual_timer_t *)(vtlp)) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if (CH_CFG_ST_TIMEDELTA > 0) || defined(__DOXYGEN__) +/** + * @brief Delta list compression. + * + * @param[in] vtlp pointer to the delta list to be compressed + * @param[in] deltanow interval to be compacted starting from "lasttime" + * + * @notapi + */ +static void vt_list_compress(virtual_timers_list_t *vtlp, + sysinterval_t deltanow) { + virtual_timer_t *vtp = vtlp->next; + + /* The loop is bounded because the delta list header has the delta field + set to (sysinterval_t)-1 which is larger than all deltas.*/ + while (vtp->delta < deltanow) { + deltanow -= vtp->delta; + vtp->delta = (sysinterval_t)0; + vtp = vtp->next; + } + + vtlp->lasttime = vtlp->lasttime + deltanow; + + /* Adjusting next timer in the list, if any.*/ + if (is_timer(vtlp, vtp)) { + vtp->delta -= deltanow; + } +} +#endif + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Virtual Timers initialization. + * @note Internal use only. + * + * @notapi + */ +void _vt_init(void) { + + ch.vtlist.next = (virtual_timer_t *)&ch.vtlist; + ch.vtlist.prev = (virtual_timer_t *)&ch.vtlist; + ch.vtlist.delta = (sysinterval_t)-1; +#if CH_CFG_ST_TIMEDELTA == 0 + ch.vtlist.systime = (systime_t)0; +#else /* CH_CFG_ST_TIMEDELTA > 0 */ + ch.vtlist.lasttime = (systime_t)0; +#endif /* CH_CFG_ST_TIMEDELTA > 0 */ +} + +/** + * @brief Enables a virtual timer. + * @details The timer is enabled and programmed to trigger after the delay + * specified as parameter. + * @pre The timer must not be already armed before calling this function. + * @note The callback function is invoked from interrupt context. + * + * @param[out] vtp the @p virtual_timer_t structure pointer + * @param[in] delay the number of ticks before the operation timeouts, the + * special values are handled as follow: + * - @a TIME_INFINITE is allowed but interpreted as a + * normal time specification. + * - @a TIME_IMMEDIATE this value is not allowed. + * . + * @param[in] vtfunc the timer callback function. After invoking the + * callback the timer is disabled and the structure can + * be disposed or reused. + * @param[in] par a parameter that will be passed to the callback + * function + * + * @iclass + */ +void chVTDoSetI(virtual_timer_t *vtp, sysinterval_t delay, + vtfunc_t vtfunc, void *par) { + virtual_timers_list_t *vtlp = &ch.vtlist; + virtual_timer_t *p; + sysinterval_t delta; + + chDbgCheckClassI(); + chDbgCheck((vtp != NULL) && (vtfunc != NULL) && (delay != TIME_IMMEDIATE)); + + vtp->par = par; + vtp->func = vtfunc; + +#if CH_CFG_ST_TIMEDELTA > 0 + { + systime_t now = chVTGetSystemTimeX(); + sysinterval_t deltanow; + + /* If the requested delay is lower than the minimum safe delta then it + is raised to the minimum safe value.*/ + if (delay < (sysinterval_t)CH_CFG_ST_TIMEDELTA) { + delay = (sysinterval_t)CH_CFG_ST_TIMEDELTA; + } + + /* Special case where the timers list is empty.*/ + if (is_vtlist_empty(vtlp)) { + + /* The delta list is empty, the current time becomes the new + delta list base time, the timer is inserted.*/ + vtlp->lasttime = now; + vtlp->next = vtp; + vtlp->prev = vtp; + vtp->next = (virtual_timer_t *)vtlp; + vtp->prev = (virtual_timer_t *)vtlp; + vtp->delta = delay; + +#if CH_CFG_INTERVALS_SIZE > CH_CFG_ST_RESOLUTION + /* The delta could be too large for the physical timer to handle.*/ + if (delay > (sysinterval_t)TIME_MAX_SYSTIME) { + delay = (sysinterval_t)TIME_MAX_SYSTIME; + } +#endif + + /* Being the first element in the list the alarm timer is started.*/ + port_timer_start_alarm(chTimeAddX(vtlp->lasttime, delay)); + + return; + } + + /* Delay as delta from 'lasttime'. Note, it can overflow and the value + becomes lower than 'deltanow'.*/ + deltanow = chTimeDiffX(vtlp->lasttime, now); + delta = deltanow + delay; + + /* Scenario where a very large delay exceeded the numeric range, it + requires a special handling, the compression procedure.*/ + if (delta < deltanow) { + vt_list_compress(vtlp, deltanow); + delta -= deltanow; + } + else if (delta < vtlp->next->delta) { + sysinterval_t deadline_delta; + + /* A small delay that will become the first element in the delta list + and next deadline.*/ + deadline_delta = delta; +#if CH_CFG_INTERVALS_SIZE > CH_CFG_ST_RESOLUTION + /* The delta could be too large for the physical timer to handle.*/ + if (deadline_delta > (sysinterval_t)TIME_MAX_SYSTIME) { + deadline_delta = (sysinterval_t)TIME_MAX_SYSTIME; + } +#endif + port_timer_set_alarm(chTimeAddX(vtlp->lasttime, deadline_delta)); + } + } +#else /* CH_CFG_ST_TIMEDELTA == 0 */ + /* Delta is initially equal to the specified delay.*/ + delta = delay; +#endif /* CH_CFG_ST_TIMEDELTA == 0 */ + + /* The delta list is scanned in order to find the correct position for + this timer. */ + p = vtlp->next; + while (p->delta < delta) { + /* Debug assert if the timer is already in the list.*/ + chDbgAssert(p != vtp, "timer already armed"); + + delta -= p->delta; + p = p->next; + } + + /* The timer is inserted in the delta list.*/ + vtp->next = p; + vtp->prev = vtp->next->prev; + vtp->prev->next = vtp; + p->prev = vtp; + vtp->delta = delta; + + /* Calculate new delta for the following entry.*/ + p->delta -= delta; + + /* Special case when the timer is in last position in the list, the + value in the header must be restored.*/ + vtlp->delta = (sysinterval_t)-1; +} + +/** + * @brief Disables a Virtual Timer. + * @pre The timer must be in armed state before calling this function. + * + * @param[in] vtp the @p virtual_timer_t structure pointer + * + * @iclass + */ +void chVTDoResetI(virtual_timer_t *vtp) { + virtual_timers_list_t *vtlp = &ch.vtlist; + + chDbgCheckClassI(); + chDbgCheck(vtp != NULL); + chDbgAssert(vtp->func != NULL, "timer not set or already triggered"); + +#if CH_CFG_ST_TIMEDELTA == 0 + + /* The delta of the timer is added to the next timer.*/ + vtp->next->delta += vtp->delta; + + /* Removing the element from the delta list.*/ + vtp->prev->next = vtp->next; + vtp->next->prev = vtp->prev; + vtp->func = NULL; + + /* The above code changes the value in the header when the removed element + is the last of the list, restoring it.*/ + vtlp->delta = (sysinterval_t)-1; +#else /* CH_CFG_ST_TIMEDELTA > 0 */ + sysinterval_t nowdelta, delta; + + /* If the timer is not the first of the list then it is simply unlinked + else the operation is more complex.*/ + if (!is_first_timer(vtlp, vtp)) { + /* Removing the element from the delta list.*/ + vtp->prev->next = vtp->next; + vtp->next->prev = vtp->prev; + vtp->func = NULL; + + /* Adding delta to the next element, if it is not the last one.*/ + if (is_timer(vtlp, vtp->next)) + vtp->next->delta += vtp->delta; + + return; + } + + /* Removing the first timer from the list.*/ + vtlp->next = vtp->next; + vtlp->next->prev = (virtual_timer_t *)vtlp; + vtp->func = NULL; + + /* If the list become empty then the alarm timer is stopped and done.*/ + if (is_vtlist_empty(vtlp)) { + port_timer_stop_alarm(); + + return; + } + + /* The delta of the removed timer is added to the new first timer.*/ + vtlp->next->delta += vtp->delta; + + /* If the new first timer has a delta of zero then the alarm is not + modified, the already programmed alarm will serve it.*/ +/* if (vtlp->next->delta == 0) { + return; + }*/ + + /* Distance in ticks between the last alarm event and current time.*/ + nowdelta = chTimeDiffX(vtlp->lasttime, chVTGetSystemTimeX()); + + /* If the current time surpassed the time of the next element in list + then the event interrupt is already pending, just return.*/ + if (nowdelta >= vtlp->next->delta) { + return; + } + + /* Distance from the next scheduled event and now.*/ + delta = vtlp->next->delta - nowdelta; + + /* Making sure to not schedule an event closer than CH_CFG_ST_TIMEDELTA + ticks from now.*/ + if (delta < (sysinterval_t)CH_CFG_ST_TIMEDELTA) { + delta = nowdelta + (sysinterval_t)CH_CFG_ST_TIMEDELTA; + } + else { + delta = nowdelta + delta; +#if CH_CFG_INTERVALS_SIZE > CH_CFG_ST_RESOLUTION + /* The delta could be too large for the physical timer to handle.*/ + if (delta > (sysinterval_t)TIME_MAX_SYSTIME) { + delta = (sysinterval_t)TIME_MAX_SYSTIME; + } +#endif + } + port_timer_set_alarm(chTimeAddX(vtlp->lasttime, delta)); +#endif /* CH_CFG_ST_TIMEDELTA > 0 */ +} + +/** + * @brief Virtual timers ticker. + * @note The system lock is released before entering the callback and + * re-acquired immediately after. It is callback's responsibility + * to acquire the lock if needed. This is done in order to reduce + * interrupts jitter when many timers are in use. + * + * @iclass + */ +void chVTDoTickI(void) { + virtual_timers_list_t *vtlp = &ch.vtlist; + + chDbgCheckClassI(); + +#if CH_CFG_ST_TIMEDELTA == 0 + vtlp->systime++; + if (!is_vtlist_empty(vtlp)) { + /* The list is not empty, processing elements on top.*/ + --vtlp->next->delta; + while (vtlp->next->delta == (sysinterval_t)0) { + virtual_timer_t *vtp; + vtfunc_t fn; + + vtp = vtlp->next; + fn = vtp->func; + vtp->func = NULL; + vtp->next->prev = (virtual_timer_t *)vtlp; + vtlp->next = vtp->next; + chSysUnlockFromISR(); + fn(vtp->par); + chSysLockFromISR(); + } + } +#else /* CH_CFG_ST_TIMEDELTA > 0 */ + virtual_timer_t *vtp; + systime_t now; + sysinterval_t delta, nowdelta; + + /* Looping through timers.*/ + vtp = vtlp->next; + while (true) { + + /* Getting the system time as reference.*/ + now = chVTGetSystemTimeX(); + nowdelta = chTimeDiffX(vtlp->lasttime, now); + + /* The list scan is limited by the timers header having + "vtlp->vt_delta == (sysinterval_t)-1" which is + greater than all deltas.*/ + if (nowdelta < vtp->delta) { + break; + } + + /* Consuming all timers between "vtp->lasttime" and now.*/ + do { + vtfunc_t fn; + + /* The "last time" becomes this timer's expiration time.*/ + vtlp->lasttime += vtp->delta; + nowdelta -= vtp->delta; + + vtp->next->prev = (virtual_timer_t *)vtlp; + vtlp->next = vtp->next; + fn = vtp->func; + vtp->func = NULL; + + /* If the list becomes empty then the timer is stopped.*/ + if (is_vtlist_empty(vtlp)) { + port_timer_stop_alarm(); + } + + /* The callback is invoked outside the kernel critical zone.*/ + chSysUnlockFromISR(); + fn(vtp->par); + chSysLockFromISR(); + + /* Next element in the list.*/ + vtp = vtlp->next; + } + while (vtp->delta <= nowdelta); + } + + /* If the list is empty, nothing else to do.*/ + if (is_vtlist_empty(vtlp)) { + return; + } + + /* The "unprocessed nowdelta" time slice is added to "last time" + and subtracted to next timer's delta.*/ + vtlp->lasttime += nowdelta; + vtlp->next->delta -= nowdelta; + + /* Recalculating the next alarm time.*/ + delta = vtp->delta - chTimeDiffX(vtlp->lasttime, now); + if (delta < (sysinterval_t)CH_CFG_ST_TIMEDELTA) { + delta = (sysinterval_t)CH_CFG_ST_TIMEDELTA; + } +#if CH_CFG_INTERVALS_SIZE > CH_CFG_ST_RESOLUTION + /* The delta could be too large for the physical timer to handle.*/ + else if (delta > (sysinterval_t)TIME_MAX_SYSTIME) { + delta = (sysinterval_t)TIME_MAX_SYSTIME; + } +#endif + port_timer_set_alarm(chTimeAddX(now, delta)); + + chDbgAssert(chTimeDiffX(vtlp->lasttime, chVTGetSystemTimeX()) <= + chTimeDiffX(vtlp->lasttime, chTimeAddX(now, delta)), + "exceeding delta"); +#endif /* CH_CFG_ST_TIMEDELTA > 0 */ +} + +/** @} */ -- cgit v1.2.3