From a1435e018bfc9919cb76b1356509ecc883767fb4 Mon Sep 17 00:00:00 2001 From: gdisirio Date: Sat, 10 Aug 2013 14:51:16 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/branches/kernel_3_dev@6123 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/rt/src/chcond.c | 305 ++++++++++++++++++++++++++ os/rt/src/chdebug.c | 260 ++++++++++++++++++++++ os/rt/src/chdynamic.c | 228 ++++++++++++++++++++ os/rt/src/chevents.c | 573 +++++++++++++++++++++++++++++++++++++++++++++++++ os/rt/src/chheap.c | 276 ++++++++++++++++++++++++ os/rt/src/chlists.c | 180 ++++++++++++++++ os/rt/src/chmboxes.c | 398 ++++++++++++++++++++++++++++++++++ os/rt/src/chmemcore.c | 153 +++++++++++++ os/rt/src/chmempools.c | 194 +++++++++++++++++ os/rt/src/chmsg.c | 151 +++++++++++++ os/rt/src/chmtx.c | 420 ++++++++++++++++++++++++++++++++++++ os/rt/src/chqueues.c | 455 +++++++++++++++++++++++++++++++++++++++ os/rt/src/chregistry.c | 175 +++++++++++++++ os/rt/src/chschd.c | 372 ++++++++++++++++++++++++++++++++ os/rt/src/chsem.c | 401 ++++++++++++++++++++++++++++++++++ os/rt/src/chstats.c | 122 +++++++++++ os/rt/src/chsys.c | 298 +++++++++++++++++++++++++ os/rt/src/chthreads.c | 462 +++++++++++++++++++++++++++++++++++++++ os/rt/src/chtm.c | 155 +++++++++++++ os/rt/src/chvt.c | 211 ++++++++++++++++++ 20 files changed, 5789 insertions(+) create mode 100644 os/rt/src/chcond.c create mode 100644 os/rt/src/chdebug.c create mode 100644 os/rt/src/chdynamic.c create mode 100644 os/rt/src/chevents.c create mode 100644 os/rt/src/chheap.c create mode 100644 os/rt/src/chlists.c create mode 100644 os/rt/src/chmboxes.c create mode 100644 os/rt/src/chmemcore.c create mode 100644 os/rt/src/chmempools.c create mode 100644 os/rt/src/chmsg.c create mode 100644 os/rt/src/chmtx.c create mode 100644 os/rt/src/chqueues.c create mode 100644 os/rt/src/chregistry.c create mode 100644 os/rt/src/chschd.c create mode 100644 os/rt/src/chsem.c create mode 100644 os/rt/src/chstats.c create mode 100644 os/rt/src/chsys.c create mode 100644 os/rt/src/chthreads.c create mode 100644 os/rt/src/chtm.c create mode 100644 os/rt/src/chvt.c (limited to 'os/rt/src') diff --git a/os/rt/src/chcond.c b/os/rt/src/chcond.c new file mode 100644 index 000000000..94fd8e81c --- /dev/null +++ b/os/rt/src/chcond.c @@ -0,0 +1,305 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Concepts and parts of this file have been contributed by Leon Woestenberg. + */ + +/** + * @file chcond.c + * @brief Condition Variables code. + * + * @addtogroup condition variables Condition Variables + * @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 || 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->c_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->c_queue)) + chSchWakeupS(queue_fifo_remove(&cp->c_queue), RDY_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->c_queue)) + chSchReadyI(queue_fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_OK; +} + +/** + * @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 RDY_RESET in + order to make a chCondBroadcast() detectable from a chCondSignal().*/ + while (cp->c_queue.p_next != (void *)&cp->c_queue) + chSchReadyI(queue_fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_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 RDY_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval RDY_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 RDY_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval RDY_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; + msg_t msg; + + chDbgCheckClassS(); + chDbgCheck(cp != NULL); + chDbgAssert(ctp->p_mtxlist != NULL, "not owning a mutex"); + + mp = chMtxUnlockS(); + ctp->p_u.wtobjp = cp; + queue_prio_insert(ctp, &cp->c_queue); + chSchGoSleepS(CH_STATE_WTCOND); + msg = ctp->p_u.rdymsg; + chMtxLockS(mp); + return msg; +} + +#if CH_CFG_USE_CONDVARS_TIMEOUT || 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] time 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 RDY_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval RDY_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * @retval RDY_TIMEOUT if the condition variable has not been signaled within + * the specified timeout. + * + * @api + */ +msg_t chCondWaitTimeout(condition_variable_t *cp, systime_t time) { + msg_t msg; + + chSysLock(); + msg = chCondWaitTimeoutS(cp, time); + 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] time 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 RDY_OK if the condition variable has been signaled using + * @p chCondSignal(). + * @retval RDY_RESET if the condition variable has been signaled using + * @p chCondBroadcast(). + * @retval RDY_TIMEOUT if the condition variable has not been signaled within + * the specified timeout. + * + * @sclass + */ +msg_t chCondWaitTimeoutS(condition_variable_t *cp, systime_t time) { + mutex_t *mp; + msg_t msg; + + chDbgCheckClassS(); + chDbgCheck((cp != NULL) && (time != TIME_IMMEDIATE)); + chDbgAssert(currp->p_mtxlist != NULL, "not owning a mutex"); + + mp = chMtxUnlockS(); + currp->p_u.wtobjp = cp; + queue_prio_insert(currp, &cp->c_queue); + msg = chSchGoSleepTimeoutS(CH_STATE_WTCOND, time); + if (msg != RDY_TIMEOUT) + chMtxLockS(mp); + return msg; +} +#endif /* CH_CFG_USE_CONDVARS_TIMEOUT */ + +#endif /* CH_CFG_USE_CONDVARS */ + +/** @} */ diff --git a/os/rt/src/chdebug.c b/os/rt/src/chdebug.c new file mode 100644 index 000000000..05b746357 --- /dev/null +++ b/os/rt/src/chdebug.c @@ -0,0 +1,260 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chdebug.c + * @brief ChibiOS/RT Debug code. + * + * @addtogroup debug + * @details Debug APIs and services: + * - Runtime system state and call protocol check. The following + * panic messages can be generated: + * - SV#1, misplaced @p chSysDisable(). + * - SV#2, misplaced @p chSysSuspend() + * - SV#3, misplaced @p chSysEnable(). + * - SV#4, misplaced @p chSysLock(). + * - SV#5, misplaced @p chSysUnlock(). + * - SV#6, misplaced @p chSysLockFromIsr(). + * - SV#7, misplaced @p chSysUnlockFromIsr(). + * - SV#8, misplaced @p CH_IRQ_PROLOGUE(). + * - SV#9, misplaced @p CH_IRQ_EPILOGUE(). + * - SV#10, misplaced I-class function. + * - SV#11, misplaced S-class function. + * . + * - Trace buffer. + * - Parameters check. + * - Kernel assertions. + * - Kernel panics. + * . + * @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 || defined(__DOXYGEN__) +/** + * @brief Guard code for @p chSysDisable(). + * + * @notapi + */ +void _dbg_check_disable(void) { + + if ((ch.dbg_isr_cnt != 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("SV#1"); +} + +/** + * @brief Guard code for @p chSysSuspend(). + * + * @notapi + */ +void _dbg_check_suspend(void) { + + if ((ch.dbg_isr_cnt != 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("SV#2"); +} + +/** + * @brief Guard code for @p chSysEnable(). + * + * @notapi + */ +void _dbg_check_enable(void) { + + if ((ch.dbg_isr_cnt != 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("SV#3"); +} + +/** + * @brief Guard code for @p chSysLock(). + * + * @notapi + */ +void _dbg_check_lock(void) { + + if ((ch.dbg_isr_cnt != 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("SV#4"); + _dbg_enter_lock(); +} + +/** + * @brief Guard code for @p chSysUnlock(). + * + * @notapi + */ +void _dbg_check_unlock(void) { + + if ((ch.dbg_isr_cnt != 0) || (ch.dbg_lock_cnt <= 0)) + chDbgPanic("SV#5"); + _dbg_leave_lock(); +} + +/** + * @brief Guard code for @p chSysLockFromIsr(). + * + * @notapi + */ +void _dbg_check_lock_from_isr(void) { + + if ((ch.dbg_isr_cnt <= 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("SV#6"); + _dbg_enter_lock(); +} + +/** + * @brief Guard code for @p chSysUnlockFromIsr(). + * + * @notapi + */ +void _dbg_check_unlock_from_isr(void) { + + if ((ch.dbg_isr_cnt <= 0) || (ch.dbg_lock_cnt <= 0)) + chDbgPanic("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 < 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("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 <= 0) || (ch.dbg_lock_cnt != 0)) + chDbgPanic("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 < 0) || (ch.dbg_lock_cnt <= 0)) + chDbgPanic("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 != 0) || (ch.dbg_lock_cnt <= 0)) + chDbgPanic("SV#11"); +} + +#endif /* CH_DBG_SYSTEM_STATE_CHECK */ + +#if CH_DBG_ENABLE_TRACE || defined(__DOXYGEN__) +/** + * @brief Trace circular buffer subsystem initialization. + * @note Internal use only. + */ +void _trace_init(void) { + + ch.dbg_trace_buffer.tb_size = CH_DBG_TRACE_BUFFER_SIZE; + ch.dbg_trace_buffer.tb_ptr = &ch.dbg_trace_buffer.tb_buffer[0]; +} + +/** + * @brief Inserts in the circular debug trace buffer a context switch record. + * + * @param[in] otp the thread being switched out + * + * @notapi + */ +void _dbg_trace(thread_t *otp) { + + ch.dbg_trace_buffer.tb_ptr->se_time = chVTGetSystemTimeX(); + ch.dbg_trace_buffer.tb_ptr->se_tp = currp; + ch.dbg_trace_buffer.tb_ptr->se_wtobjp = otp->p_u.wtobjp; + ch.dbg_trace_buffer.tb_ptr->se_state = (uint8_t)otp->p_state; + if (++ch.dbg_trace_buffer.tb_ptr >= + &ch.dbg_trace_buffer.tb_buffer[CH_DBG_TRACE_BUFFER_SIZE]) + ch.dbg_trace_buffer.tb_ptr = &ch.dbg_trace_buffer.tb_buffer[0]; +} +#endif /* CH_DBG_ENABLE_TRACE */ + +#if CH_DBG_ENABLED || defined(__DOXYGEN__) +/** + * @brief Prints a panic message on the console and then halts the system. + * + * @param[in] msg the pointer to the panic message string + */ +void chDbgPanic(const char *msg) { + + ch.dbg_panic_msg = msg; + chSysHalt(); +} +#endif /* CH_DBG_ENABLED */ + +/** @} */ diff --git a/os/rt/src/chdynamic.c b/os/rt/src/chdynamic.c new file mode 100644 index 000000000..5f174eda7 --- /dev/null +++ b/os/rt/src/chdynamic.c @@ -0,0 +1,228 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chdynamic.c + * @brief Dynamic threads code. + * + * @addtogroup dynamic_threads + * @details Dynamic threads related APIs and services. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_DYNAMIC || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Adds a reference to a thread object. + * @pre The configuration option @p CH_CFG_USE_DYNAMIC 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->p_refs < 255, "too many references"); + tp->p_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. + * @pre The configuration option @p CH_CFG_USE_DYNAMIC 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) { + trefs_t refs; + + chSysLock(); + chDbgAssert(tp->p_refs > 0, "not referenced"); + refs = --tp->p_refs; + chSysUnlock(); + + /* If the references counter reaches zero and the thread is in its + terminated state then the memory can be returned to the proper + allocator. Of course static threads are not affected.*/ + if ((refs == 0) && (tp->p_state == CH_STATE_FINAL)) { + switch (tp->p_flags & CH_FLAG_MODE_MASK) { +#if CH_CFG_USE_HEAP + case CH_FLAG_MODE_HEAP: +#if CH_CFG_USE_REGISTRY + REG_REMOVE(tp); +#endif + chHeapFree(tp); + break; +#endif +#if CH_CFG_USE_MEMPOOLS + case CH_FLAG_MODE_MEMPOOL: +#if CH_CFG_USE_REGISTRY + REG_REMOVE(tp); +#endif + chPoolFree(tp->p_mpool, tp); + break; +#endif + } + } +} + +#if CH_CFG_USE_HEAP || 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 when the thread + * terminates but when a @p chThdWait() is performed. + * + * @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] 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, + tprio_t prio, tfunc_t pf, void *arg) { + void *wsp; + thread_t *tp; + + wsp = chHeapAlloc(heapp, size); + if (wsp == NULL) + return NULL; + +#if CH_DBG_FILL_THREADS + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + sizeof(thread_t), + CH_DBG_THREAD_FILL_VALUE); + _thread_memfill((uint8_t *)wsp + sizeof(thread_t), + (uint8_t *)wsp + size, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateI(wsp, size, prio, pf, arg); + tp->p_flags = CH_FLAG_MODE_HEAP; + chSchWakeupS(tp, RDY_OK); + chSysUnlock(); + return tp; +} +#endif /* CH_CFG_USE_HEAP */ + +#if CH_CFG_USE_MEMPOOLS || 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. + * @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 when the thread + * terminates but when a @p chThdWait() is performed. + * + * @param[in] mp pointer to the memory pool object + * @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, tprio_t prio, + tfunc_t pf, void *arg) { + void *wsp; + thread_t *tp; + + chDbgCheck(mp != NULL); + + wsp = chPoolAlloc(mp); + if (wsp == NULL) + return NULL; + +#if CH_DBG_FILL_THREADS + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + sizeof(thread_t), + CH_DBG_THREAD_FILL_VALUE); + _thread_memfill((uint8_t *)wsp + sizeof(thread_t), + (uint8_t *)wsp + mp->mp_object_size, + CH_DBG_STACK_FILL_VALUE); +#endif + + chSysLock(); + tp = chThdCreateI(wsp, mp->mp_object_size, prio, pf, arg); + tp->p_flags = CH_FLAG_MODE_MEMPOOL; + tp->p_mpool = mp; + chSchWakeupS(tp, RDY_OK); + chSysUnlock(); + return tp; +} +#endif /* CH_CFG_USE_MEMPOOLS */ + +#endif /* CH_CFG_USE_DYNAMIC */ + +/** @} */ diff --git a/os/rt/src/chevents.c b/os/rt/src/chevents.c new file mode 100644 index 000000000..04b0e6fb8 --- /dev/null +++ b/os/rt/src/chevents.c @@ -0,0 +1,573 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Concepts and parts of this file have been contributed by Scott (skute). + */ + +/** + * @file chevents.c + * @brief Events code. + * + * @addtogroup events + * @details Event Flags, Event Sources and Event Listeners. + *

Operation mode

+ * Each thread has a mask of pending event flags inside its + * @p thread_t structure. + * Operations defined for event flags: + * - Wait, the invoking thread goes to sleep until a certain + * AND/OR combination of event flags becomes pending. + * - Clear, a mask of event flags is cleared from the pending + * events mask, the cleared event flags mask is returned (only the + * flags that were actually pending and then cleared). + * - Signal, an event mask is directly ORed to the mask of the + * signaled thread. + * - Broadcast, each thread registered on an Event Source is + * signaled with the event flags 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 || 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] mask the mask of event flags to be ORed to the thread when + * the event source is broadcasted + * + * @api + */ +void chEvtRegisterMask(event_source_t *esp, + event_listener_t *elp, + eventmask_t mask) { + + chDbgCheck((esp != NULL) && (elp != NULL)); + + chSysLock(); + elp->el_next = esp->es_next; + esp->es_next = elp; + elp->el_listener = currp; + elp->el_mask = mask; + elp->el_flags = 0; + 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)); + + p = (event_listener_t *)esp; + chSysLock(); + while (p->el_next != (event_listener_t *)esp) { + if (p->el_next == elp) { + p->el_next = elp->el_next; + break; + } + p = p->el_next; + } + chSysUnlock(); +} + +/** + * @brief Clears the pending events specified in the mask. + * + * @param[in] mask the events to be cleared + * @return The pending events that were cleared. + * + * @api + */ +eventmask_t chEvtGetAndClearEvents(eventmask_t mask) { + eventmask_t m; + + chSysLock(); + + m = currp->p_epending & mask; + currp->p_epending &= ~mask; + + chSysUnlock(); + return m; +} + +/** + * @brief Adds (OR) a set of event flags on the current thread, this is + * @b much faster than using @p chEvtBroadcast() or @p chEvtSignal(). + * + * @param[in] mask the event flags to be added + * @return The current pending events mask. + * + * @api + */ +eventmask_t chEvtAddEvents(eventmask_t mask) { + + chSysLock(); + + mask = (currp->p_epending |= mask); + + chSysUnlock(); + return mask; +} + +/** + * @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->es_next; + while (elp != (event_listener_t *)esp) { + elp->el_flags |= flags; + chEvtSignalI(elp->el_listener, elp->el_mask); + elp = elp->el_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. + * + * @iclass + */ +eventflags_t chEvtGetAndClearFlags(event_listener_t *elp) { + eventflags_t flags; + + chSysLock(); + + flags = elp->el_flags; + elp->el_flags = 0; + + chSysUnlock(); + return flags; +} + +/** + * @brief Adds a set of event flags directly to the specified @p thread_t. + * + * @param[in] tp the thread to be signaled + * @param[in] mask the event flags set to be ORed + * + * @api + */ +void chEvtSignal(thread_t *tp, eventmask_t mask) { + + chDbgCheck(tp != NULL); + + chSysLock(); + chEvtSignalI(tp, mask); + 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] mask the event flags set to be ORed + * + * @iclass + */ +void chEvtSignalI(thread_t *tp, eventmask_t mask) { + + chDbgCheckClassI(); + chDbgCheck(tp != NULL); + + tp->p_epending |= mask; + /* Test on the AND/OR conditions wait states.*/ + if (((tp->p_state == CH_STATE_WTOREVT) && + ((tp->p_epending & tp->p_u.ewmask) != 0)) || + ((tp->p_state == CH_STATE_WTANDEVT) && + ((tp->p_epending & tp->p_u.ewmask) == tp->p_u.ewmask))) + chSchReadyI(tp)->p_u.rdymsg = RDY_OK; +} + +/** + * @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 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->el_flags; + elp->el_flags = 0; + + return flags; +} + +/** + * @brief Invokes the event handlers associated to an event flags mask. + * + * @param[in] mask mask of the event flags 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 mask) { + eventid_t eid; + + chDbgCheck(handlers != NULL); + + eid = 0; + while (mask) { + if (mask & EVENT_MASK(eid)) { + chDbgAssert(handlers[eid] != NULL, "null handler"); + mask &= ~EVENT_MASK(eid); + handlers[eid](eid); + } + eid++; + } +} + +#if CH_CFG_OPTIMIZE_SPEED || !CH_CFG_USE_EVENTS_TIMEOUT || defined(__DOXYGEN__) +/** + * @brief Waits for exactly one of the specified events. + * @details The function waits for one event among those specified in + * @p mask 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] mask mask of the event flags that the function should wait + * for, @p ALL_EVENTS enables all the events + * @return The mask of the lowest id served and cleared event. + * + * @api + */ +eventmask_t chEvtWaitOne(eventmask_t mask) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + + if ((m = (ctp->p_epending & mask)) == 0) { + ctp->p_u.ewmask = mask; + chSchGoSleepS(CH_STATE_WTOREVT); + m = ctp->p_epending & mask; + } + m &= -m; + ctp->p_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 mask to become pending then the events are cleared and returned. + * + * @param[in] mask mask of the event flags 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 mask) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + + if ((m = (ctp->p_epending & mask)) == 0) { + ctp->p_u.ewmask = mask; + chSchGoSleepS(CH_STATE_WTOREVT); + m = ctp->p_epending & mask; + } + ctp->p_epending &= ~m; + + chSysUnlock(); + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p mask to + * become pending then the events are cleared and returned. + * + * @param[in] mask mask of the event flags 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 mask) { + thread_t *ctp = currp; + + chSysLock(); + + if ((ctp->p_epending & mask) != mask) { + ctp->p_u.ewmask = mask; + chSchGoSleepS(CH_STATE_WTANDEVT); + } + ctp->p_epending &= ~mask; + + chSysUnlock(); + return mask; +} +#endif /* CH_CFG_OPTIMIZE_SPEED || !CH_CFG_USE_EVENTS_TIMEOUT */ + +#if CH_CFG_USE_EVENTS_TIMEOUT || defined(__DOXYGEN__) +/** + * @brief Waits for exactly one of the specified events. + * @details The function waits for one event among those specified in + * @p mask 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] mask mask of the event flags that the function should wait + * for, @p ALL_EVENTS enables all the events + * @param[in] time 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 id served and cleared event. + * @retval 0 if the operation has timed out. + * + * @api + */ +eventmask_t chEvtWaitOneTimeout(eventmask_t mask, systime_t time) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + + if ((m = (ctp->p_epending & mask)) == 0) { + if (TIME_IMMEDIATE == time) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->p_u.ewmask = mask; + if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, time) < RDY_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + m = ctp->p_epending & mask; + } + m &= -m; + ctp->p_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 mask to become pending then the events are cleared and + * returned. + * + * @param[in] mask mask of the event flags that the function should wait + * for, @p ALL_EVENTS enables all the events + * @param[in] time 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 mask, systime_t time) { + thread_t *ctp = currp; + eventmask_t m; + + chSysLock(); + + if ((m = (ctp->p_epending & mask)) == 0) { + if (TIME_IMMEDIATE == time) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->p_u.ewmask = mask; + if (chSchGoSleepTimeoutS(CH_STATE_WTOREVT, time) < RDY_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + m = ctp->p_epending & mask; + } + ctp->p_epending &= ~m; + + chSysUnlock(); + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p mask to + * become pending then the events are cleared and returned. + * + * @param[in] mask mask of the event flags that the function should wait + * for, @p ALL_EVENTS requires all the events + * @param[in] time 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 mask, systime_t time) { + thread_t *ctp = currp; + + chSysLock(); + + if ((ctp->p_epending & mask) != mask) { + if (TIME_IMMEDIATE == time) { + chSysUnlock(); + return (eventmask_t)0; + } + ctp->p_u.ewmask = mask; + if (chSchGoSleepTimeoutS(CH_STATE_WTANDEVT, time) < RDY_OK) { + chSysUnlock(); + return (eventmask_t)0; + } + } + ctp->p_epending &= ~mask; + + chSysUnlock(); + return mask; +} +#endif /* CH_CFG_USE_EVENTS_TIMEOUT */ + +#endif /* CH_CFG_USE_EVENTS */ + +/** @} */ diff --git a/os/rt/src/chheap.c b/os/rt/src/chheap.c new file mode 100644 index 000000000..abda4ff10 --- /dev/null +++ b/os/rt/src/chheap.c @@ -0,0 +1,276 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chheap.c + * @brief Heaps code. + * + * @addtogroup heaps + * @details Heap Allocator related APIs. + *

Operation mode

+ * The heap allocator implements a first-fit strategy and its APIs + * are functionally equivalent to the usual @p malloc() and @p free() + * library functions. The main difference is that the OS heap APIs + * are guaranteed to be thread safe.
+ * @pre In order to use the heap APIs the @p CH_CFG_USE_HEAP option must + * be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_HEAP || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/* + * Defaults on the best synchronization mechanism available. + */ +#if CH_CFG_USE_MUTEXES || defined(__DOXYGEN__) +#define H_LOCK(h) chMtxLock(&(h)->h_mtx) +#define H_UNLOCK(h) chMtxUnlock() +#else +#define H_LOCK(h) chSemWait(&(h)->h_sem) +#define H_UNLOCK(h) chSemSignal(&(h)->h_sem) +#endif + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/** + * @brief Default heap descriptor. + */ +static memory_heap_t default_heap; + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes the default heap. + * + * @notapi + */ +void _heap_init(void) { + default_heap.h_provider = chCoreAlloc; + default_heap.h_free.h.u.next = (union heap_header *)NULL; + default_heap.h_free.h.size = 0; +#if CH_CFG_USE_MUTEXES || defined(__DOXYGEN__) + chMtxObjectInit(&default_heap.h_mtx); +#else + chSemObjectInit(&default_heap.h_sem, 1); +#endif +} + +/** + * @brief Initializes a memory heap from a static memory area. + * @pre Both the heap buffer base and the heap size must be aligned to + * the @p stkalign_t type size. + * + * @param[out] heapp pointer to the memory heap descriptor to be initialized + * @param[in] buf heap buffer base + * @param[in] size heap size + * + * @init + */ +void chHeapObjectInit(memory_heap_t *heapp, void *buf, size_t size) { + union heap_header *hp; + + chDbgCheck(MEM_IS_ALIGNED(buf) && MEM_IS_ALIGNED(size)); + + heapp->h_provider = (memgetfunc_t)NULL; + heapp->h_free.h.u.next = hp = buf; + heapp->h_free.h.size = 0; + hp->h.u.next = NULL; + hp->h.size = size - sizeof(union heap_header); +#if CH_CFG_USE_MUTEXES || defined(__DOXYGEN__) + chMtxObjectInit(&heapp->h_mtx); +#else + chSemObjectInit(&heapp->h_sem, 1); +#endif +} + +/** + * @brief Allocates a block of memory from the heap by using the first-fit + * algorithm. + * @details The allocated block is guaranteed to be properly aligned for a + * pointer data type (@p stkalign_t). + * + * @param[in] heapp pointer to a heap descriptor or @p NULL in order to + * access the default heap. + * @param[in] size the size of the block to be allocated. Note that the + * allocated block may be a bit bigger than the requested + * size for alignment and fragmentation reasons. + * @return A pointer to the allocated block. + * @retval NULL if the block cannot be allocated. + * + * @api + */ +void *chHeapAlloc(memory_heap_t *heapp, size_t size) { + union heap_header *qp, *hp, *fp; + + if (heapp == NULL) + heapp = &default_heap; + + size = MEM_ALIGN_NEXT(size); + qp = &heapp->h_free; + H_LOCK(heapp); + + while (qp->h.u.next != NULL) { + hp = qp->h.u.next; + if (hp->h.size >= size) { + if (hp->h.size < size + sizeof(union heap_header)) { + /* Gets the whole block even if it is slightly bigger than the + requested size because the fragment would be too small to be + useful.*/ + qp->h.u.next = hp->h.u.next; + } + else { + /* Block bigger enough, must split it.*/ + fp = (void *)((uint8_t *)(hp) + sizeof(union heap_header) + size); + fp->h.u.next = hp->h.u.next; + fp->h.size = hp->h.size - sizeof(union heap_header) - size; + qp->h.u.next = fp; + hp->h.size = size; + } + hp->h.u.heap = heapp; + + H_UNLOCK(heapp); + return (void *)(hp + 1); + } + qp = hp; + } + + H_UNLOCK(heapp); + + /* More memory is required, tries to get it from the associated provider + else fails.*/ + if (heapp->h_provider) { + hp = heapp->h_provider(size + sizeof(union heap_header)); + if (hp != NULL) { + hp->h.u.heap = heapp; + hp->h.size = size; + hp++; + return (void *)hp; + } + } + return NULL; +} + +#define LIMIT(p) (union heap_header *)((uint8_t *)(p) + \ + sizeof(union heap_header) + \ + (p)->h.size) + +/** + * @brief Frees a previously allocated memory block. + * + * @param[in] p pointer to the memory block to be freed + * + * @api + */ +void chHeapFree(void *p) { + union heap_header *qp, *hp; + memory_heap_t *heapp; + + chDbgCheck(p != NULL); + + hp = (union heap_header *)p - 1; + heapp = hp->h.u.heap; + qp = &heapp->h_free; + H_LOCK(heapp); + + while (true) { + chDbgAssert((hp < qp) || (hp >= LIMIT(qp)), "within free block"); + + if (((qp == &heapp->h_free) || (hp > qp)) && + ((qp->h.u.next == NULL) || (hp < qp->h.u.next))) { + /* Insertion after qp.*/ + hp->h.u.next = qp->h.u.next; + qp->h.u.next = hp; + /* Verifies if the newly inserted block should be merged.*/ + if (LIMIT(hp) == hp->h.u.next) { + /* Merge with the next block.*/ + hp->h.size += hp->h.u.next->h.size + sizeof(union heap_header); + hp->h.u.next = hp->h.u.next->h.u.next; + } + if ((LIMIT(qp) == hp)) { + /* Merge with the previous block.*/ + qp->h.size += hp->h.size + sizeof(union heap_header); + qp->h.u.next = hp->h.u.next; + } + break; + } + qp = qp->h.u.next; + } + + H_UNLOCK(heapp); + return; +} + +/** + * @brief Reports the heap status. + * @note This function is meant to be used in the test suite, it should + * not be really useful for the application code. + * + * @param[in] heapp pointer to a heap descriptor or @p NULL in order to + * access the default heap. + * @param[in] sizep pointer to a variable that will receive the total + * fragmented free space + * @return The number of fragments in the heap. + * + * @api + */ +size_t chHeapStatus(memory_heap_t *heapp, size_t *sizep) { + union heap_header *qp; + size_t n, sz; + + if (heapp == NULL) + heapp = &default_heap; + + H_LOCK(heapp); + + sz = 0; + for (n = 0, qp = &heapp->h_free; qp->h.u.next; n++, qp = qp->h.u.next) + sz += qp->h.u.next->h.size; + if (sizep) + *sizep = sz; + + H_UNLOCK(heapp); + return n; +} + +#endif /* CH_CFG_USE_HEAP */ + +/** @} */ diff --git a/os/rt/src/chlists.c b/os/rt/src/chlists.c new file mode 100644 index 000000000..0f492db5f --- /dev/null +++ b/os/rt/src/chlists.c @@ -0,0 +1,180 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chlists.c + * @brief Thread queues/lists code. + * + * @addtogroup internals + * @details All the functions present in this module, while public, are not + * OS APIs and should not be directly used in the user applications + * code. + * @{ + */ +#include "ch.h" + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +#if !CH_CFG_OPTIMIZE_SPEED || 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) { + + /* cp iterates over the queue.*/ + thread_t *cp = (thread_t *)tqp; + do { + /* Iterate to next thread in queue.*/ + cp = cp->p_next; + /* Not end of queue? and cp has equal or higher priority than tp?.*/ + } while ((cp != (thread_t *)tqp) && (cp->p_prio >= tp->p_prio)); + /* Insertion on p_prev.*/ + tp->p_next = cp; + tp->p_prev = cp->p_prev; + tp->p_prev->p_next = cp->p_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->p_next = (thread_t *)tqp; + tp->p_prev = tqp->p_prev; + tp->p_prev->p_next = tqp->p_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->p_next; + + (tqp->p_next = tp->p_next)->p_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->p_prev; + + (tqp->p_prev = tp->p_prev)->p_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->p_prev->p_next = tp->p_next; + tp->p_next->p_prev = tp->p_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->p_next = tlp->p_next; + tlp->p_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->p_next; + tlp->p_next = tp->p_next; + return tp; +} +#endif /* CH_CFG_OPTIMIZE_SPEED */ + +/** @} */ diff --git a/os/rt/src/chmboxes.c b/os/rt/src/chmboxes.c new file mode 100644 index 000000000..003c779ec --- /dev/null +++ b/os/rt/src/chmboxes.c @@ -0,0 +1,398 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chmboxes.c + * @brief Mailboxes code. + * + * @addtogroup mailboxes + * @details Asynchronous messages. + *

Operation mode

+ * A mailbox is an asynchronous communication mechanism.
+ * Operations defined for mailboxes: + * - Post: Posts a message on the mailbox in FIFO order. + * - Post Ahead: Posts a message on the mailbox with urgent + * priority. + * - Fetch: A message is fetched from the mailbox and removed + * from the queue. + * - Reset: The mailbox is emptied and all the stored messages + * are lost. + * . + * A message is a variable of type msg_t that is guaranteed to have + * the same size of and be compatible with (data) pointers (anyway an + * explicit cast is needed). + * If larger messages need to be exchanged then a pointer to a + * structure can be posted in the mailbox but the posting side has + * no predefined way to know when the message has been processed. A + * possible approach is to allocate memory (from a memory pool for + * example) from the posting side and free it on the fetching side. + * Another approach is to set a "done" flag into the structure pointed + * by the message. + * @pre In order to use the mailboxes APIs the @p CH_CFG_USE_MAILBOXES option + * must be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_MAILBOXES || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes a @p mailbox_t object. + * + * @param[out] mbp the pointer to the @p mailbox_t structure to be + * initialized + * @param[in] buf pointer to the messages buffer as an array of @p msg_t + * @param[in] n number of elements in the buffer array + * + * @init + */ +void chMBObjectInit(mailbox_t *mbp, msg_t *buf, cnt_t n) { + + chDbgCheck((mbp != NULL) && (buf != NULL) && (n > 0)); + + mbp->mb_buffer = mbp->mb_wrptr = mbp->mb_rdptr = buf; + mbp->mb_top = &buf[n]; + chSemObjectInit(&mbp->mb_emptysem, n); + chSemObjectInit(&mbp->mb_fullsem, 0); +} + +/** + * @brief Resets a @p mailbox_t object. + * @details All the waiting threads are resumed with status @p RDY_RESET and + * the queued messages are lost. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * + * @api + */ +void chMBReset(mailbox_t *mbp) { + + chDbgCheck(mbp != NULL); + + chSysLock(); + mbp->mb_wrptr = mbp->mb_rdptr = mbp->mb_buffer; + chSemResetI(&mbp->mb_emptysem, mbp->mb_top - mbp->mb_buffer); + chSemResetI(&mbp->mb_fullsem, 0); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Posts a message into a mailbox. + * @details The invoking thread waits until a empty slot in the mailbox becomes + * available or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @api + */ +msg_t chMBPost(mailbox_t *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chSysLock(); + rdymsg = chMBPostS(mbp, msg, time); + chSysUnlock(); + return rdymsg; +} + +/** + * @brief Posts a message into a mailbox. + * @details The invoking thread waits until a empty slot in the mailbox becomes + * available or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @sclass + */ +msg_t chMBPostS(mailbox_t *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chDbgCheckClassS(); + chDbgCheck(mbp != NULL); + + rdymsg = chSemWaitTimeoutS(&mbp->mb_emptysem, time); + if (rdymsg == RDY_OK) { + *mbp->mb_wrptr++ = msg; + if (mbp->mb_wrptr >= mbp->mb_top) + mbp->mb_wrptr = mbp->mb_buffer; + chSemSignalI(&mbp->mb_fullsem); + chSchRescheduleS(); + } + return rdymsg; +} + +/** + * @brief Posts a message into a mailbox. + * @details This variant is non-blocking, the function returns a timeout + * condition if the queue is full. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @return The operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_TIMEOUT if the mailbox is full and the message cannot be + * posted. + * + * @iclass + */ +msg_t chMBPostI(mailbox_t *mbp, msg_t msg) { + + chDbgCheckClassI(); + chDbgCheck(mbp != NULL); + + if (chSemGetCounterI(&mbp->mb_emptysem) <= 0) + return RDY_TIMEOUT; + chSemFastWaitI(&mbp->mb_emptysem); + *mbp->mb_wrptr++ = msg; + if (mbp->mb_wrptr >= mbp->mb_top) + mbp->mb_wrptr = mbp->mb_buffer; + chSemSignalI(&mbp->mb_fullsem); + return RDY_OK; +} + +/** + * @brief Posts an high priority message into a mailbox. + * @details The invoking thread waits until a empty slot in the mailbox becomes + * available or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @api + */ +msg_t chMBPostAhead(mailbox_t *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chSysLock(); + rdymsg = chMBPostAheadS(mbp, msg, time); + chSysUnlock(); + return rdymsg; +} + +/** + * @brief Posts an high priority message into a mailbox. + * @details The invoking thread waits until a empty slot in the mailbox becomes + * available or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @sclass + */ +msg_t chMBPostAheadS(mailbox_t *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chDbgCheckClassS(); + chDbgCheck(mbp != NULL); + + rdymsg = chSemWaitTimeoutS(&mbp->mb_emptysem, time); + if (rdymsg == RDY_OK) { + if (--mbp->mb_rdptr < mbp->mb_buffer) + mbp->mb_rdptr = mbp->mb_top - 1; + *mbp->mb_rdptr = msg; + chSemSignalI(&mbp->mb_fullsem); + chSchRescheduleS(); + } + return rdymsg; +} + +/** + * @brief Posts an high priority message into a mailbox. + * @details This variant is non-blocking, the function returns a timeout + * condition if the queue is full. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[in] msg the message to be posted on the mailbox + * @return The operation status. + * @retval RDY_OK if a message has been correctly posted. + * @retval RDY_TIMEOUT if the mailbox is full and the message cannot be + * posted. + * + * @iclass + */ +msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg) { + + chDbgCheckClassI(); + chDbgCheck(mbp != NULL); + + if (chSemGetCounterI(&mbp->mb_emptysem) <= 0) + return RDY_TIMEOUT; + chSemFastWaitI(&mbp->mb_emptysem); + if (--mbp->mb_rdptr < mbp->mb_buffer) + mbp->mb_rdptr = mbp->mb_top - 1; + *mbp->mb_rdptr = msg; + chSemSignalI(&mbp->mb_fullsem); + return RDY_OK; +} + +/** + * @brief Retrieves a message from a mailbox. + * @details The invoking thread waits until a message is posted in the mailbox + * or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[out] msgp pointer to a message variable for the received message + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly fetched. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @api + */ +msg_t chMBFetch(mailbox_t *mbp, msg_t *msgp, systime_t time) { + msg_t rdymsg; + + chSysLock(); + rdymsg = chMBFetchS(mbp, msgp, time); + chSysUnlock(); + return rdymsg; +} + +/** + * @brief Retrieves a message from a mailbox. + * @details The invoking thread waits until a message is posted in the mailbox + * or the specified time runs out. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[out] msgp pointer to a message variable for the received message + * @param[in] time 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 operation status. + * @retval RDY_OK if a message has been correctly fetched. + * @retval RDY_RESET if the mailbox has been reset while waiting. + * @retval RDY_TIMEOUT if the operation has timed out. + * + * @sclass + */ +msg_t chMBFetchS(mailbox_t *mbp, msg_t *msgp, systime_t time) { + msg_t rdymsg; + + chDbgCheckClassS(); + chDbgCheck((mbp != NULL) && (msgp != NULL)); + + rdymsg = chSemWaitTimeoutS(&mbp->mb_fullsem, time); + if (rdymsg == RDY_OK) { + *msgp = *mbp->mb_rdptr++; + if (mbp->mb_rdptr >= mbp->mb_top) + mbp->mb_rdptr = mbp->mb_buffer; + chSemSignalI(&mbp->mb_emptysem); + chSchRescheduleS(); + } + return rdymsg; +} + +/** + * @brief Retrieves a message from a mailbox. + * @details This variant is non-blocking, the function returns a timeout + * condition if the queue is empty. + * + * @param[in] mbp the pointer to an initialized @p mailbox_t object + * @param[out] msgp pointer to a message variable for the received message + * @return The operation status. + * @retval RDY_OK if a message has been correctly fetched. + * @retval RDY_TIMEOUT if the mailbox is empty and a message cannot be + * fetched. + * + * @iclass + */ +msg_t chMBFetchI(mailbox_t *mbp, msg_t *msgp) { + + chDbgCheckClassI(); + chDbgCheck((mbp != NULL) && (msgp != NULL)); + + if (chSemGetCounterI(&mbp->mb_fullsem) <= 0) + return RDY_TIMEOUT; + chSemFastWaitI(&mbp->mb_fullsem); + *msgp = *mbp->mb_rdptr++; + if (mbp->mb_rdptr >= mbp->mb_top) + mbp->mb_rdptr = mbp->mb_buffer; + chSemSignalI(&mbp->mb_emptysem); + return RDY_OK; +} +#endif /* CH_CFG_USE_MAILBOXES */ + +/** @} */ diff --git a/os/rt/src/chmemcore.c b/os/rt/src/chmemcore.c new file mode 100644 index 000000000..d5e7ed79c --- /dev/null +++ b/os/rt/src/chmemcore.c @@ -0,0 +1,153 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chmemcore.c + * @brief Core memory manager code. + * + * @addtogroup memcore + * @details Core Memory Manager related APIs and services. + *

Operation mode

+ * The core memory manager is a simplified allocator that only + * allows to allocate memory blocks without the possibility to + * free them.
+ * This allocator is meant as a memory blocks provider for the + * other allocators such as: + * - C-Runtime allocator (through a compiler specific adapter module). + * - Heap allocator (see @ref heaps). + * - Memory pools allocator (see @ref pools). + * . + * By having a centralized memory provider the various allocators + * can coexist and share the main memory.
+ * This allocator, alone, is also useful for very simple + * applications that just require a simple way to get memory + * blocks. + * @pre In order to use the core memory manager APIs the @p CH_CFG_USE_MEMCORE + * option must be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_MEMCORE || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +static uint8_t *nextmem; +static uint8_t *endmem; + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Low level memory manager initialization. + * + * @notapi + */ +void _core_init(void) { +#if CH_CFG_MEMCORE_SIZE == 0 + extern uint8_t __heap_base__[]; + extern uint8_t __heap_end__[]; + + nextmem = (uint8_t *)MEM_ALIGN_NEXT(__heap_base__); + endmem = (uint8_t *)MEM_ALIGN_PREV(__heap_end__); +#else + static stkalign_t buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE)/MEM_ALIGN_SIZE]; + + nextmem = (uint8_t *)&buffer[0]; + endmem = (uint8_t *)&buffer[MEM_ALIGN_NEXT(CH_CFG_MEMCORE_SIZE)/MEM_ALIGN_SIZE]; +#endif +} + +/** + * @brief Allocates a memory block. + * @details The size of the returned block is aligned to the alignment + * type so it is not possible to allocate less + * than MEM_ALIGN_SIZE. + * + * @param[in] size the size of the block to be allocated + * @return A pointer to the allocated memory block. + * @retval NULL allocation failed, core memory exhausted. + * + * @api + */ +void *chCoreAlloc(size_t size) { + void *p; + + chSysLock(); + p = chCoreAllocI(size); + chSysUnlock(); + return p; +} + +/** + * @brief Allocates a memory block. + * @details The size of the returned block is aligned to the alignment + * type so it is not possible to allocate less than + * MEM_ALIGN_SIZE. + * + * @param[in] size the size of the block to be allocated. + * @return A pointer to the allocated memory block. + * @retval NULL allocation failed, core memory exhausted. + * + * @iclass + */ +void *chCoreAllocI(size_t size) { + void *p; + + chDbgCheckClassI(); + + size = MEM_ALIGN_NEXT(size); + if ((size_t)(endmem - nextmem) < size) + return NULL; + p = nextmem; + nextmem += size; + return p; +} + +/** + * @brief Core memory status. + * + * @return The size, in bytes, of the free core memory. + * + * @api + */ +size_t chCoreStatus(void) { + + return (size_t)(endmem - nextmem); +} +#endif /* CH_CFG_USE_MEMCORE */ + +/** @} */ diff --git a/os/rt/src/chmempools.c b/os/rt/src/chmempools.c new file mode 100644 index 000000000..d92ea7517 --- /dev/null +++ b/os/rt/src/chmempools.c @@ -0,0 +1,194 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chmempools.c + * @brief Memory Pools code. + * + * @addtogroup pools + * @details Memory Pools related APIs and services. + *

Operation mode

+ * The Memory Pools APIs allow to allocate/free fixed size objects in + * constant time and reliably without memory fragmentation + * problems.
+ * Memory Pools do not enforce any alignment constraint on the + * contained object however the objects must be properly aligned + * to contain a pointer to void. + * @pre In order to use the memory pools APIs the @p CH_CFG_USE_MEMPOOLS option + * must be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_MEMPOOLS || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes an empty memory pool. + * + * @param[out] mp pointer to a @p memory_pool_t structure + * @param[in] size the size of the objects contained in this memory pool, + * the minimum accepted size is the size of a pointer to + * void. + * @param[in] provider memory provider function for the memory pool or + * @p NULL if the pool is not allowed to grow + * automatically + * + * @init + */ +void chPoolObjectInit(memory_pool_t *mp, size_t size, memgetfunc_t provider) { + + chDbgCheck((mp != NULL) && (size >= sizeof(void *))); + + mp->mp_next = NULL; + mp->mp_object_size = size; + mp->mp_provider = provider; +} + +/** + * @brief Loads a memory pool with an array of static objects. + * @pre The memory pool must be already been initialized. + * @pre The array elements must be of the right size for the specified + * memory pool. + * @post The memory pool contains the elements of the input array. + * + * @param[in] mp pointer to a @p memory_pool_t structure + * @param[in] p pointer to the array first element + * @param[in] n number of elements in the array + * + * @api + */ +void chPoolLoadArray(memory_pool_t *mp, void *p, size_t n) { + + chDbgCheck((mp != NULL) && (n != 0)); + + while (n) { + chPoolAdd(mp, p); + p = (void *)(((uint8_t *)p) + mp->mp_object_size); + n--; + } +} + +/** + * @brief Allocates an object from a memory pool. + * @pre The memory pool must be already been initialized. + * + * @param[in] mp pointer to a @p memory_pool_t structure + * @return The pointer to the allocated object. + * @retval NULL if pool is empty. + * + * @iclass + */ +void *chPoolAllocI(memory_pool_t *mp) { + void *objp; + + chDbgCheckClassI(); + chDbgCheck(mp != NULL); + + if ((objp = mp->mp_next) != NULL) + mp->mp_next = mp->mp_next->ph_next; + else if (mp->mp_provider != NULL) + objp = mp->mp_provider(mp->mp_object_size); + return objp; +} + +/** + * @brief Allocates an object from a memory pool. + * @pre The memory pool must be already been initialized. + * + * @param[in] mp pointer to a @p memory_pool_t structure + * @return The pointer to the allocated object. + * @retval NULL if pool is empty. + * + * @api + */ +void *chPoolAlloc(memory_pool_t *mp) { + void *objp; + + chSysLock(); + objp = chPoolAllocI(mp); + chSysUnlock(); + return objp; +} + +/** + * @brief Releases an object into a memory pool. + * @pre The memory pool must be already been initialized. + * @pre The freed object must be of the right size for the specified + * memory pool. + * @pre The object must be properly aligned to contain a pointer to void. + * + * @param[in] mp pointer to a @p memory_pool_t structure + * @param[in] objp the pointer to the object to be released + * + * @iclass + */ +void chPoolFreeI(memory_pool_t *mp, void *objp) { + struct pool_header *php = objp; + + chDbgCheckClassI(); + chDbgCheck((mp != NULL) && (objp != NULL)); + + php->ph_next = mp->mp_next; + mp->mp_next = php; +} + +/** + * @brief Releases an object into a memory pool. + * @pre The memory pool must be already been initialized. + * @pre The freed object must be of the right size for the specified + * memory pool. + * @pre The object must be properly aligned to contain a pointer to void. + * + * @param[in] mp pointer to a @p memory_pool_t structure + * @param[in] objp the pointer to the object to be released + * + * @api + */ +void chPoolFree(memory_pool_t *mp, void *objp) { + + chSysLock(); + chPoolFreeI(mp, objp); + chSysUnlock(); +} + +#endif /* CH_CFG_USE_MEMPOOLS */ + +/** @} */ diff --git a/os/rt/src/chmsg.c b/os/rt/src/chmsg.c new file mode 100644 index 000000000..335b46c61 --- /dev/null +++ b/os/rt/src/chmsg.c @@ -0,0 +1,151 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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 || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if CH_CFG_USE_MESSAGES_PRIORITY +#define msg_insert(tp, qp) 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->p_msg = msg; + ctp->p_u.wtobjp = &tp->p_msgqueue; + msg_insert(ctp, &tp->p_msgqueue); + if (tp->p_state == CH_STATE_WTMSG) + chSchReadyI(tp); + chSchGoSleepS(CH_STATE_SNDMSGQ); + msg = ctp->p_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. + * + * @return A reference to the thread carrying the message. + * + * @api + */ +thread_t *chMsgWait(void) { + thread_t *tp; + + chSysLock(); + if (!chMsgIsPendingI(currp)) + chSchGoSleepS(CH_STATE_WTMSG); + tp = queue_fifo_remove(&currp->p_msgqueue); + tp->p_state = CH_STATE_SNDMSG; + chSysUnlock(); + 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->p_state == CH_STATE_SNDMSG, "invalid state"); + chMsgReleaseS(tp, msg); + chSysUnlock(); +} + +#endif /* CH_CFG_USE_MESSAGES */ + +/** @} */ diff --git a/os/rt/src/chmtx.c b/os/rt/src/chmtx.c new file mode 100644 index 000000000..0eb02e2b5 --- /dev/null +++ b/os/rt/src/chmtx.c @@ -0,0 +1,420 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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 are always performed in + * lock-reverse order. The unlock API does not even have a parameter, + * the mutex to unlock is selected from an internal, per-thread, stack + * of owned mutexes. This both improves the performance and is + * required for an efficient implementation of the priority + * inheritance mechanism. + * + *

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 || 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->m_queue); + mp->m_owner = NULL; +} + +/** + * @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->m_owner != NULL) { + /* 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->m_owner; + + /* Does the running thread have higher priority than the mutex + owning thread? */ + while (tp->p_prio < ctp->p_prio) { + /* Make priority of thread tp match the running thread's priority.*/ + tp->p_prio = ctp->p_prio; + + /* The following states need priority queues reordering.*/ + switch (tp->p_state) { + case CH_STATE_WTMTX: + /* Re-enqueues the mutex owner with its new priority.*/ + queue_prio_insert(queue_dequeue(tp), + (threads_queue_t *)tp->p_u.wtobjp); + tp = ((mutex_t *)tp->p_u.wtobjp)->m_owner; + continue; +#if CH_CFG_USE_CONDVARS | \ + (CH_CFG_USE_SEMAPHORES && CH_CFG_USE_SEMAPHORES_PRIORITY) | \ + (CH_CFG_USE_MESSAGES && CH_CFG_USE_MESSAGES_PRIORITY) +#if CH_CFG_USE_CONDVARS + case CH_STATE_WTCOND: +#endif +#if CH_CFG_USE_SEMAPHORES && CH_CFG_USE_SEMAPHORES_PRIORITY + case CH_STATE_WTSEM: +#endif +#if CH_CFG_USE_MESSAGES && CH_CFG_USE_MESSAGES_PRIORITY + case CH_STATE_SNDMSGQ: +#endif + /* Re-enqueues tp with its new priority on the queue.*/ + queue_prio_insert(queue_dequeue(tp), + (threads_queue_t *)tp->p_u.wtobjp); + break; +#endif + case CH_STATE_READY: +#if CH_DBG_ENABLE_ASSERTS + /* Prevents an assertion in chSchReadyI().*/ + tp->p_state = CH_STATE_CURRENT; +#endif + /* Re-enqueues tp with its new priority on the ready list.*/ + chSchReadyI(queue_dequeue(tp)); + break; + } + break; + } + + /* Sleep on the mutex.*/ + queue_prio_insert(ctp, &mp->m_queue); + ctp->p_u.wtobjp = mp; + chSchGoSleepS(CH_STATE_WTMTX); + + /* It is assumed that the thread performing the unlock operation assigns + the mutex to this thread.*/ + chDbgAssert(mp->m_owner == ctp, "not owner"); + chDbgAssert(ctp->p_mtxlist == mp, "not owned"); + } + else { + /* It was not owned, inserted in the owned mutexes list.*/ + mp->m_owner = ctp; + mp->m_next = ctp->p_mtxlist; + ctp->p_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->m_owner != NULL) + return false; + + mp->m_owner = currp; + mp->m_next = currp->p_mtxlist; + currp->p_mtxlist = mp; + return true; +} + +/** + * @brief Unlocks the next owned mutex in reverse lock order. + * @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. + * + * @return A pointer to the unlocked mutex. + * + * @api + */ +mutex_t *chMtxUnlock(void) { + thread_t *ctp = currp; + mutex_t *ump, *mp; + + chSysLock(); + + chDbgAssert(ctp->p_mtxlist != NULL, "owned mutexes list empty"); + chDbgAssert(ctp->p_mtxlist->m_owner == ctp, "ownership failure"); + + /* Removes the top mutex from the thread's owned mutexes list and marks it + as not owned.*/ + ump = ctp->p_mtxlist; + ctp->p_mtxlist = ump->m_next; + + /* If a thread is waiting on the mutex then the fun part begins.*/ + if (chMtxQueueNotEmptyS(ump)) { + thread_t *tp; + + /* Recalculates the optimal thread priority by scanning the owned + mutexes list.*/ + tprio_t newprio = ctp->p_realprio; + mp = ctp->p_mtxlist; + while (mp != 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(mp) && (mp->m_queue.p_next->p_prio > newprio)) + newprio = mp->m_queue.p_next->p_prio; + mp = mp->m_next; + } + + /* Assigns to the current thread the highest priority among all the + waiting threads.*/ + ctp->p_prio = newprio; + + /* Awakens the highest priority thread waiting for the unlocked mutex and + assigns the mutex to it.*/ + tp = queue_fifo_remove(&ump->m_queue); + ump->m_owner = tp; + ump->m_next = tp->p_mtxlist; + tp->p_mtxlist = ump; + chSchWakeupS(tp, RDY_OK); + } + else + ump->m_owner = NULL; + chSysUnlock(); + return ump; +} + +/** + * @brief Unlocks the next owned mutex in reverse lock order. + * @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. + * + * @return A pointer to the unlocked mutex. + * + * @sclass + */ +mutex_t *chMtxUnlockS(void) { + thread_t *ctp = currp; + mutex_t *ump, *mp; + + chDbgCheckClassS(); + chDbgAssert(ctp->p_mtxlist != NULL, "owned mutexes list empty"); + chDbgAssert(ctp->p_mtxlist->m_owner == ctp, "ownership failure"); + + /* Removes the top mutex from the owned mutexes list and marks it as not + owned.*/ + ump = ctp->p_mtxlist; + ctp->p_mtxlist = ump->m_next; + + /* If a thread is waiting on the mutex then the fun part begins.*/ + if (chMtxQueueNotEmptyS(ump)) { + thread_t *tp; + + /* Recalculates the optimal thread priority by scanning the owned + mutexes list.*/ + tprio_t newprio = ctp->p_realprio; + mp = ctp->p_mtxlist; + while (mp != 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(mp) && (mp->m_queue.p_next->p_prio > newprio)) + newprio = mp->m_queue.p_next->p_prio; + + mp = mp->m_next; + } + ctp->p_prio = newprio; + + /* Awakens the highest priority thread waiting for the unlocked mutex and + assigns the mutex to it.*/ + tp = queue_fifo_remove(&ump->m_queue); + ump->m_owner = tp; + ump->m_next = tp->p_mtxlist; + tp->p_mtxlist = ump; + chSchReadyI(tp); + } + else + ump->m_owner = NULL; + return ump; +} + +/** + * @brief Unlocks all the 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) { + thread_t *ctp = currp; + + chSysLock(); + if (ctp->p_mtxlist != NULL) { + do { + mutex_t *ump = ctp->p_mtxlist; + ctp->p_mtxlist = ump->m_next; + if (chMtxQueueNotEmptyS(ump)) { + thread_t *tp = queue_fifo_remove(&ump->m_queue); + ump->m_owner = tp; + ump->m_next = tp->p_mtxlist; + tp->p_mtxlist = ump; + chSchReadyI(tp); + } + else + ump->m_owner = NULL; + } while (ctp->p_mtxlist != NULL); + ctp->p_prio = ctp->p_realprio; + chSchRescheduleS(); + } + chSysUnlock(); +} + +#endif /* CH_CFG_USE_MUTEXES */ + +/** @} */ diff --git a/os/rt/src/chqueues.c b/os/rt/src/chqueues.c new file mode 100644 index 000000000..679d69337 --- /dev/null +++ b/os/rt/src/chqueues.c @@ -0,0 +1,455 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chqueues.c + * @brief I/O Queues code. + * + * @addtogroup io_queues + * @details ChibiOS/RT queues are mostly used in serial-like device drivers. + * The device drivers are usually designed to have a lower side + * (lower driver, it is usually an interrupt service routine) and an + * upper side (upper driver, accessed by the application threads).
+ * There are several kind of queues:
+ * - Input queue, unidirectional queue where the writer is the + * lower side and the reader is the upper side. + * - Output queue, unidirectional queue where the writer is the + * upper side and the reader is the lower side. + * - Full duplex queue, bidirectional queue. Full duplex queues + * are implemented by pairing an input queue and an output queue + * together. + * . + * @pre In order to use the I/O queues the @p CH_CFG_USE_QUEUES option must + * be enabled in @p chconf.h. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_QUEUES || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/** + * @brief Puts the invoking thread into the queue's threads queue. + * + * @param[out] qp pointer to an @p io_queue_t structure + * @param[in] time 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 threads queue. + * @retval Q_OK is the normal exit, thread signaled. + * @retval Q_RESET if the queue has been reset. + * @retval Q_TIMEOUT if the queue operation timed out. + */ +static msg_t qwait(io_queue_t *qp, systime_t time) { + + if (TIME_IMMEDIATE == time) + return Q_TIMEOUT; + currp->p_u.wtobjp = qp; + queue_insert(currp, &qp->q_waiting); + return chSchGoSleepTimeoutS(CH_STATE_WTQUEUE, time); +} + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Initializes an input queue. + * @details A Semaphore is internally initialized and works as a counter of + * the bytes contained in the queue. + * @note The callback is invoked from within the S-Locked system state, + * see @ref system_states. + * + * @param[out] iqp pointer to an @p input_queue_t structure + * @param[in] bp pointer to a memory area allocated as queue buffer + * @param[in] size size of the queue buffer + * @param[in] infy pointer to a callback function that is invoked when + * data is read from the queue. The value can be @p NULL. + * @param[in] link application defined pointer + * + * @init + */ +void chIQObjectInit(input_queue_t *iqp, uint8_t *bp, size_t size, + qnotify_t infy, void *link) { + + queue_init(&iqp->q_waiting); + iqp->q_counter = 0; + iqp->q_buffer = iqp->q_rdptr = iqp->q_wrptr = bp; + iqp->q_top = bp + size; + iqp->q_notify = infy; + iqp->q_link = link; +} + +/** + * @brief Resets an input queue. + * @details All the data in the input queue is erased and lost, any waiting + * thread is resumed with status @p Q_RESET. + * @note A reset operation can be used by a low level driver in order to + * obtain immediate attention from the high level layers. + * + * @param[in] iqp pointer to an @p input_queue_t structure + * + * @iclass + */ +void chIQResetI(input_queue_t *iqp) { + + chDbgCheckClassI(); + + iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer; + iqp->q_counter = 0; + while (queue_notempty(&iqp->q_waiting)) + chSchReadyI(queue_fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_RESET; +} + +/** + * @brief Input queue write. + * @details A byte value is written into the low end of an input queue. + * + * @param[in] iqp pointer to an @p input_queue_t structure + * @param[in] b the byte value to be written in the queue + * @return The operation status. + * @retval Q_OK if the operation has been completed with success. + * @retval Q_FULL if the queue is full and the operation cannot be + * completed. + * + * @iclass + */ +msg_t chIQPutI(input_queue_t *iqp, uint8_t b) { + + chDbgCheckClassI(); + + if (chIQIsFullI(iqp)) + return Q_FULL; + + iqp->q_counter++; + *iqp->q_wrptr++ = b; + if (iqp->q_wrptr >= iqp->q_top) + iqp->q_wrptr = iqp->q_buffer; + + if (queue_notempty(&iqp->q_waiting)) + chSchReadyI(queue_fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK; + + return Q_OK; +} + +/** + * @brief Input queue read with timeout. + * @details This function reads a byte value from an input queue. If the queue + * is empty then the calling thread is suspended until a byte arrives + * in the queue or a timeout occurs. + * @note The callback is invoked before reading the character from the + * buffer or before entering the state @p CH_STATE_WTQUEUE. + * + * @param[in] iqp pointer to an @p input_queue_t structure + * @param[in] time 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 byte value from the queue. + * @retval Q_TIMEOUT if the specified time expired. + * @retval Q_RESET if the queue has been reset. + * + * @api + */ +msg_t chIQGetTimeout(input_queue_t *iqp, systime_t time) { + uint8_t b; + + chSysLock(); + if (iqp->q_notify) + iqp->q_notify(iqp); + + while (chIQIsEmptyI(iqp)) { + msg_t msg; + if ((msg = qwait((io_queue_t *)iqp, time)) < Q_OK) { + chSysUnlock(); + return msg; + } + } + + iqp->q_counter--; + b = *iqp->q_rdptr++; + if (iqp->q_rdptr >= iqp->q_top) + iqp->q_rdptr = iqp->q_buffer; + + chSysUnlock(); + return b; +} + +/** + * @brief Input queue read with timeout. + * @details The function reads data from an input queue into a buffer. The + * operation completes when the specified amount of data has been + * transferred or after the specified timeout or if the queue has + * been reset. + * @note The function is not atomic, if you need atomicity it is suggested + * to use a semaphore or a mutex for mutual exclusion. + * @note The callback is invoked before reading each character from the + * buffer or before entering the state @p CH_STATE_WTQUEUE. + * + * @param[in] iqp pointer to an @p input_queue_t structure + * @param[out] bp pointer to the data buffer + * @param[in] n the maximum amount of data to be transferred, the + * value 0 is reserved + * @param[in] time 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 number of bytes effectively transferred. + * + * @api + */ +size_t chIQReadTimeout(input_queue_t *iqp, uint8_t *bp, + size_t n, systime_t time) { + qnotify_t nfy = iqp->q_notify; + size_t r = 0; + + chDbgCheck(n > 0); + + chSysLock(); + while (true) { + if (nfy) + nfy(iqp); + + while (chIQIsEmptyI(iqp)) { + if (qwait((io_queue_t *)iqp, time) != Q_OK) { + chSysUnlock(); + return r; + } + } + + iqp->q_counter--; + *bp++ = *iqp->q_rdptr++; + if (iqp->q_rdptr >= iqp->q_top) + iqp->q_rdptr = iqp->q_buffer; + + chSysUnlock(); /* Gives a preemption chance in a controlled point.*/ + r++; + if (--n == 0) + return r; + + chSysLock(); + } +} + +/** + * @brief Initializes an output queue. + * @details A Semaphore is internally initialized and works as a counter of + * the free bytes in the queue. + * @note The callback is invoked from within the S-Locked system state, + * see @ref system_states. + * + * @param[out] oqp pointer to an @p output_queue_t structure + * @param[in] bp pointer to a memory area allocated as queue buffer + * @param[in] size size of the queue buffer + * @param[in] onfy pointer to a callback function that is invoked when + * data is written to the queue. The value can be @p NULL. + * @param[in] link application defined pointer + * + * @init + */ +void chOQObjectInit(output_queue_t *oqp, uint8_t *bp, size_t size, + qnotify_t onfy, void *link) { + + queue_init(&oqp->q_waiting); + oqp->q_counter = size; + oqp->q_buffer = oqp->q_rdptr = oqp->q_wrptr = bp; + oqp->q_top = bp + size; + oqp->q_notify = onfy; + oqp->q_link = link; +} + +/** + * @brief Resets an output queue. + * @details All the data in the output queue is erased and lost, any waiting + * thread is resumed with status @p Q_RESET. + * @note A reset operation can be used by a low level driver in order to + * obtain immediate attention from the high level layers. + * + * @param[in] oqp pointer to an @p output_queue_t structure + * + * @iclass + */ +void chOQResetI(output_queue_t *oqp) { + + chDbgCheckClassI(); + + oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer; + oqp->q_counter = chQSizeI(oqp); + while (queue_notempty(&oqp->q_waiting)) + chSchReadyI(queue_fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_RESET; +} + +/** + * @brief Output queue write with timeout. + * @details This function writes a byte value to an output queue. If the queue + * is full then the calling thread is suspended until there is space + * in the queue or a timeout occurs. + * @note The callback is invoked after writing the character into the + * buffer. + * + * @param[in] oqp pointer to an @p output_queue_t structure + * @param[in] b the byte value to be written in the queue + * @param[in] time 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 operation status. + * @retval Q_OK if the operation succeeded. + * @retval Q_TIMEOUT if the specified time expired. + * @retval Q_RESET if the queue has been reset. + * + * @api + */ +msg_t chOQPutTimeout(output_queue_t *oqp, uint8_t b, systime_t time) { + + chSysLock(); + while (chOQIsFullI(oqp)) { + msg_t msg; + + if ((msg = qwait((io_queue_t *)oqp, time)) < Q_OK) { + chSysUnlock(); + return msg; + } + } + + oqp->q_counter--; + *oqp->q_wrptr++ = b; + if (oqp->q_wrptr >= oqp->q_top) + oqp->q_wrptr = oqp->q_buffer; + + if (oqp->q_notify) + oqp->q_notify(oqp); + + chSysUnlock(); + return Q_OK; +} + +/** + * @brief Output queue read. + * @details A byte value is read from the low end of an output queue. + * + * @param[in] oqp pointer to an @p output_queue_t structure + * @return The byte value from the queue. + * @retval Q_EMPTY if the queue is empty. + * + * @iclass + */ +msg_t chOQGetI(output_queue_t *oqp) { + uint8_t b; + + chDbgCheckClassI(); + + if (chOQIsEmptyI(oqp)) + return Q_EMPTY; + + oqp->q_counter++; + b = *oqp->q_rdptr++; + if (oqp->q_rdptr >= oqp->q_top) + oqp->q_rdptr = oqp->q_buffer; + + if (queue_notempty(&oqp->q_waiting)) + chSchReadyI(queue_fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK; + + return b; +} + +/** + * @brief Output queue write with timeout. + * @details The function writes data from a buffer to an output queue. The + * operation completes when the specified amount of data has been + * transferred or after the specified timeout or if the queue has + * been reset. + * @note The function is not atomic, if you need atomicity it is suggested + * to use a semaphore or a mutex for mutual exclusion. + * @note The callback is invoked after writing each character into the + * buffer. + * + * @param[in] oqp pointer to an @p output_queue_t structure + * @param[out] bp pointer to the data buffer + * @param[in] n the maximum amount of data to be transferred, the + * value 0 is reserved + * @param[in] time 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 number of bytes effectively transferred. + * + * @api + */ +size_t chOQWriteTimeout(output_queue_t *oqp, const uint8_t *bp, + size_t n, systime_t time) { + qnotify_t nfy = oqp->q_notify; + size_t w = 0; + + chDbgCheck(n > 0); + + chSysLock(); + while (true) { + while (chOQIsFullI(oqp)) { + if (qwait((io_queue_t *)oqp, time) != Q_OK) { + chSysUnlock(); + return w; + } + } + oqp->q_counter--; + *oqp->q_wrptr++ = *bp++; + if (oqp->q_wrptr >= oqp->q_top) + oqp->q_wrptr = oqp->q_buffer; + + if (nfy) + nfy(oqp); + + chSysUnlock(); /* Gives a preemption chance in a controlled point.*/ + w++; + if (--n == 0) + return w; + chSysLock(); + } +} +#endif /* CH_CFG_USE_QUEUES */ + +/** @} */ diff --git a/os/rt/src/chregistry.c b/os/rt/src/chregistry.c new file mode 100644 index 000000000..685d0c109 --- /dev/null +++ b/os/rt/src/chregistry.c @@ -0,0 +1,175 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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 "ch.h" + +#if CH_CFG_USE_REGISTRY || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#define _offsetof(st, m) \ + ((size_t)((char *)&((st *)0)->m - (char *)0)) + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/* + * OS signature in ROM plus debug-related information. + */ +ROMCONST chdebug_t ch_debug = { + "main", + (uint8_t)0, + (uint8_t)sizeof (chdebug_t), + (uint16_t)((CH_KERNEL_MAJOR << 11) | + (CH_KERNEL_MINOR << 6) | + (CH_KERNEL_PATCH) << 0), + (uint8_t)sizeof (void *), + (uint8_t)sizeof (systime_t), + (uint8_t)sizeof (thread_t), + (uint8_t)_offsetof(thread_t, p_prio), + (uint8_t)_offsetof(thread_t, p_ctx), + (uint8_t)_offsetof(thread_t, p_newer), + (uint8_t)_offsetof(thread_t, p_older), + (uint8_t)_offsetof(thread_t, p_name), +#if CH_DBG_ENABLE_STACK_CHECK + (uint8_t)_offsetof(thread_t, p_stklimit), +#else + (uint8_t)0, +#endif + (uint8_t)_offsetof(thread_t, p_state), + (uint8_t)_offsetof(thread_t, p_flags), +#if CH_CFG_USE_DYNAMIC + (uint8_t)_offsetof(thread_t, p_refs), +#else + (uint8_t)0, +#endif +#if CH_CFG_TIME_QUANTUM > 0 + (uint8_t)_offsetof(thread_t, p_preempt), +#else + (uint8_t)0, +#endif +#if CH_DBG_THREADS_PROFILING + (uint8_t)_offsetof(thread_t, p_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.r_newer; +#if CH_CFG_USE_DYNAMIC + tp->p_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->p_newer; + if (ntp == (thread_t *)&ch.rlist) + ntp = NULL; +#if CH_CFG_USE_DYNAMIC + else { + chDbgAssert(ntp->p_refs < 255, "too many references"); + ntp->p_refs++; + } +#endif + chSysUnlock(); +#if CH_CFG_USE_DYNAMIC + chThdRelease(tp); +#endif + return ntp; +} + +#endif /* CH_CFG_USE_REGISTRY */ + +/** @} */ diff --git a/os/rt/src/chschd.c b/os/rt/src/chschd.c new file mode 100644 index 000000000..615008589 --- /dev/null +++ b/os/rt/src/chschd.c @@ -0,0 +1,372 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Scheduler initialization. + * + * @notapi + */ +void _scheduler_init(void) { + + queue_init(&ch.rlist.r_queue); + ch.rlist.r_prio = NOPRIO; +#if CH_CFG_USE_REGISTRY + ch.rlist.r_newer = ch.rlist.r_older = (thread_t *)&ch.rlist; +#endif +} + +/** + * @brief Inserts a thread in the Ready List. + * @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 p_next and @p 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(); + + /* Integrity checks.*/ + chDbgAssert((tp->p_state != CH_STATE_READY) && + (tp->p_state != CH_STATE_FINAL), + "invalid state"); + + tp->p_state = CH_STATE_READY; + cp = (thread_t *)&ch.rlist.r_queue; + do { + cp = cp->p_next; + } while (cp->p_prio >= tp->p_prio); + /* Insertion on p_prev.*/ + tp->p_next = cp; + tp->p_prev = cp->p_prev; + tp->p_prev->p_next = cp->p_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; + + chDbgCheckClassS(); + + (otp = currp)->p_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->p_preempt = CH_CFG_TIME_QUANTUM; +#endif + setcurrp(queue_fifo_remove(&ch.rlist.r_queue)); + currp->p_state = CH_STATE_CURRENT; + chSysSwitch(currp, otp); +} + +/* + * Timeout wakeup callback. + */ +static void wakeup(void *p) { + thread_t *tp = (thread_t *)p; + + chSysLockFromISR(); + switch (tp->p_state) { + case CH_STATE_READY: + /* Handling the special case where the thread has been made ready by + another thread with higher priority.*/ + chSysUnlockFromISR(); + return; +#if CH_CFG_USE_SEMAPHORES || CH_CFG_USE_QUEUES || \ + (CH_CFG_USE_CONDVARS && CH_CFG_USE_CONDVARS_TIMEOUT) +#if CH_CFG_USE_SEMAPHORES + case CH_STATE_WTSEM: + chSemFastSignalI((semaphore_t *)tp->p_u.wtobjp); + /* Falls into, intentional. */ +#endif +#if CH_CFG_USE_QUEUES + case CH_STATE_WTQUEUE: +#endif +#if CH_CFG_USE_CONDVARS && CH_CFG_USE_CONDVARS_TIMEOUT + case CH_STATE_WTCOND: +#endif + /* States requiring dequeuing.*/ + queue_dequeue(tp); +#endif + } + tp->p_u.rdymsg = RDY_TIMEOUT; + 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 RDY_TIMEOUT low level message. The possible + * @ref thread_states are defined into @p threads.h. + * + * @param[in] newstate the new thread state + * @param[in] time 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 RDY_TIMEOUT if a timeout occurs. + * + * @sclass + */ +msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) { + + chDbgCheckClassS(); + + if (TIME_INFINITE != time) { + virtual_timer_t vt; + + chVTDoSetI(&vt, time, wakeup, currp); + chSchGoSleepS(newstate); + if (chVTIsArmedI(&vt)) + chVTDoResetI(&vt); + } + else + chSchGoSleepS(newstate); + return currp->p_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 p_next and @p 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 message to the awakened thread + * + * @sclass + */ +void chSchWakeupS(thread_t *ntp, msg_t msg) { + + chDbgCheckClassS(); + + ntp->p_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->p_prio <= currp->p_prio) + chSchReadyI(ntp); + else { + thread_t *otp = chSchReadyI(currp); + setcurrp(ntp); + ntp->p_state = CH_STATE_CURRENT; + 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(); +} + +/** + * @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.r_queue); + tprio_t p2 = currp->p_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->p_preempt ? p1 > p2 : p1 >= p2; +#else + /* If the round robin preemption feature is not enabled then performs a + simpler comparison.*/ + return p1 > p2; +#endif +} + +/** + * @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 or from within the port layer. + * + * @special + */ +void chSchDoRescheduleBehind(void) { + thread_t *otp; + + otp = currp; + /* Picks the first thread from the ready queue and makes it current.*/ + setcurrp(queue_fifo_remove(&ch.rlist.r_queue)); + currp->p_state = CH_STATE_CURRENT; +#if CH_CFG_TIME_QUANTUM > 0 + otp->p_preempt = CH_CFG_TIME_QUANTUM; +#endif + chSchReadyI(otp); + 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 or from within the port layer. + * + * @special + */ +void chSchDoRescheduleAhead(void) { + thread_t *otp, *cp; + + otp = currp; + /* Picks the first thread from the ready queue and makes it current.*/ + setcurrp(queue_fifo_remove(&ch.rlist.r_queue)); + currp->p_state = CH_STATE_CURRENT; + + otp->p_state = CH_STATE_READY; + cp = (thread_t *)&ch.rlist.r_queue; + do { + cp = cp->p_next; + } while (cp->p_prio > otp->p_prio); + /* Insertion on p_prev.*/ + otp->p_next = cp; + otp->p_prev = cp->p_prev; + otp->p_prev->p_next = cp->p_prev = otp; + + chSysSwitch(currp, otp); +} + +/** + * @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) { + +#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 (currp->p_preempt == 0) { + /* The thread consumed its time quantum so it is enqueued behind threads + with same priority level, however, it acquires a new time quantum.*/ + chSchDoRescheduleBehind(); + } + 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.*/ + chSchDoRescheduleAhead(); + } +#else /* !(CH_CFG_TIME_QUANTUM > 0) */ + /* If the round-robin mechanism is disabled then the thread goes always + ahead of its peers.*/ + chSchDoRescheduleAhead(); +#endif /* !(CH_CFG_TIME_QUANTUM > 0) */ +} + +/** @} */ diff --git a/os/rt/src/chsem.c b/os/rt/src/chsem.c new file mode 100644 index 000000000..388e50bc6 --- /dev/null +++ b/os/rt/src/chsem.c @@ -0,0 +1,401 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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 || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if CH_CFG_USE_SEMAPHORES_PRIORITY +#define sem_insert(tp, qp) 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 >= 0)); + + queue_init(&sp->s_queue); + sp->s_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. + * @note The released threads can recognize they were waked up by a reset + * rather than a signal because the @p chSemWait() will return + * @p RDY_RESET instead of @p RDY_OK. + * + * @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. + * + * @api + */ +void chSemReset(semaphore_t *sp, cnt_t n) { + + chSysLock(); + chSemResetI(sp, n); + 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. + * @note The released threads can recognize they were waked up by a reset + * rather than a signal because the @p chSemWait() will return + * @p RDY_RESET instead of @p RDY_OK. + * + * @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. + * + * @iclass + */ +void chSemResetI(semaphore_t *sp, cnt_t n) { + cnt_t cnt; + + chDbgCheckClassI(); + chDbgCheck((sp != NULL) && (n >= 0)); + chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + cnt = sp->s_cnt; + sp->s_cnt = n; + while (++cnt <= 0) + chSchReadyI(queue_lifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_RESET; +} + +/** + * @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 RDY_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval RDY_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 RDY_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval RDY_RESET if the semaphore has been reset using @p chSemReset(). + * + * @sclass + */ +msg_t chSemWaitS(semaphore_t *sp) { + + chDbgCheckClassS(); + chDbgCheck(sp != NULL); + chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + if (--sp->s_cnt < 0) { + currp->p_u.wtobjp = sp; + sem_insert(currp, &sp->s_queue); + chSchGoSleepS(CH_STATE_WTSEM); + return currp->p_u.rdymsg; + } + return RDY_OK; +} + +/** + * @brief Performs a wait operation on a semaphore with timeout specification. + * + * @param[in] sp pointer to a @p semaphore_t structure + * @param[in] time 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 RDY_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval RDY_RESET if the semaphore has been reset using @p chSemReset(). + * @retval RDY_TIMEOUT if the semaphore has not been signaled or reset within + * the specified timeout. + * + * @api + */ +msg_t chSemWaitTimeout(semaphore_t *sp, systime_t time) { + msg_t msg; + + chSysLock(); + msg = chSemWaitTimeoutS(sp, time); + 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] time 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 RDY_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval RDY_RESET if the semaphore has been reset using @p chSemReset(). + * @retval RDY_TIMEOUT if the semaphore has not been signaled or reset within + * the specified timeout. + * + * @sclass + */ +msg_t chSemWaitTimeoutS(semaphore_t *sp, systime_t time) { + + chDbgCheckClassS(); + chDbgCheck(sp != NULL); + chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + if (--sp->s_cnt < 0) { + if (TIME_IMMEDIATE == time) { + sp->s_cnt++; + return RDY_TIMEOUT; + } + currp->p_u.wtobjp = sp; + sem_insert(currp, &sp->s_queue); + return chSchGoSleepTimeoutS(CH_STATE_WTSEM, time); + } + return RDY_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); + chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + chSysLock(); + if (++sp->s_cnt <= 0) + chSchWakeupS(queue_fifo_remove(&sp->s_queue), RDY_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->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + if (++sp->s_cnt <= 0) { + /* Note, it is done this way in order to allow a tail call on + chSchReadyI().*/ + thread_t *tp = queue_fifo_remove(&sp->s_queue); + tp->p_u.rdymsg = RDY_OK; + 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 > 0)); + chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) || + ((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)), + "inconsistent semaphore"); + + while (n > 0) { + if (++sp->s_cnt <= 0) + chSchReadyI(queue_fifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_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 RDY_OK if the thread has not stopped on the semaphore or the + * semaphore has been signaled. + * @retval RDY_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)); + chDbgAssert(((sps->s_cnt >= 0) && queue_isempty(&sps->s_queue)) || + ((sps->s_cnt < 0) && queue_notempty(&sps->s_queue)), + "inconsistent semaphore"); + chDbgAssert(((spw->s_cnt >= 0) && queue_isempty(&spw->s_queue)) || + ((spw->s_cnt < 0) && queue_notempty(&spw->s_queue)), + "inconsistent semaphore"); + + chSysLock(); + if (++sps->s_cnt <= 0) + chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK; + if (--spw->s_cnt < 0) { + thread_t *ctp = currp; + sem_insert(ctp, &spw->s_queue); + ctp->p_u.wtobjp = spw; + chSchGoSleepS(CH_STATE_WTSEM); + msg = ctp->p_u.rdymsg; + } + else { + chSchRescheduleS(); + msg = RDY_OK; + } + chSysUnlock(); + return msg; +} + +#endif /* CH_CFG_USE_SEMAPHORES */ + +/** @} */ diff --git a/os/rt/src/chstats.c b/os/rt/src/chstats.c new file mode 100644 index 000000000..653d8da77 --- /dev/null +++ b/os/rt/src/chstats.c @@ -0,0 +1,122 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chstats.c + * @brief Statistics module code. + * + * @addtogroup statistics + * @details Statistics services. + * @{ + */ + +#include "ch.h" + +#if CH_DBG_STATISTICS || 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 = 0; + ch.kernel_stats.n_ctxswc = 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) { + + ch.kernel_stats.n_irq++; +} + +/** + * @brief Updates context switch related statistics. + */ +void _stats_ctxswc(thread_t *ntp, thread_t *otp) { + + ch.kernel_stats.n_ctxswc++; + chTMChainMeasurementToX(&otp->p_stats, &ntp->p_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 */ + +/** @} */ diff --git a/os/rt/src/chsys.c b/os/rt/src/chsys.c new file mode 100644 index 000000000..9212cbf5f --- /dev/null +++ b/os/rt/src/chsys.c @@ -0,0 +1,298 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/** + * @brief System data structures. + */ +ch_system_t ch; + +#if !CH_CFG_NO_IDLE_THREAD || defined(__DOXYGEN__) +/** + * @brief Idle thread working area. + */ +static WORKING_AREA(_idle_thread_wa, CH_PORT_IDLE_THREAD_STACK_SIZE); +#endif /* CH_CFG_NO_IDLE_THREAD */ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +#if !CH_CFG_NO_IDLE_THREAD || 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; + chRegSetThreadName("idle"); + while (true) { + port_wait_for_interrupt(); + CH_CFG_IDLE_LOOP_HOOK(); + } +} +#endif /* CH_CFG_NO_IDLE_THREAD */ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief ChibiOS/RT initialization. + * @details After executing this function the current instructions stream + * becomes the main thread. + * @pre Interrupts must be still disabled when @p chSysInit() is invoked + * and are internally enabled. + * @post The main thread is created with priority @p NORMALPRIO. + * @note This function has special, architecture-dependent, requirements, + * see the notes into the various port reference manuals. + * + * @special + */ +void chSysInit(void) { + static thread_t mainthread; +#if CH_DBG_ENABLE_STACK_CHECK + extern stkalign_t __main_thread_stack_base__; +#endif + + port_init(); + _scheduler_init(); + _vt_init(); +#if CH_CFG_USE_TM + _tm_init(); +#endif +#if CH_CFG_USE_MEMCORE + _core_init(); +#endif +#if CH_CFG_USE_HEAP + _heap_init(); +#endif +#if CH_DBG_STATISTICS + _stats_init(); +#endif +#if CH_DBG_ENABLE_TRACE + _trace_init(); +#endif + +#if !CH_CFG_NO_IDLE_THREAD + /* Now this instructions flow becomes the main thread.*/ + setcurrp(_thread_init(&mainthread, NORMALPRIO)); +#else + /* Now this instructions flow becomes the main thread.*/ + setcurrp(_thread_init(&mainthread, IDLEPRIO)); +#endif + + currp->p_state = CH_STATE_CURRENT; +#if CH_DBG_ENABLE_STACK_CHECK + /* This is a special case because the main thread thread_t structure is not + adjacent to its stack area.*/ + currp->p_stklimit = &__main_thread_stack_base__; +#endif + chSysEnable(); + + /* Note, &ch_debug points to the string "main" if the registry is + active, else the parameter is ignored.*/ + chRegSetThreadName((const char *)&ch_debug); + +#if !CH_CFG_NO_IDLE_THREAD + /* 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.*/ + chThdCreateStatic(_idle_thread_wa, sizeof(_idle_thread_wa), IDLEPRIO, + (tfunc_t)_idle_thread, NULL); +#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. + * + * @special + */ +void chSysHalt(void) { + + port_disable(); + +#if defined(CH_CFG_SYSTEM_HALT_HOOK) || defined(__DOXYGEN__) + CH_CFG_SYSTEM_HALT_HOOK(); +#endif + + /* Harmless infinite loop.*/ + while (true) + ; +} + +/** + * @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->p_preempt > 0) + /* Decrement remaining quantum.*/ + currp->p_preempt--; +#endif +#if CH_DBG_THREADS_PROFILING + currp->p_time++; +#endif + chVTDoTickI(); +#if defined(CH_CFG_SYSTEM_TICK_HOOK) + CH_CFG_SYSTEM_TICK_HOOK(); +#endif +} + + +/** + * @brief Returns the execution context and enters the kernel lock mode. + * @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. + * + * @return The previous system status, the encoding of this + * status word is architecture-dependent and opaque. + * + * @xclass + */ +syssts_t chSysGetAndLockX(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. + * + * @param[in] sts the system status to be restored. + * + * @xclass + */ +void chSysRestoreLockX(syssts_t sts) { + + if (port_irq_enabled(sts)) { + if (port_is_isr_context()) + chSysUnlockFromISR(); + else + chSysUnlock(); + } +} + +#if CH_PORT_SUPPORTS_RT || 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 true because the + * whole time range is specified. + * @note This function is only available if the port layer supports the + * option @p CH_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 end > start ? (cnt >= start) && (cnt < end) : + (cnt >= start) || (cnt < end); +} + +/** + * @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 CH_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 /* CH_PORT_SUPPORTS_RT */ + +/** @} */ diff --git a/os/rt/src/chthreads.c b/os/rt/src/chthreads.c new file mode 100644 index 000000000..073b6eed7 --- /dev/null +++ b/os/rt/src/chthreads.c @@ -0,0 +1,462 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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. + * . + * The threads subsystem is implicitly included in kernel however + * some of its part may be excluded by disabling them in @p chconf.h, + * see the @p CH_CFG_USE_WAITEXIT and @p CH_CFG_USE_DYNAMIC configuration + * options. + * @{ + */ + +#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] prio the priority level for the new thread + * @return The same thread pointer passed as parameter. + * + * @notapi + */ +thread_t *_thread_init(thread_t *tp, tprio_t prio) { + + tp->p_prio = prio; + tp->p_state = CH_STATE_SUSPENDED; + tp->p_flags = CH_FLAG_MODE_STATIC; +#if CH_CFG_TIME_QUANTUM > 0 + tp->p_preempt = CH_CFG_TIME_QUANTUM; +#endif +#if CH_CFG_USE_MUTEXES + tp->p_realprio = prio; + tp->p_mtxlist = NULL; +#endif +#if CH_CFG_USE_EVENTS + tp->p_epending = 0; +#endif +#if CH_DBG_THREADS_PROFILING + tp->p_time = 0; +#endif +#if CH_CFG_USE_DYNAMIC + tp->p_refs = 1; +#endif +#if CH_CFG_USE_REGISTRY + tp->p_name = NULL; + REG_INSERT(tp); +#endif +#if CH_CFG_USE_WAITEXIT + list_init(&tp->p_waiting); +#endif +#if CH_CFG_USE_MESSAGES + queue_init(&tp->p_msgqueue); +#endif +#if CH_DBG_ENABLE_STACK_CHECK + tp->p_stklimit = (stkalign_t *)(tp + 1); +#endif +#if CH_DBG_STATISTICS || defined(__DOXYGEN__) + chTMStartMeasurementX(&tp->p_stats); +#endif +#if defined(CH_CFG_THREAD_INIT_HOOK) + CH_CFG_THREAD_INIT_HOOK(tp); +#endif + return tp; +} + +#if CH_DBG_FILL_THREADS || 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_SUSPENDED. + * @post The initialized thread can be subsequently started by invoking + * @p chThdResume(), @p chThdResumeI() 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] 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. + * + * @iclass + */ +thread_t *chThdCreateI(void *wsp, size_t size, + tprio_t prio, tfunc_t pf, void *arg) { + /* The thread structure is laid out in the lower part of the thread + workspace.*/ + thread_t *tp = wsp; + + chDbgCheckClassI(); + + chDbgCheck((wsp != NULL) && (size >= THD_WA_SIZE(0)) && + (prio <= HIGHPRIO) && (pf != NULL)); + SETUP_CONTEXT(wsp, size, pf, arg); + return _thread_init(tp, prio); +} + +/** + * @brief Creates a new thread into a static memory area. + * @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; + +#if CH_DBG_FILL_THREADS + _thread_memfill((uint8_t *)wsp, + (uint8_t *)wsp + sizeof(thread_t), + CH_DBG_THREAD_FILL_VALUE); + _thread_memfill((uint8_t *)wsp + sizeof(thread_t), + (uint8_t *)wsp + size, + CH_DBG_STACK_FILL_VALUE); +#endif + chSysLock(); + chSchWakeupS(tp = chThdCreateI(wsp, size, prio, pf, arg), RDY_OK); + chSysUnlock(); + return tp; +} + +/** + * @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 + oldprio = currp->p_realprio; + if ((currp->p_prio == currp->p_realprio) || (newprio > currp->p_prio)) + currp->p_prio = newprio; + currp->p_realprio = newprio; +#else + oldprio = currp->p_prio; + currp->p_prio = newprio; +#endif + chSchRescheduleS(); + chSysUnlock(); + return oldprio; +} + +/** + * @brief Resumes a suspended thread. + * @pre The specified thread pointer must refer to an initialized thread + * in the @p CH_STATE_SUSPENDED state. + * @post The specified thread is immediately started or put in the ready + * list depending on the relative priority levels. + * @note Use this function to start threads created with @p chThdCreateI(). + * + * @param[in] tp pointer to the thread + * @return The pointer to the thread. + * + * @api + */ +thread_t *chThdResume(thread_t *tp) { + + chSysLock(); + chDbgAssert(tp->p_state == CH_STATE_SUSPENDED, + "thread not in CH_STATE_SUSPENDED state"); + chSchWakeupS(tp, RDY_OK); + chSysUnlock(); + return tp; +} + +/** + * @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->p_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(systime_t time) { + + chDbgCheck(time != TIME_IMMEDIATE); + + chSysLock(); + chThdSleepS(time); + chSysUnlock(); +} + +/** + * @brief Suspends the invoking thread until the system time arrives to the + * specified value. + * + * @param[in] time absolute system time + * + * @api + */ +void chThdSleepUntil(systime_t time) { + + chSysLock(); + if ((time -= chVTGetSystemTimeX()) > 0) + chThdSleepS(time); + chSysUnlock(); +} + +/** + * @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 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 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; + + tp->p_u.exitcode = msg; +#if defined(CH_CFG_THREAD_EXIT_HOOK) + CH_CFG_THREAD_EXIT_HOOK(tp); +#endif +#if CH_CFG_USE_WAITEXIT + while (list_notempty(&tp->p_waiting)) + chSchReadyI(list_remove(&tp->p_waiting)); +#endif +#if CH_CFG_USE_REGISTRY + /* Static threads are immediately removed from the registry because + there is no memory to recover.*/ + if ((tp->p_flags & CH_FLAG_MODE_MASK) == CH_FLAG_MODE_STATIC) + REG_REMOVE(tp); +#endif + chSchGoSleepS(CH_STATE_FINAL); + /* The thread never returns here.*/ + chDbgAssert(false, "zombies apocalypse"); +} + +#if CH_CFG_USE_WAITEXIT || 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.
+ * The memory used by the exited thread is handled in different ways + * depending on the API that spawned the thread: + * - If the thread was spawned by @p chThdCreateStatic() or by + * @p chThdCreateI() then nothing happens and the thread working + * area is not released or modified in any way. This is the + * default, totally static, behavior. + * - If the thread was spawned by @p chThdCreateFromHeap() then + * the working area is returned to the system heap. + * - If the thread was spawned by @p chThdCreateFromMemoryPool() + * then the working area is returned to the owning memory pool. + * . + * @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. + * @post After invoking @p chThdWait() the thread pointer becomes invalid + * and must not be used as parameter for further system calls. + * @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_DYNAMIC + chDbgAssert(tp->p_refs > 0, "not referenced"); +#endif + if (tp->p_state != CH_STATE_FINAL) { + list_insert(currp, &tp->p_waiting); + chSchGoSleepS(CH_STATE_WTEXIT); + } + msg = tp->p_u.exitcode; + chSysUnlock(); +#if CH_CFG_USE_DYNAMIC + chThdRelease(tp); +#endif + return msg; +} +#endif /* CH_CFG_USE_WAITEXIT */ + +/** @} */ diff --git a/os/rt/src/chtm.c b/os/rt/src/chtm.c new file mode 100644 index 000000000..24239e0e2 --- /dev/null +++ b/os/rt/src/chtm.c @@ -0,0 +1,155 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file chtm.c + * @brief Time Measurement module code. + * + * @addtogroup time_measurement + * @details Time Measurement APIs and services. + * @{ + */ + +#include "ch.h" + +#if CH_CFG_USE_TM || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Module local definitions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* 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; + else 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; + + /* Time Measurement subsystem calibration, it does a null measurement + and calculates the call overhead which is subtracted to real + measurements.*/ + ch.measurement_offset = 0; + chTMObjectInit(&tm); + chTMStartMeasurementX(&tm); + chTMStopMeasurementX(&tm); + ch.measurement_offset = tm.last; +} + +/** + * @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.measurement_offset); +} + +#endif /* CH_CFG_USE_TM */ + +/** + * @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, 0); +} + +/** @} */ diff --git a/os/rt/src/chvt.c b/os/rt/src/chvt.c new file mode 100644 index 000000000..3f1559ac4 --- /dev/null +++ b/os/rt/src/chvt.c @@ -0,0 +1,211 @@ +/* + ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, + 2011,2012,2013 Giovanni Di Sirio. + + This file is part of ChibiOS/RT. + + ChibiOS/RT is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS/RT is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file 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. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local variables. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Module exported functions. */ +/*===========================================================================*/ + +/** + * @brief Virtual Timers initialization. + * @note Internal use only. + * + * @notapi + */ +void _vt_init(void) { + + ch.vtlist.vt_next = ch.vtlist.vt_prev = (void *)&ch.vtlist; + ch.vtlist.vt_delta = (systime_t)-1; +#if CH_CFG_TIMEDELTA == 0 + ch.vtlist.vt_systime = 0; +#else /* CH_CFG_TIMEDELTA > 0 */ + ch.vtlist.vt_lasttime = 0; +#endif /* CH_CFG_TIMEDELTA > 0 */ +} + +/** + * @brief Checks if the current system time is within the specified time + * window. + * @note When start==end then the function returns always true because the + * whole time range is specified. + * @note This function can be called from any context. + * + * @param[in] time the time to be verified + * @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 chVTIsTimeWithinX(systime_t time, systime_t start, systime_t end) { + + return end > start ? (time >= start) && (time < end) : + (time >= start) || (time < end); +} + +/** + * @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, systime_t delay, + vtfunc_t vtfunc, void *par) { + virtual_timer_t *p; + + chDbgCheckClassI(); + chDbgCheck((vtp != NULL) && (vtfunc != NULL) && (delay != TIME_IMMEDIATE)); + + vtp->vt_par = par; + vtp->vt_func = vtfunc; + p = ch.vtlist.vt_next; + +#if CH_CFG_TIMEDELTA > 0 || defined(__DOXYGEN__) + { + systime_t now = port_timer_get_time(); + + /* If the requested delay is lower than the minimum safe delta then it + is raised to the minimum safe value.*/ + if (delay < CH_CFG_TIMEDELTA) + delay = CH_CFG_TIMEDELTA; + + if (&ch.vtlist == (virtual_timers_list_t *)p) { + /* The delta list is empty, the current time becomes the new + delta list base time.*/ + ch.vtlist.vt_lasttime = now; + port_timer_start_alarm(ch.vtlist.vt_lasttime + delay); + } + else { + /* Now the delay is calculated as delta from the last tick interrupt + time.*/ + delay += now - ch.vtlist.vt_lasttime; + + /* If the specified delay is closer in time than the first element + in the delta list then it becomes the next alarm event in time.*/ + if (delay < p->vt_delta) + port_timer_set_alarm(ch.vtlist.vt_lasttime + delay); + } + } +#endif /* CH_CFG_TIMEDELTA > 0 */ + + /* The delta list is scanned in order to find the correct position for + this timer. */ + while (p->vt_delta < delay) { + delay -= p->vt_delta; + p = p->vt_next; + } + /* The timer is inserted in the delta list.*/ + vtp->vt_prev = (vtp->vt_next = p)->vt_prev; + vtp->vt_prev->vt_next = p->vt_prev = vtp; + vtp->vt_delta = delay + + /* Special case when the timer is in last position in the list, the + value in the header must be restored.*/; + p->vt_delta -= delay; + ch.vtlist.vt_delta = (systime_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) { + + chDbgCheckClassI(); + chDbgCheck(vtp != NULL); + chDbgAssert(vtp->vt_func != NULL, "timer not set or already triggered"); + + /* Removing the element from the delta list.*/ + vtp->vt_next->vt_delta += vtp->vt_delta; + vtp->vt_prev->vt_next = vtp->vt_next; + vtp->vt_next->vt_prev = vtp->vt_prev; + vtp->vt_func = (vtfunc_t)NULL; + + /* The above code changes the value in the header when the removed element + is the last of the list, restoring it.*/ + ch.vtlist.vt_delta = (systime_t)-1; + +#if CH_CFG_TIMEDELTA > 0 || defined(__DOXYGEN__) + { + if (&ch.vtlist == (virtual_timers_list_t *)ch.vtlist.vt_next) { + /* Just removed the last element in the list, alarm timer stopped.*/ + port_timer_stop_alarm(); + } + else { + /* The alarm is set to the next element in the delta list.*/ + port_timer_set_alarm(ch.vtlist.vt_lasttime + + ch.vtlist.vt_next->vt_delta); + } + } +#endif /* CH_CFG_TIMEDELTA > 0 */ +} + +/** @} */ -- cgit v1.2.3