From 1ea7355d85e316aadfd90468b3e808bb3dc95ee9 Mon Sep 17 00:00:00 2001 From: gdisirio Date: Sun, 16 Aug 2009 13:07:24 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1073 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/kernel/src/chcond.c | 227 ++++++++++++++++++++++++++ os/kernel/src/chdebug.c | 81 ++++++++++ os/kernel/src/chevents.c | 392 +++++++++++++++++++++++++++++++++++++++++++++ os/kernel/src/chheap.c | 276 +++++++++++++++++++++++++++++++ os/kernel/src/chlists.c | 111 +++++++++++++ os/kernel/src/chmboxes.c | 244 ++++++++++++++++++++++++++++ os/kernel/src/chmempools.c | 112 +++++++++++++ os/kernel/src/chmsg.c | 125 +++++++++++++++ os/kernel/src/chmtx.c | 299 ++++++++++++++++++++++++++++++++++ os/kernel/src/chqueues.c | 304 +++++++++++++++++++++++++++++++++++ os/kernel/src/chschd.c | 242 ++++++++++++++++++++++++++++ os/kernel/src/chsem.c | 257 +++++++++++++++++++++++++++++ os/kernel/src/chserial.c | 168 +++++++++++++++++++ os/kernel/src/chsys.c | 129 +++++++++++++++ os/kernel/src/chthreads.c | 381 +++++++++++++++++++++++++++++++++++++++++++ os/kernel/src/chvt.c | 116 ++++++++++++++ 16 files changed, 3464 insertions(+) create mode 100644 os/kernel/src/chcond.c create mode 100644 os/kernel/src/chdebug.c create mode 100644 os/kernel/src/chevents.c create mode 100644 os/kernel/src/chheap.c create mode 100644 os/kernel/src/chlists.c create mode 100644 os/kernel/src/chmboxes.c create mode 100644 os/kernel/src/chmempools.c create mode 100644 os/kernel/src/chmsg.c create mode 100644 os/kernel/src/chmtx.c create mode 100644 os/kernel/src/chqueues.c create mode 100644 os/kernel/src/chschd.c create mode 100644 os/kernel/src/chsem.c create mode 100644 os/kernel/src/chserial.c create mode 100644 os/kernel/src/chsys.c create mode 100644 os/kernel/src/chthreads.c create mode 100644 os/kernel/src/chvt.c (limited to 'os/kernel/src') diff --git a/os/kernel/src/chcond.c b/os/kernel/src/chcond.c new file mode 100644 index 000000000..ef58f1ddf --- /dev/null +++ b/os/kernel/src/chcond.c @@ -0,0 +1,227 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 are contributed by and Copyright (C) 2008 + of Leon Woestenberg. + */ + +/** + * @file chcond.c + * @brief Condition Variables code. + * @addtogroup CondVars + * @{ + */ + +#include + +#if CH_USE_CONDVARS && CH_USE_MUTEXES + +/** + * @brief Initializes s @p CondVar structure. + * + * @param[out] cp pointer to a @p CondVar structure + * @note This function can be invoked from within an interrupt handler even if + * it is not an I-Class API because it does not touch any critical kernel + * data structure. + */ +void chCondInit(CondVar *cp) { + + chDbgCheck(cp != NULL, "chCondInit"); + + queue_init(&cp->c_queue); +} + +/** + * @brief Signals one thread that is waiting on the condition variable. + * + * @param[in] cp pointer to the @p CondVar structure + */ +void chCondSignal(CondVar *cp) { + + chDbgCheck(cp != NULL, "chCondSignal"); + + chSysLock(); + if (notempty(&cp->c_queue)) /* any thread ? */ + chSchWakeupS(fifo_remove(&cp->c_queue), RDY_OK); + chSysUnlock(); +} + +/** + * @brief Signals one thread that is waiting on the condition variable. + * + * @param[in] cp pointer to the @p CondVar structure + */ +void chCondSignalI(CondVar *cp) { + + chDbgCheck(cp != NULL, "chCondSignalI"); + + if (notempty(&cp->c_queue)) /* any thread ? */ + chSchReadyI(fifo_remove(&cp->c_queue))->p_rdymsg = RDY_OK; +} + +/** + * @brief Signals all threads that are waiting on the condition variable. + * + * @param[in] cp pointer to the @p CondVar structure + */ +void chCondBroadcast(CondVar *cp) { + + chSysLock(); + chCondBroadcastI(cp); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Signals all threads that are waiting on the condition variable. + * + * @param[in] cp pointer to the @p CondVar structure + */ +void chCondBroadcastI(CondVar *cp) { + + chDbgCheck(cp != NULL, "chCondBroadcastI"); + + /* 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(fifo_remove(&cp->c_queue))->p_rdymsg = RDY_RESET; +} + +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the mutex, waits on the condition variable, and finally + * acquires the mutex again. This is done atomically. + * + * @param[in] cp pointer to the @p CondVar structure + * @return The wakep mode. + * @retval RDY_OK if the condvar was signaled using chCondSignal(). + * @retval RDY_RESET if the condvar was signaled using chCondBroadcast(). + * @note The thread MUST already have locked the mutex when calling + * @p chCondWait(). + */ +msg_t chCondWait(CondVar *cp) { + msg_t msg; + + chSysLock(); + msg = chCondWaitS(cp); + chSysUnlock(); + return msg; +} + +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the mutex, waits on the condition variable, and finally + * acquires the mutex again. This is done atomically. + * + * @param[in] cp pointer to the @p CondVar structure + * @return The wakep mode. + * @retval RDY_OK if the condvar was signaled using chCondSignal(). + * @retval RDY_RESET if the condvar was signaled using chCondBroadcast(). + * @note The thread MUST already have locked the mutex when calling + * @p chCondWaitS(). + */ +msg_t chCondWaitS(CondVar *cp) { + Mutex *mp; + msg_t msg; + + chDbgCheck(cp != NULL, "chCondWaitS"); + chDbgAssert(currp->p_mtxlist != NULL, + "chCondWaitS(), #1", + "not owning a mutex"); + + mp = chMtxUnlockS(); /* unlocks the condvar mutex */ + prio_insert(currp, &cp->c_queue); /* enters the condvar queue */ + currp->p_wtcondp = cp; /* needed by the tracer */ + chSchGoSleepS(PRWTCOND); /* waits on the condvar */ + msg = currp->p_rdymsg; /* fetches the wakeup message */ + chMtxLockS(mp); /* atomically relocks the mutex */ + return msg; /* returns the wakeup message */ +} + +#if CH_USE_CONDVARS_TIMEOUT +/** + * @brief Waits on the condition variable releasing the mutex lock. + * @details Releases the mutex, waits on the condition variable, and finally + * acquires the mutex again. This is done atomically. + * + * @param[in] cp pointer to the @p CondVar structure + * @param[in] time the number of ticks before the operation timeouts, + * the special value @p TIME_INFINITE is allowed. + * It is not possible to specify zero @p TIME_IMMEDIATE + * as timeout specification because it would make no sense + * in this function. + * @return The wakep mode. + * @retval RDY_OK if the condvar was signaled using chCondSignal(). + * @retval RDY_RESET if the condvar was signaled using chCondBroadcast(). + * @retval RDY_TIMEOUT if the condvar was not signaled within the specified + * timeout. + * @note The thread MUST already have locked the mutex when calling + * @p chCondWaitTimeout(). + */ +msg_t chCondWaitTimeout(CondVar *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 mutex, waits on the condition variable, and finally + * acquires the mutex again. This is done atomically. + * + * @param[in] cp pointer to the @p CondVar structure + * @param[in] time the number of ticks before the operation timeouts, + * the special value @p TIME_INFINITE is allowed. + * It is not possible to specify zero @p TIME_IMMEDIATE + * as timeout specification because it would make no sense + * in this function. + * @return The wakep mode. + * @retval RDY_OK if the condvar was signaled using chCondSignal(). + * @retval RDY_RESET if the condvar was signaled using chCondBroadcast(). + * @retval RDY_TIMEOUT if the condvar was not signaled within the specified + * timeout. + * @note The thread MUST already have locked the mutex when calling + * @p chCondWaitTimeoutS(). + */ +msg_t chCondWaitTimeoutS(CondVar *cp, systime_t time) { + Mutex *mp; + msg_t msg; + + chDbgCheck(cp != NULL, "chCondWaitTimeoutS"); + chDbgAssert(currp->p_mtxlist != NULL, + "chCondWaitTimeoutS(), #1", + "not owning a mutex"); + + mp = chMtxUnlockS(); /* unlocks the condvar mutex */ + prio_insert(currp, &cp->c_queue); /* enters the condvar queue */ + currp->p_wtcondp = cp; /* needed by the tracer */ + chSchGoSleepTimeoutS(PRWTCOND, time); /* waits on the condvar */ + msg = currp->p_rdymsg; /* fetches the wakeup message */ + chMtxLockS(mp); /* atomically relocks the mutex */ + return msg; /* returns the wakeup message */ +} +#endif /* CH_USE_CONDVARS_TIMEOUT */ + +#endif /* CH_USE_CONDVARS && CH_USE_MUTEXES */ + +/** @} */ diff --git a/os/kernel/src/chdebug.c b/os/kernel/src/chdebug.c new file mode 100644 index 000000000..9a8120b29 --- /dev/null +++ b/os/kernel/src/chdebug.c @@ -0,0 +1,81 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +#if CH_DBG_ENABLE_TRACE +/** + * @brief Public trace buffer. + */ +TraceBuffer trace_buffer; + +/** + * @brief Trace circular buffer subsystem initialization. + */ +void trace_init(void) { + + trace_buffer.tb_size = TRACE_BUFFER_SIZE; + trace_buffer.tb_ptr = &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 + * @param[in] ntp the thread to be switched in + */ +void chDbgTrace(Thread *otp, Thread *ntp) { + + trace_buffer.tb_ptr->cse_wtobjp = otp->p_wtobjp; + trace_buffer.tb_ptr->cse_time = chTimeNow(); + trace_buffer.tb_ptr->cse_state = otp->p_state; + trace_buffer.tb_ptr->cse_tid = (unsigned)ntp >> 4; + if (++trace_buffer.tb_ptr >= &trace_buffer.tb_buffer[TRACE_BUFFER_SIZE]) + trace_buffer.tb_ptr = &trace_buffer.tb_buffer[0]; +} +#endif /* CH_DBG_ENABLE_TRACE */ + +#if CH_DBG_ENABLE_ASSERTS || CH_DBG_ENABLE_CHECKS || CH_DBG_ENABLE_STACK_CHECK +/** + * @brief Pointer to the panic message. + * @details This pointer is meant to be accessed through the debugger, it is + * written once and then the system is halted. + */ +char *panic_msg; + +/** + * @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(char *msg) { + + panic_msg = msg; + chSysHalt(); +} +#endif /* CH_DBG_ENABLE_ASSERTS || CH_DBG_ENABLE_CHECKS || CH_DBG_ENABLE_STACK_CHECK */ + +/** @} */ diff --git a/os/kernel/src/chevents.c b/os/kernel/src/chevents.c new file mode 100644 index 000000000..098adce5e --- /dev/null +++ b/os/kernel/src/chevents.c @@ -0,0 +1,392 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 chevents.c + * @brief Events code. + * @addtogroup Events + * @{ + */ +#include + +#if CH_USE_EVENTS +/** + * @brief Registers an Event Listener on an Event Source. + * + * @param[in] esp pointer to the @p EventSource structure + * @param[in] elp pointer to the @p EventListener structure + * @param[in] emask the mask of event flags to be pended to the thread when the + * event source is broadcasted + * @note Multiple Event Listeners can specify the same bits to be pended. + */ +void chEvtRegisterMask(EventSource *esp, EventListener *elp, eventmask_t emask) { + + chDbgCheck((esp != NULL) && (elp != NULL), "chEvtRegisterMask"); + + chSysLock(); + elp->el_next = esp->es_next; + esp->es_next = elp; + elp->el_listener = currp; + elp->el_mask = emask; + chSysUnlock(); +} + +/** + * @brief Unregisters an Event Listener from its Event Source. + * + * @param[in] esp pointer to the @p EventSource structure + * @param[in] elp pointer to the @p EventListener structure + * @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). + */ +void chEvtUnregister(EventSource *esp, EventListener *elp) { + EventListener *p; + + chDbgCheck((esp != NULL) && (elp != NULL), "chEvtUnregister"); + + p = (EventListener *)esp; + chSysLock(); + while (p->el_next != (EventListener *)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. + */ +eventmask_t chEvtClear(eventmask_t mask) { + eventmask_t m; + + chSysLock(); + + m = currp->p_epending & mask; + currp->p_epending &= ~mask; + + chSysUnlock(); + return m; +} + +/** + * @brief Pends 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 events to be pended + * @return The current pending events mask. + */ +eventmask_t chEvtPend(eventmask_t mask) { + + chSysLock(); + + mask = (currp->p_epending |= mask); + + chSysUnlock(); + return mask; +} + +/** + * @brief Pends a set of event flags on the specified @p Thread. + * + * @param[in] tp the thread to be signaled + * @param[in] mask the event flags set to be pended + */ +void chEvtSignal(Thread *tp, eventmask_t mask) { + + chDbgCheck(tp != NULL, "chEvtSignal"); + + chSysLock(); + chEvtSignalI(tp, mask); + chSysUnlock(); +} + +/** + * @brief Pends a set of event flags on the specified @p Thread. + * + * @param[in] tp the thread to be signaled + * @param[in] mask the event flags set to be pended + */ +void chEvtSignalI(Thread *tp, eventmask_t mask) { + + chDbgCheck(tp != NULL, "chEvtSignalI"); + + tp->p_epending |= mask; + /* Test on the AND/OR conditions wait states.*/ + if (((tp->p_state == PRWTOREVT) && ((tp->p_epending & tp->p_ewmask) != 0)) || + ((tp->p_state == PRWTANDEVT) && ((tp->p_epending & tp->p_ewmask) == tp->p_ewmask))) + chSchReadyI(tp)->p_rdymsg = RDY_OK; +} + +/** + * @brief Signals all the Event Listeners registered on the specified Event + * Source. + * + * @param[in] esp pointer to the @p EventSource structure + */ +void chEvtBroadcast(EventSource *esp) { + + chSysLock(); + chEvtBroadcastI(esp); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Signals all the Event Listeners registered on the specified Event + * Source. + * + * @param[in] esp pointer to the @p EventSource structure + */ +void chEvtBroadcastI(EventSource *esp) { + EventListener *elp; + + chDbgCheck(esp != NULL, "chEvtBroadcastI"); + + elp = esp->es_next; + while (elp != (EventListener *)esp) { + chEvtSignalI(elp->el_listener, elp->el_mask); + elp = elp->el_next; + } +} + +/** + * @brief Invokes the event handlers associated with a mask. + * + * @param[in] mask mask of the events to be dispatched + * @param[in] handlers an array of @p evhandler_t. The array must have size + * equal to the number of bits in eventmask_t. + */ +void chEvtDispatch(const evhandler_t handlers[], eventmask_t mask) { + eventid_t eid; + + chDbgCheck(handlers != NULL, "chEvtDispatch"); + + eid = 0; + while (mask) { + if (mask & EVENT_MASK(eid)) { + chDbgAssert(handlers[eid] != NULL, + "chEvtDispatch(), #1", + "null handler"); + mask &= ~EVENT_MASK(eid); + handlers[eid](eid); + } + eid++; + } +} + +#if CH_OPTIMIZE_SPEED || !CH_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 ewmask to become pending then the event is cleared and returned. + * + * @param[in] ewmask mask of the events that the function should wait for, + * @p ALL_EVENTS enables all the events + * @return The mask of the lowest id served and cleared event. + * @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. + */ +eventmask_t chEvtWaitOne(eventmask_t ewmask) { + eventmask_t m; + + chSysLock(); + + if ((m = (currp->p_epending & ewmask)) == 0) { + currp->p_ewmask = ewmask; + chSchGoSleepS(PRWTOREVT); + m = currp->p_epending & ewmask; + } + m &= -m; + currp->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 ewmask to become pending then the events are cleared and returned. + * + * @param[in] ewmask mask of the events that the function should wait for, + * @p ALL_EVENTS enables all the events + * @return The mask of the served and cleared events. + */ +eventmask_t chEvtWaitAny(eventmask_t ewmask) { + eventmask_t m; + + chSysLock(); + + if ((m = (currp->p_epending & ewmask)) == 0) { + currp->p_ewmask = ewmask; + chSchGoSleepS(PRWTOREVT); + m = currp->p_epending & ewmask; + } + currp->p_epending &= ~m; + + chSysUnlock(); + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p ewmask to + * become pending then the events are cleared and returned. + * + * @param[in] ewmask mask of the event ids that the function should wait for + * @return The mask of the served and cleared events. + */ +eventmask_t chEvtWaitAll(eventmask_t ewmask) { + + chSysLock(); + + if ((currp->p_epending & ewmask) != ewmask) { + currp->p_ewmask = ewmask; + chSchGoSleepS(PRWTANDEVT); + } + currp->p_epending &= ~ewmask; + + chSysUnlock(); + return ewmask; +} +#endif /* CH_OPTIMIZE_SPEED || !CH_USE_EVENTS_TIMEOUT */ + +#if CH_USE_EVENTS_TIMEOUT +/** + * @brief Waits for exactly one of the specified events. + * @details The function waits for one event among those specified in + * @p ewmask to become pending then the event is cleared and returned. + * + * @param[in] ewmask mask of the events 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 specified timeout expired. + * @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. + */ +eventmask_t chEvtWaitOneTimeout(eventmask_t ewmask, systime_t time) { + eventmask_t m; + + chSysLock(); + + if ((m = (currp->p_epending & ewmask)) == 0) { + if (TIME_IMMEDIATE == time) + return (eventmask_t)0; + currp->p_ewmask = ewmask; + if (chSchGoSleepTimeoutS(PRWTOREVT, time) < RDY_OK) + return (eventmask_t)0; + m = currp->p_epending & ewmask; + } + m &= -m; + currp->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 ewmask to become pending then the events are cleared and + * returned. + * + * @param[in] ewmask mask of the events 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 specified timeout expired. + */ +eventmask_t chEvtWaitAnyTimeout(eventmask_t ewmask, systime_t time) { + eventmask_t m; + + chSysLock(); + + if ((m = (currp->p_epending & ewmask)) == 0) { + if (TIME_IMMEDIATE == time) + return (eventmask_t)0; + currp->p_ewmask = ewmask; + if (chSchGoSleepTimeoutS(PRWTOREVT, time) < RDY_OK) + return (eventmask_t)0; + m = currp->p_epending & ewmask; + } + currp->p_epending &= ~m; + + chSysUnlock(); + return m; +} + +/** + * @brief Waits for all the specified events. + * @details The function waits for all the events specified in @p ewmask to + * become pending then the events are cleared and returned. + * + * @param[in] ewmask mask of the event ids that the function should wait for + * @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 specified timeout expired. + */ +eventmask_t chEvtWaitAllTimeout(eventmask_t ewmask, systime_t time) { + + chSysLock(); + + if ((currp->p_epending & ewmask) != ewmask) { + if (TIME_IMMEDIATE == time) + return (eventmask_t)0; + currp->p_ewmask = ewmask; + if (chSchGoSleepTimeoutS(PRWTANDEVT, time) < RDY_OK) + return (eventmask_t)0; + } + currp->p_epending &= ~ewmask; + + chSysUnlock(); + return ewmask; +} +#endif /* CH_USE_EVENTS_TIMEOUT */ + +#endif /* CH_USE_EVENTS */ + +/** @} */ diff --git a/os/kernel/src/chheap.c b/os/kernel/src/chheap.c new file mode 100644 index 000000000..82b1ba785 --- /dev/null +++ b/os/kernel/src/chheap.c @@ -0,0 +1,276 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 Heap code. + * @addtogroup Heap + * @{ + */ + +#include + +#if CH_USE_HEAP + +#if !CH_USE_MALLOC_HEAP + +#define MAGIC 0xF5A0 +#define ALIGN_TYPE void * +#define ALIGN_MASK (sizeof(ALIGN_TYPE) - 1) +#define ALIGN_SIZE(p) (((size_t)(p) + ALIGN_MASK) & ~ALIGN_MASK) + +struct header { + union { + struct header *h_next; + size_t h_magic; + }; + size_t h_size; +}; + +static struct { + struct header free; /* Guaranteed to be not adjacent to the heap */ +#if CH_USE_MUTEXES +#define H_LOCK() chMtxLock(&heap.hmtx) +#define H_UNLOCK() chMtxUnlock() + Mutex hmtx; +#elif CH_USE_SEMAPHORES +#define H_LOCK() chSemWait(&heap.hsem) +#define H_UNLOCK() chSemSignal(&heap.hsem) + Semaphore hsem; +#else +#error "The heap allocator requires mutexes or semaphores to be enabled" +#endif +#if CH_HEAP_SIZE > 0 + union { + ALIGN_TYPE alignment; + char buffer[ALIGN_SIZE(CH_HEAP_SIZE)]; + }; +#endif +} heap; + +/** + * @brief Initializes the allocator subsystem. + * + * @note Internal use only. + */ +void heap_init(void) { + struct header *hp; + +#if CH_HEAP_SIZE == 0 + extern char __heap_base__; + extern char __heap_end__; + + hp = (void *)&__heap_base__; + hp->h_size = &__heap_end__ - &__heap_base__ - sizeof(struct header); +#else + hp = (void *)&heap.buffer[0]; + hp->h_size = (&heap.buffer[ALIGN_SIZE(CH_HEAP_SIZE)] - &heap.buffer[0]) - + sizeof(struct header); +#endif + hp->h_next = NULL; + heap.free.h_next = hp; + heap.free.h_size = 0; +#if CH_USE_MUTEXES + chMtxInit(&heap.hmtx); +#else + chSemInit(&heap.hsem, 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. + * + * @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. + */ +void *chHeapAlloc(size_t size) { + struct header *qp, *hp, *fp; + + size = ALIGN_SIZE(size); + qp = &heap.free; + H_LOCK(); + + while (qp->h_next != NULL) { + hp = qp->h_next; + if (hp->h_size >= size) { + if (hp->h_size < size + sizeof(struct 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_next = hp->h_next; + } + else { + /* Block bigger enough, must split it */ + fp = (void *)((char *)(hp) + sizeof(struct header) + size); + fp->h_next = hp->h_next; + fp->h_size = hp->h_size - sizeof(struct header) - size; + qp->h_next = fp; + hp->h_size = size; + } + hp->h_magic = MAGIC; + + H_UNLOCK(); + return (void *)(hp + 1); + } + qp = hp; + } + + H_UNLOCK(); + return NULL; +} + +#define LIMIT(p) (struct header *)((char *)(p) + \ + sizeof(struct header) + \ + (p)->h_size) + +/** + * @brief Frees a previously allocated memory block. + * + * @param[in] p the memory block pointer + */ +void chHeapFree(void *p) { + struct header *qp, *hp; + + chDbgCheck(p != NULL, "chHeapFree"); + + hp = (struct header *)p - 1; + chDbgAssert(hp->h_magic == MAGIC, + "chHeapFree(), #1", + "it is not magic"); + qp = &heap.free; + H_LOCK(); + + while (TRUE) { + + chDbgAssert((hp < qp) || (hp >= LIMIT(qp)), + "chHeapFree(), #2", + "within free block"); + + if (((qp == &heap.free) || (hp > qp)) && + ((qp->h_next == NULL) || (hp < qp->h_next))) { + /* Insertion after qp */ + hp->h_next = qp->h_next; + qp->h_next = hp; + /* Verifies if the newly inserted block should be merged */ + if (LIMIT(hp) == hp->h_next) { + /* Merge with the next block */ + hp->h_size += hp->h_next->h_size + sizeof(struct header); + hp->h_next = hp->h_next->h_next; + } + if ((LIMIT(qp) == hp)) { /* Cannot happen when qp == &heap.free */ + /* Merge with the previous block */ + qp->h_size += hp->h_size + sizeof(struct header); + qp->h_next = hp->h_next; + } + + H_UNLOCK(); + return; + } + qp = qp->h_next; + } +} + +/** + * @brief Reports the heap status. + * + * @param[in] sizep pointer to a variable that will receive the total + * fragmented free space + * @return The number of fragments in the heap. + * @note This function is meant to be used in the test suite, it should not be + * really useful for the application code. + * @note This function is not implemented when the @p CH_USE_MALLOC_HEAP + * configuration option is used (it always returns zero). + */ +size_t chHeapStatus(size_t *sizep) { + struct header *qp; + size_t n, sz; + + H_LOCK(); + + sz = 0; + for (n = 0, qp = &heap.free; qp->h_next; n++, qp = qp->h_next) + sz += qp->h_next->h_size; + if (sizep) + *sizep = sz; + + H_UNLOCK(); + return n; +} + +#else /* CH_USE_MALLOC_HEAP */ + +#include + +#if CH_USE_MUTEXES +#define H_LOCK() chMtxLock(&hmtx) +#define H_UNLOCK() chMtxLock(&hmtx) +static Mutex hmtx; +#elif CH_USE_SEMAPHORES +#define H_LOCK() chSemWait(&hsem) +#define H_UNLOCK() chSemSignal(&hsem) +static Semaphore hsem; +#else +#error "The heap allocator requires mutexes or semaphores to be enabled" +#endif + +void heap_init(void) { + +#if CH_USE_MUTEXES + chMtxInit(&hmtx); +#else + chSemInit(&hsem, 1); +#endif +} + +void *chHeapAlloc(size_t size) { + void *p; + + H_LOCK(); + p = malloc(size); + H_UNLOCK(); + return p; +} + +void chHeapFree(void *p) { + + chDbgCheck(p != NULL, "chHeapFree"); + + H_LOCK(); + free(p); + H_UNLOCK(); +} + +size_t chHeapStatus(size_t *sizep) { + + if (sizep) + *sizep = 0; + return 0; +} + +#endif /* CH_USE_MALLOC_HEAP */ + +#endif /* CH_USE_HEAP */ + +/** @} */ diff --git a/os/kernel/src/chlists.c b/os/kernel/src/chlists.c new file mode 100644 index 000000000..a2177ca63 --- /dev/null +++ b/os/kernel/src/chlists.c @@ -0,0 +1,111 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 ThreadLists + * @{ + */ +#include + +#if !CH_OPTIMIZE_SPEED || defined(__DOXYGEN__) +/** + * @brief Inserts a thread into a priority ordered 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 + * @note The insertion is done by scanning the list from the highest priority + * toward the lowest. + * @note This function is @b not an API. + */ +void prio_insert(Thread *tp, ThreadsQueue *tqp) { + + /* cp iterates over the queue */ + Thread *cp = (Thread *)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 *)tqp) && (cp->p_prio >= tp->p_prio)); + /* insert before cp, point tp to next and prev in queue */ + tp->p_prev = (tp->p_next = cp)->p_prev; + /* make prev point to tp, and cp point back to tp */ + 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 + * @note This function is @b not an API. + */ +void queue_insert(Thread *tp, ThreadsQueue *tqp) { + + tp->p_prev = (tp->p_next = (Thread *)tqp)->p_prev; + tp->p_prev->p_next = tqp->p_prev = tp; +} + +/** + * @brief Removes the first-out Thread from a queue and returns it. + * + * @param[in] tqp the pointer to the threads list header + * @return The removed thread pointer. + * @note This function is @b not an API. + */ +Thread *fifo_remove(ThreadsQueue *tqp) { + Thread *tp = tqp->p_next; + + (tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp; + return tp; +} + +/** + * @brief Removes the last-out Thread from a queue and returns it. + * + * @param[in] tqp the pointer to the threads list header + * @return The removed thread pointer. + * @note This function is @b not an API. + */ +Thread *lifo_remove(ThreadsQueue *tqp) { + Thread *tp = tqp->p_next; + + (tqp->p_next = tp->p_next)->p_prev = (Thread *)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. + * @note This function is @b not an API. + */ +Thread *dequeue(Thread *tp) { + + tp->p_prev->p_next = tp->p_next; + tp->p_next->p_prev = tp->p_prev; + return tp; +} +#endif /* CH_OPTIMIZE_SPEED */ + +/** @} */ diff --git a/os/kernel/src/chmboxes.c b/os/kernel/src/chmboxes.c new file mode 100644 index 000000000..8a791a984 --- /dev/null +++ b/os/kernel/src/chmboxes.c @@ -0,0 +1,244 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +#if CH_USE_MAILBOXES +/** + * @brief Initializes a Mailbox object. + * + * @param[out] mbp the pointer to the Mailbox structure to be initialized + * @param[in] buf the circular messages buffer + * @param[in] n the buffer size as number of @p msg_t + */ +void chMBInit(Mailbox *mbp, msg_t *buf, cnt_t n) { + + chDbgCheck((mbp != NULL) && (buf != NULL) && (n > 0), "chMBInit"); + + mbp->mb_buffer = mbp->mb_wrptr = mbp->mb_rdptr = buf; + mbp->mb_top = &buf[n]; + chSemInit(&mbp->mb_emptysem, n); + chSemInit(&mbp->mb_fullsem, 0); +} + +/** + * @brief Resets a Mailbox 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 Mailbox object + */ +void chMBReset(Mailbox *mbp) { + + chDbgCheck(mbp != NULL, "chMBReset"); + + 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 Mailbox 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 the message was correctly posted. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBPost(Mailbox *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 Mailbox 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 the message was correctly posted. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBPostS(Mailbox *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chDbgCheck(mbp != NULL, "chMBPostS"); + + 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 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 Mailbox 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 the message was correctly posted. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBPostAhead(Mailbox *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 Mailbox 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 the message was correctly posted. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBPostAheadS(Mailbox *mbp, msg_t msg, systime_t time) { + msg_t rdymsg; + + chDbgCheck(mbp != NULL, "chMBPostAheadS"); + + 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 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 Mailbox 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 was correctly fetched. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBFetch(Mailbox *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 Mailbox 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 was correctly fetched. + * @retval RDY_RESET if the mailbox was reset while waiting. + * @retval RDY_TIMEOUT if the operation timed out. + */ +msg_t chMBFetchS(Mailbox *mbp, msg_t *msgp, systime_t time) { + msg_t rdymsg; + + chDbgCheck((mbp != NULL) && (msgp != NULL), "chMBFetchS"); + + 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; +} +#endif /* CH_USE_MAILBOXES */ + +/** @} */ diff --git a/os/kernel/src/chmempools.c b/os/kernel/src/chmempools.c new file mode 100644 index 000000000..dd6f3d1ba --- /dev/null +++ b/os/kernel/src/chmempools.c @@ -0,0 +1,112 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 MemoryPools + * @{ + */ + +#include + +#if CH_USE_MEMPOOLS +/** + * @brief Initializes an empty memory pool. + * + * @param[out] mp pointer to a @p MemoryPool 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 + */ +void chPoolInit(MemoryPool *mp, size_t size) { + + chDbgCheck((mp != NULL) && (size >= sizeof(void *)), "chPoolInit"); + + mp->mp_next = NULL; + mp->mp_object_size = size; +} + +/** + * @brief Allocates an object from a memory pool. + * + * @param[in] mp pointer to a @p MemoryPool structure + * @return The pointer to the allocated object. + * @retval NULL if pool is empty. + */ +void *chPoolAllocI(MemoryPool *mp) { + void *objp; + + chDbgCheck(mp != NULL, "chPoolAllocI"); + + if ((objp = mp->mp_next) != NULL) + mp->mp_next = mp->mp_next->ph_next; + + return objp; +} + +/** + * @brief Allocates an object from a memory pool. + * + * @param[in] mp pointer to a @p MemoryPool structure + * @return The pointer to the allocated object. + * @retval NULL if pool is empty. + */ +void *chPoolAlloc(MemoryPool *mp) { + void *objp; + + chSysLock(); + objp = chPoolAllocI(mp); + chSysUnlock(); + return objp; +} + +/** + * @brief Releases (or adds) an object into (to) a memory pool. + * + * @param[in] mp pointer to a @p MemoryPool structure + * @param[in] objp the pointer to the object to be released or added + * @note the object is assumed to be of the right size for the specified + * memory pool. + */ +void chPoolFreeI(MemoryPool *mp, void *objp) { + struct pool_header *php = objp; + + chDbgCheck((mp != NULL) && (objp != NULL), "chPoolFreeI"); + + php->ph_next = mp->mp_next; + mp->mp_next = php; +} + +/** + * @brief Releases (or adds) an object into (to) a memory pool. + * + * @param[in] mp pointer to a @p MemoryPool structure + * @param[in] objp the pointer to the object to be released or added + * @note the object is assumed to be of the right size for the specified + * memory pool. + */ +void chPoolFree(MemoryPool *mp, void *objp) { + + chSysLock(); + chPoolFreeI(mp, objp); + chSysUnlock(); +} +#endif /* CH_USE_MEMPOOLS */ + +/** @} */ diff --git a/os/kernel/src/chmsg.c b/os/kernel/src/chmsg.c new file mode 100644 index 000000000..393ab8dad --- /dev/null +++ b/os/kernel/src/chmsg.c @@ -0,0 +1,125 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +#if CH_USE_MESSAGES + +#if CH_USE_MESSAGES_PRIORITY +#define msg_insert(tp, qp) prio_insert(tp, qp) +#else +#define msg_insert(tp, qp) queue_insert(tp, qp) +#endif + +/** + * @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 return message from @p chMsgRelease(). + */ +msg_t chMsgSend(Thread *tp, msg_t msg) { + + chDbgCheck(tp != NULL, "chMsgSend"); + + chSysLock(); + msg_insert(currp, &tp->p_msgqueue); + currp->p_msg = msg; + currp->p_wtthdp = tp; + if (tp->p_state == PRWTMSG) + chSchReadyI(tp); + chSchGoSleepS(PRSNDMSG); + msg = currp->p_rdymsg; + chSysUnlock(); + return msg; +} + +/** + * @brief Suspends the thread and waits for an incoming message. + * + * @return The pointer to the message structure. Note, it is always the + * message associated to the thread on the top of the messages queue. + * @note You can assume that the data contained in the message is stable until + * you invoke @p chMsgRelease() because the sending thread is + * suspended until then. + */ +msg_t chMsgWait(void) { + msg_t msg; + + chSysLock(); + if (!chMsgIsPendingI(currp)) + chSchGoSleepS(PRWTMSG); + msg = chMsgGetI(currp); + chSysUnlock(); + return msg; +} + +/** + * @brief Returns the next message in the queue. + * + * @return The pointer to the message structure. Note, it is always the + * message associated to the thread on the top of the messages queue. + * If the queue is empty then @p NULL is returned. + * @note 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. Always remember that the message data is not + * copied between the sender and the receiver, just a pointer is passed. + */ +msg_t chMsgGet(void) { + msg_t msg; + + chSysLock(); + msg = chMsgIsPendingI(currp) ? chMsgGetI(currp) : (msg_t)NULL; + chSysUnlock(); + return msg; +} + +/** + * @brief Releases the thread waiting on top of the messages queue. + * + * @param[in] msg the message returned to the message sender + * @note You can call this function only if there is a message already in the + * queue else the result will be unpredictable (a crash most likely). + * Exiting from the @p chMsgWait() ensures you have at least one + * message in the queue so it is not a big deal.
+ * The condition is only tested in debug mode in order to make this code + * as fast as possible. + */ +void chMsgRelease(msg_t msg) { + + chSysLock(); + chDbgAssert(chMsgIsPendingI(currp), + "chMsgRelease(), #1", + "no message pending"); + chSchWakeupS(fifo_remove(&currp->p_msgqueue), msg); + chSysUnlock(); +} + +#endif /* CH_USE_MESSAGES */ + +/** @} */ diff --git a/os/kernel/src/chmtx.c b/os/kernel/src/chmtx.c new file mode 100644 index 000000000..5fcb6fd5e --- /dev/null +++ b/os/kernel/src/chmtx.c @@ -0,0 +1,299 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +#if CH_USE_MUTEXES + +/** + * @brief Initializes s @p Mutex structure. + * + * @param[out] mp pointer to a @p Mutex structure + * @note This function can be invoked from within an interrupt handler even if + * it is not an I-Class API because it does not touch any critical kernel + * data structure. + */ +void chMtxInit(Mutex *mp) { + + chDbgCheck(mp != NULL, "chMtxInit"); + + queue_init(&mp->m_queue); + mp->m_owner = NULL; +} + +/** + * @brief Locks the specified mutex. + * + * @param[in] mp pointer to the @p Mutex structure + */ +void chMtxLock(Mutex *mp) { + + chSysLock(); + + chMtxLockS(mp); + + chSysUnlock(); +} + +/** + * @brief Locks the specified mutex. + * + * @param[in] mp pointer to the @p Mutex structure + * @note This function must be called within a @p chSysLock() / @p chSysUnlock() + * block. + */ +void chMtxLockS(Mutex *mp) { + + chDbgCheck(mp != NULL, "chMtxLockS"); + + /* the mutex is 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 *tp = mp->m_owner; + /* { tp is the thread currently owning the mutex } */ + /* the running thread has higher priority than tp? */ + while (tp->p_prio < currp->p_prio) { + /* make priority of thread tp match the running thread's priority */ + tp->p_prio = currp->p_prio; + /* + * The following states need priority queues reordering. + */ + switch (tp->p_state) { + /* thread tp is waiting on a mutex? */ + case PRWTMTX: + /* Requeues tp with its new priority on the mutex wait queue. */ + prio_insert(dequeue(tp), &tp->p_wtmtxp->m_queue); + /* boost the owner of this mutex if needed */ + tp = tp->p_wtmtxp->m_owner; + continue; +#if CH_USE_CONDVARS + case PRWTCOND: + /* Requeues tp with its new priority on the condvar queue. */ + prio_insert(dequeue(tp), &tp->p_wtcondp->c_queue); + break; +#endif +#if CH_USE_SEMAPHORES_PRIORITY + case PRWTSEM: + /* Requeues tp with its new priority on the semaphore queue. */ + prio_insert(dequeue(tp), &tp->p_wtsemp->s_queue); + break; +#endif +#if CH_USE_MESSAGES_PRIORITY + case PRSNDMSG: + /* Requeues tp with its new priority on the server thread queue. */ + prio_insert(dequeue(tp), &tp->p_wtthdp->p_msgqueue); + break; +#endif + /* thread tp is ready? */ + case PRREADY: + /* Requeue tp with its new priority on the ready list. */ + chSchReadyI(dequeue(tp)); + } + break; + } + /* sleep on the mutex */ + prio_insert(currp, &mp->m_queue); + /* thread remembers the mutex where it is waiting on */ + currp->p_wtmtxp = mp; + chSchGoSleepS(PRWTMTX); + chDbgAssert(mp->m_owner == NULL, "chMtxLockS(), #1", "still owned"); + } + /* + * The mutex is now inserted in the owned mutexes list. + */ + mp->m_owner = currp; + mp->m_next = currp->p_mtxlist; + currp->p_mtxlist = mp; +} + +/** + * @brief Tries to lock a mutex. + * @details This function does not have any overhead related to + * the priority inheritance mechanism because it does not try to + * enter a sleep state on the mutex. + * + * @param[in] mp pointer to the @p Mutex structure + * @retval TRUE if the mutex was successfully acquired + * @retval FALSE if the lock attempt failed. + */ +bool_t chMtxTryLock(Mutex *mp) { + bool_t b; + + chSysLock(); + + b = chMtxTryLockS(mp); + + chSysUnlock(); + return b; +} + +/** + * @brief Tries to lock a mutex. + * @details This function does not have any overhead related to + * the priority inheritance mechanism because it does not try to + * enter a sleep state on the mutex. + * @param[in] mp pointer to the @p Mutex structure + * @retval TRUE if the mutex was successfully acquired + * @retval FALSE if the lock attempt failed. + * @note This function must be called within a @p chSysLock() / @p chSysUnlock() + * block. + */ +bool_t chMtxTryLockS(Mutex *mp) { + + chDbgCheck(mp != NULL, "chMtxTryLockS"); + + 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. + * + * @return The pointer to the unlocked mutex. + */ +Mutex *chMtxUnlock(void) { + Mutex *ump, *mp; + + chSysLock(); + chDbgAssert(currp->p_mtxlist != NULL, + "chMtxUnlock(), #1", + "owned mutexes list empty"); + chDbgAssert(currp->p_mtxlist->m_owner == currp, + "chMtxUnlock(), #2", + "ownership failure"); + /* remove the top Mutex from the Threads's owned mutexes list */ + ump = currp->p_mtxlist; + currp->p_mtxlist = ump->m_next; + /* mark the Mutex as not owned */ + ump->m_owner = NULL; + /* + * If a thread is waiting on the mutex then the hard part begins. + */ + if (chMtxQueueNotEmptyS(ump)) { + /* get the highest priority thread waiting for the unlocked mutex */ + Thread *tp = fifo_remove(&ump->m_queue); + /* + * Recalculates the optimal thread priority by scanning the owned mutexes list. + */ + tprio_t newprio = currp->p_realprio; + /* iterate mp over all the (other) mutexes the current thread still owns */ + mp = currp->p_mtxlist; + while (mp != NULL) { + /* mutex mp has a higher priority thread pending? */ + if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio)) + /* boost current thread's priority to waiting thread */ + newprio = mp->m_queue.p_next->p_prio; + mp = mp->m_next; + } + /* (possibly) boost the priority of the current thread */ + currp->p_prio = newprio; + /* awaken the highest priority thread waiting for the unlocked mutex */ + chSchWakeupS(tp, RDY_OK); + } + chSysUnlock(); + return ump; +} + +/** + * @brief Unlocks the next owned mutex in reverse lock order. + * + * @return The pointer to the unlocked mutex. + * @note This function must be called within a @p chSysLock() / @p chSysUnlock() + * block. + * @note This function does not reschedule internally. + */ +Mutex *chMtxUnlockS(void) { + Mutex *ump, *mp; + + chDbgAssert(currp->p_mtxlist != NULL, + "chMtxUnlockS(), #1", + "owned mutexes list empty"); + chDbgAssert(currp->p_mtxlist->m_owner == currp, + "chMtxUnlockS(), #2", + "ownership failure"); + + /* + * Removes the top Mutex from the owned mutexes list and marks it as not owned. + */ + ump = currp->p_mtxlist; + currp->p_mtxlist = ump->m_next; + ump->m_owner = NULL; + /* + * If a thread is waiting on the mutex then the hard part begins. + */ + if (chMtxQueueNotEmptyS(ump)) { + Thread *tp = fifo_remove(&ump->m_queue); + /* + * Recalculates the optimal thread priority by scanning the owned mutexes list. + */ + tprio_t newprio = currp->p_realprio; + mp = currp->p_mtxlist; + while (mp != NULL) { + if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio)) + newprio = mp->m_queue.p_next->p_prio; + mp = mp->m_next; + } + currp->p_prio = newprio; + chSchReadyI(tp); + } + return ump; +} + +/** + * @brief Unlocks all the mutexes owned by the invoking thread. + * @details 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. + */ +void chMtxUnlockAll(void) { + + chSysLock(); + if (currp->p_mtxlist != NULL) { + do { + Mutex *mp = currp->p_mtxlist; + currp->p_mtxlist = mp->m_next; + mp->m_owner = NULL; + if (chMtxQueueNotEmptyS(mp)) + chSchReadyI(fifo_remove(&mp->m_queue)); + } while (currp->p_mtxlist != NULL); + currp->p_prio = currp->p_realprio; + chSchRescheduleS(); + } + chSysUnlock(); +} + +#endif /* CH_USE_MUTEXES */ + +/** @} */ diff --git a/os/kernel/src/chqueues.c b/os/kernel/src/chqueues.c new file mode 100644 index 000000000..a72b83696 --- /dev/null +++ b/os/kernel/src/chqueues.c @@ -0,0 +1,304 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 IOQueues + * @{ + */ + +#include + +#if CH_USE_QUEUES + +/** + * @brief Initializes an input queue. + * @details A Semaphore is internally initialized and works as a counter of + * the bytes contained in the queue. + * + * @param[out] iqp pointer to an @p InputQueue structure + * @param[in] buffer pointer to a memory area allocated as queue buffer + * @param[in] size size of the queue buffer + * @param[in] inotify pointer to a callback function that is invoked when + * some data is read from the queue. The value can be + * @p NULL. + * + * @note The callback is invoked from within the S-Locked system state, + * see @ref system_states. + */ +void chIQInit(InputQueue *iqp, uint8_t *buffer, + size_t size, qnotify_t inotify) { + + iqp->q_buffer = iqp->q_rdptr = iqp->q_wrptr = buffer; + iqp->q_top = buffer + size; + chSemInit(&iqp->q_sem, 0); + iqp->q_notify = inotify; +} + +/** + * @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. + * + * @param[in] iqp pointer to an @p InputQueue structure + * + * @note A reset operation can be used by a low level driver in order to obtain + * immediate attention from the high level layers. + */ +void chIQResetI(InputQueue *iqp) { + + iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer; + chSemResetI(&iqp->q_sem, 0); +} + +/** + * @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 InputQueue structure + * @param[in] b the byte value to be written in the queue + * @return The operation status, it can be one of: + * @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. + */ +msg_t chIQPutI(InputQueue *iqp, uint8_t b) { + + if (chIQIsFull(iqp)) + return Q_FULL; + + *iqp->q_wrptr++ = b; + if (iqp->q_wrptr >= iqp->q_top) + iqp->q_wrptr = iqp->q_buffer; + chSemSignalI(&iqp->q_sem); + 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. + * + * @param[in] iqp pointer to an @p InputQueue structure + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return A byte value from the queue or: + * @retval Q_TIMEOUT if the specified time expired. + * @retval Q_RESET if the queue was reset. + */ +msg_t chIQGetTimeout(InputQueue *iqp, systime_t timeout) { + uint8_t b; + msg_t msg; + + chSysLock(); + if ((msg = chSemWaitTimeoutS(&iqp->q_sem, timeout)) < RDY_OK) { + chSysUnlock(); + return msg; + } + b = *iqp->q_rdptr++; + if (iqp->q_rdptr >= iqp->q_top) + iqp->q_rdptr = iqp->q_buffer; + + if (iqp->q_notify) + iqp->q_notify(); + + chSysUnlock(); + return b; +} + +/** + * @brief Non-blocking read. + * @details The function reads data from an input queue into a buffer. The + * transfer is non-blocking and can return zero if the queue is + * empty. + * + * @param[in] iqp pointer to an @p InputQueue structure + * @param[out] buffer pointer to the buffer where the input data is copied + * @param[in] n the maximum amount of data to be transferred + * @return The number of bytes transferred. + * + * @note The function is not atomic, if you need atomicity it is suggested + * to use a semaphore or a mutex for mutual exclusion. + */ +size_t chIQRead(InputQueue *iqp, uint8_t *buffer, size_t n) { + size_t r = 0; + + while (n--) { + chSysLock(); + if (chIQIsEmpty(iqp)) { + chSysUnlock(); + break; + } + chSemFastWaitI(&iqp->q_sem); + *buffer++ = *iqp->q_rdptr++; + if (iqp->q_rdptr >= iqp->q_top) + iqp->q_rdptr = iqp->q_buffer; + chSysUnlock(); + r++; + } + if (r && iqp->q_notify) { + chSysLock(); + iqp->q_notify(); + chSysUnlock(); + } + return r; +} + +/** + * @brief Initializes an output queue. + * @details A Semaphore is internally initialized and works as a counter of + * the free bytes in the queue. + * + * @param[out] oqp pointer to an @p OutputQueue structure + * @param[in] buffer pointer to a memory area allocated as queue buffer + * @param[in] size size of the queue buffer + * @param[in] onotify pointer to a callback function that is invoked when + * some data is written to the queue. The value can be + * @p NULL. + * + * @note The callback is invoked from within the S-Locked system state, + * see @ref system_states. + */ +void chOQInit(OutputQueue *oqp, uint8_t *buffer, + size_t size, qnotify_t onotify) { + + oqp->q_buffer = oqp->q_rdptr = oqp->q_wrptr = buffer; + oqp->q_top = buffer + size; + chSemInit(&oqp->q_sem, size); + oqp->q_notify = onotify; +} + +/** + * @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. + * + * @param[in] oqp pointer to an @p OutputQueue structure + * + * @note A reset operation can be used by a low level driver in order to obtain + * immediate attention from the high level layers. + */ +void chOQResetI(OutputQueue *oqp) { + + oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer; + chSemResetI(&oqp->q_sem, (cnt_t)(oqp->q_top - oqp->q_buffer)); +} + +/** + * @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. + * + * @param[in] oqp pointer to an @p OutputQueue structure + * @param[in] b the byte value to be written in the queue + * @param[in] timeout the number of ticks before the operation timeouts, + * the following special values are allowed: + * - @a TIME_IMMEDIATE immediate timeout. + * - @a TIME_INFINITE no timeout. + * . + * @return The operation status: + * @retval Q_OK if the operation succeeded. + * @retval Q_TIMEOUT if the specified time expired. + * @retval Q_RESET if the queue was reset. + */ +msg_t chOQPutTimeout(OutputQueue *oqp, uint8_t b, systime_t timeout) { + msg_t msg; + + chSysLock(); + if ((msg = chSemWaitTimeoutS(&oqp->q_sem, timeout)) < RDY_OK) { + chSysUnlock(); + return msg; + } + *oqp->q_wrptr++ = b; + if (oqp->q_wrptr >= oqp->q_top) + oqp->q_wrptr = oqp->q_buffer; + + if (oqp->q_notify) + oqp->q_notify(); + + 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 OutputQueue structure + * @return The byte value from the queue or: + * @retval Q_EMPTY if the queue is empty. + */ +msg_t chOQGetI(OutputQueue *oqp) { + uint8_t b; + + if (chOQIsEmpty(oqp)) + return Q_EMPTY; + + b = *oqp->q_rdptr++; + if (oqp->q_rdptr >= oqp->q_top) + oqp->q_rdptr = oqp->q_buffer; + chSemSignalI(&oqp->q_sem); + return b; +} + +/** + * @brief Non-blocking write. + * @details The function writes data from a buffer to an output queue. The + * transfer is non-blocking and can return zero if the queue is + * already full. + * + * @param[in] oqp pointer to an @p OutputQueue structure + * @param[out] buffer pointer to the buffer where the output data is stored + * @param[in] n the maximum amount of data to be transferred + * @return The number of bytes transferred. + * + * @note The function is not atomic, if you need atomicity it is suggested + * to use a semaphore or a mutex for mutual exclusion. + */ +size_t chOQWrite(OutputQueue *oqp, uint8_t *buffer, size_t n) { + + size_t w = 0; + while (n--) { + chSysLock(); + if (chOQIsFull(oqp)) { + chSysUnlock(); + break; + } + chSemFastWaitI(&oqp->q_sem); + *oqp->q_wrptr++ = *buffer++; + if (oqp->q_wrptr >= oqp->q_top) + oqp->q_wrptr = oqp->q_buffer; + chSysUnlock(); + w++; + } + if (w && oqp->q_notify) { + chSysLock(); + oqp->q_notify(); + chSysUnlock(); + } + return w; +} +#endif /* CH_USE_QUEUES */ + +/** @} */ diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c new file mode 100644 index 000000000..3ba8b29e9 --- /dev/null +++ b/os/kernel/src/chschd.c @@ -0,0 +1,242 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +/** @cond never */ +ReadyList rlist; +/** @endcond */ + +/** + * @brief Scheduler initialization. + * + * @note Internally invoked by the @p chSysInit(). + */ +void scheduler_init(void) { + + queue_init(&rlist); + rlist.r_prio = NOPRIO; +#if CH_USE_ROUNDROBIN + rlist.r_preempt = CH_TIME_QUANTUM; +#endif +} + +/** + * @brief Inserts a thread in the Ready List. + * + * @param[in] tp the Thread to be made ready + * @return The Thread pointer. + * @note The function does not reschedule, the @p chSchRescheduleS() should + * be called soon after. + */ +#if CH_OPTIMIZE_SPEED +/* NOTE: it is inlined in this module only.*/ +INLINE Thread *chSchReadyI(Thread *tp) { +#else +Thread *chSchReadyI(Thread *tp) { +#endif + Thread *cp; + + tp->p_state = PRREADY; + cp = (Thread *)&rlist; + do { + cp = cp->p_next; + } while (cp->p_prio >= tp->p_prio); + /* Insertion on p_prev.*/ + tp->p_prev = (tp->p_next = 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 @ref thread_states are + * described into @p threads.h. + * + * @param[in] newstate the new thread state + */ +void chSchGoSleepS(tstate_t newstate) { + Thread *otp; + + (otp = currp)->p_state = newstate; + (currp = fifo_remove((void *)&rlist))->p_state = PRCURR; +#if CH_USE_ROUNDROBIN + rlist.r_preempt = CH_TIME_QUANTUM; +#endif + chDbgTrace(otp, currp); + chSysSwitchI(otp, currp); +} + +/* + * Timeout wakeup callback. + */ +static void wakeup(void *p) { + Thread *tp = (Thread *)p; + +#if CH_USE_SEMAPHORES || CH_USE_MUTEXES || CH_USE_CONDVARS + switch (tp->p_state) { +#if CH_USE_SEMAPHORES + case PRWTSEM: + chSemFastSignalI(tp->p_wtsemp); + /* Falls into, intentional. */ +#endif +#if CH_USE_MUTEXES + case PRWTMTX: +#endif +#if CH_USE_CONDVARS + case PRWTCOND: +#endif + /* States requiring dequeuing. */ + dequeue(tp); + } +#endif + chSchReadyI(tp)->p_rdymsg = RDY_TIMEOUT; +} + +/** + * @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 @ref + * thread_states are described 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 accepted but interpreted + * as a normal time specification not as an immediate timeout + * specification. + * . + * @return The wakeup message. + * @retval RDY_TIMEOUT if a timeout occurs. + */ +msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) { + + if (TIME_INFINITE != time) { + VirtualTimer vt; + + chVTSetI(&vt, time, wakeup, currp); + chSchGoSleepS(newstate); + if (chVTIsArmedI(&vt)) + chVTResetI(&vt); + } + else + chSchGoSleepS(newstate); + return currp->p_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. + * + * @param[in] ntp the Thread to be made ready + * @param[in] msg message to the awakened thread + * @note It is equivalent to a @p chSchReadyI() followed by a + * @p chSchRescheduleS() but much more efficient. + */ +void chSchWakeupS(Thread *ntp, msg_t msg) { + + ntp->p_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 *otp = currp; + chSchReadyI(otp); + (currp = ntp)->p_state = PRCURR; +#if CH_USE_ROUNDROBIN + rlist.r_preempt = CH_TIME_QUANTUM; +#endif + chDbgTrace(otp, ntp); + chSysSwitchI(otp, ntp); + } +} + +/** + * @brief Switches to the first thread on the runnable queue. + * + * @note It is intended to be called if @p chSchRescRequiredI() evaluates to + * @p TRUE. + */ +void chSchDoRescheduleI(void) { + + Thread *otp = currp; + /* pick the first thread from the ready queue and makes it current */ + (currp = fifo_remove((void *)&rlist))->p_state = PRCURR; + chSchReadyI(otp); +#if CH_USE_ROUNDROBIN + rlist.r_preempt = CH_TIME_QUANTUM; +#endif + chDbgTrace(otp, currp); + chSysSwitchI(otp, currp); +} + +/** + * @brief Performs a reschedulation 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. + */ +void chSchRescheduleS(void) { + /* first thread in the runnable queue has higher priority than the running + * thread? */ + if (firstprio(&rlist) > currp->p_prio) + chSchDoRescheduleI(); +} + +/** + * @brief Evaluates if a reschedulation is required. + * @details The decision is taken by comparing the relative priorities and + * depending on the state of the round robin timeout counter. + * + * @retval TRUE if there is a thread that should go in running state. + * @retval FALSE if a reschedulation is not required. + */ +bool_t chSchRescRequiredI(void) { + tprio_t p1 = firstprio(&rlist); + tprio_t p2 = currp->p_prio; +#if CH_USE_ROUNDROBIN + /* 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 rlist.r_preempt ? p1 > p2 : p1 >= p2; +#else + /* If the round robin feature is not enabled then performs a simpler + * comparison.*/ + return p1 > p2; +#endif +} + +/** @} */ diff --git a/os/kernel/src/chsem.c b/os/kernel/src/chsem.c new file mode 100644 index 000000000..9a8570580 --- /dev/null +++ b/os/kernel/src/chsem.c @@ -0,0 +1,257 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +#if CH_USE_SEMAPHORES + +#if CH_USE_SEMAPHORES_PRIORITY +#define sem_insert(tp, qp) prio_insert(tp, qp) +#else +#define sem_insert(tp, qp) queue_insert(tp, qp) +#endif + +/** + * @brief Initializes a semaphore with the specified counter value. + * + * @param[out] sp pointer to a @p Semaphore structure + * @param[in] n initial value of the semaphore counter. Must be non-negative. + * @note This function can be invoked from within an interrupt handler even if + * it is not an I-Class API because it does not touch any critical kernel + * data structure. + */ +void chSemInit(Semaphore *sp, cnt_t n) { + + chDbgCheck((sp != NULL) && (n >= 0), "chSemInit"); + + queue_init(&sp->s_queue); + sp->s_cnt = n; +} + +/** + * @brief Performs a reset operation on the semaphore. + * + * @param[in] sp pointer to a @p Semaphore structure + * @param[in] n the new value of the semaphore counter. The value must be non-negative. + * @note The released threads can recognize they were waked up by a reset + * instead than a signal because the @p chSemWait() will return + * @p RDY_RESET instead of @p RDY_OK. + */ +void chSemReset(Semaphore *sp, cnt_t n) { + + chSysLock(); + chSemResetI(sp, n); + chSchRescheduleS(); + chSysUnlock(); +} + +/** + * @brief Performs a reset operation on the semaphore. + * + * @param[in] sp pointer to a @p Semaphore structure + * @param[in] n the new value of the semaphore counter. The value must be non-negative. + * @note The released threads can recognize they were waked up by a reset + * instead than a signal because the @p chSemWait() will return + * @p RDY_RESET instead of @p RDY_OK. + * @note This function does not reschedule. + */ +void chSemResetI(Semaphore *sp, cnt_t n) { + cnt_t cnt; + + chDbgCheck((sp != NULL) && (n >= 0), "chSemResetI"); + + cnt = sp->s_cnt; + sp->s_cnt = n; + while (cnt++ < 0) + chSchReadyI(lifo_remove(&sp->s_queue))->p_rdymsg = RDY_RESET; +} + +/** + * @brief Performs a wait operation on a semaphore. + * + * @param[in] sp pointer to a @p Semaphore structure + * @retval RDY_OK if the semaphore was signaled or not taken. + * @retval RDY_RESET if the semaphore was reset using @p chSemReset(). + */ +msg_t chSemWait(Semaphore *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 structure + * @retval RDY_OK if the semaphore was signaled or not taken. + * @retval RDY_RESET if the semaphore was reset using @p chSemReset(). + * @note This function must be called with interrupts disabled. + * @note This function cannot be called by an interrupt handler. + */ +msg_t chSemWaitS(Semaphore *sp) { + + chDbgCheck(sp != NULL, "chSemWaitS"); + + if (--sp->s_cnt < 0) { + sem_insert(currp, &sp->s_queue); + currp->p_wtsemp = sp; + chSchGoSleepS(PRWTSEM); + return currp->p_rdymsg; + } + return RDY_OK; +} + +/** + * @brief Performs a wait operation on a semaphore with timeout specification. + * + * @param[in] sp pointer to a @p Semaphore 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. + * . + * @retval RDY_OK if the semaphore was signaled or not taken. + * @retval RDY_RESET if the semaphore was reset using @p chSemReset(). + * @retval RDY_TIMEOUT if the semaphore was not signaled or reset within the + * specified timeout. + */ +msg_t chSemWaitTimeout(Semaphore *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 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. + * . + * @retval RDY_OK if the semaphore was signaled or not taken. + * @retval RDY_RESET if the semaphore was reset using @p chSemReset(). + * @retval RDY_TIMEOUT if the semaphore was not signaled or reset within the specified + * timeout. + */ +msg_t chSemWaitTimeoutS(Semaphore *sp, systime_t time) { + + chDbgCheck(sp != NULL, "chSemWaitTimeoutS"); + + if (--sp->s_cnt < 0) { + if (TIME_IMMEDIATE == time) { + sp->s_cnt++; + return RDY_TIMEOUT; + } + sem_insert(currp, &sp->s_queue); + currp->p_wtsemp = sp; + return chSchGoSleepTimeoutS(PRWTSEM, time); + } + return RDY_OK; +} + +/** + * @brief Performs a signal operation on a semaphore. + * + * @param[in] sp pointer to a @p Semaphore structure + * @note The function is available only if the @p CH_USE_SEMAPHORES + * option is enabled in @p chconf.h. + */ +void chSemSignal(Semaphore *sp) { + + chDbgCheck(sp != NULL, "chSemSignal"); + + chSysLock(); + if (sp->s_cnt++ < 0) + chSchWakeupS(fifo_remove(&sp->s_queue), RDY_OK); + chSysUnlock(); +} + +/** + * @brief Performs a signal operation on a semaphore. + * + * @param[in] sp pointer to a @p Semaphore structure + * @note The function is available only if the @p CH_USE_SEMAPHORES + * option is enabled in @p chconf.h. + * @note This function does not reschedule. + */ +void chSemSignalI(Semaphore *sp) { + + chDbgCheck(sp != NULL, "chSemSignalI"); + + if (sp->s_cnt++ < 0) { + /* NOTE: It is done this way in order to allow a tail call on + chSchReadyI().*/ + Thread *tp = fifo_remove(&sp->s_queue); + tp->p_rdymsg = RDY_OK; + chSchReadyI(tp); + } +} + +#if CH_USE_SEMSW +/** + * @brief Performs atomic signal and wait operations on two semaphores. + * + * @param[in] sps pointer to a @p Semaphore structure to be signaled + * @param[in] spw pointer to a @p Semaphore structure to be wait on + * @retval RDY_OK if the semaphore was signaled or not taken. + * @retval RDY_RESET if the semaphore was reset using @p chSemReset(). + * @note The function is available only if the @p CH_USE_SEMSW + * option is enabled in @p chconf.h. + */ +msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) { + msg_t msg; + + chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait"); + + chSysLock(); + if (sps->s_cnt++ < 0) + chSchReadyI(fifo_remove(&sps->s_queue))->p_rdymsg = RDY_OK; + if (--spw->s_cnt < 0) { + sem_insert(currp, &spw->s_queue); + currp->p_wtsemp = spw; + chSchGoSleepS(PRWTSEM); + msg = currp->p_rdymsg; + } + else { + chSchRescheduleS(); + msg = RDY_OK; + } + chSysUnlock(); + return msg; +} +#endif /* CH_USE_SEMSW */ + +#endif /* CH_USE_SEMAPHORES */ + +/** @} */ diff --git a/os/kernel/src/chserial.c b/os/kernel/src/chserial.c new file mode 100644 index 000000000..65179689d --- /dev/null +++ b/os/kernel/src/chserial.c @@ -0,0 +1,168 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 chserial.c + * @brief Serial Drivers code. + * @addtogroup Serial + * @{ + */ + +#include + +#if CH_USE_SERIAL_FULLDUPLEX + +/* + * Interface implementation, the following functions just invoke the equivalent + * queue-level function or macro. + */ +static bool_t putwouldblock(void *instance) { + + return chOQIsFull(&((FullDuplexDriver *)instance)->d2.oqueue); +} + +static bool_t getwouldblock(void *instance) { + + return chIQIsEmpty(&((FullDuplexDriver *)instance)->d2.iqueue); +} + +static msg_t put(void *instance, uint8_t b, systime_t timeout) { + + return chOQPutTimeout(&((FullDuplexDriver *)instance)->d2.oqueue, b, timeout); +} + +static msg_t get(void *instance, systime_t timeout) { + + return chIQGetTimeout(&((FullDuplexDriver *)instance)->d2.iqueue, timeout); +} + +static size_t write(void *instance, uint8_t *buffer, size_t n) { + + return chOQWrite(&((FullDuplexDriver *)instance)->d2.oqueue, buffer, n); +} + +static size_t read(void *instance, uint8_t *buffer, size_t n) { + + return chIQRead(&((FullDuplexDriver *)instance)->d2.iqueue, buffer, n); +} + +static const struct FullDuplexDriverVMT vmt = { + {putwouldblock, getwouldblock, put, get}, + {write, read}, + {} +}; + +/** + * @brief Initializes a generic full duplex driver. + * @details The HW dependent part of the initialization has to be performed + * outside, usually in the hardware initialization code. + * + * @param[out] sd pointer to a @p FullDuplexDriver structure + * @param[in] ib pointer to a memory area allocated for the Input Queue buffer + * @param[in] isize size of the Input Queue buffer + * @param[in] inotify pointer to a callback function that is invoked when + * some data is read from the Queue. The value can be + * @p NULL. + * @param[in] ob pointer to a memory area allocated for the Output Queue buffer + * @param[in] osize size of the Output Queue buffer + * @param[in] onotify pointer to a callback function that is invoked when + * some data is written in the Queue. The value can be + * @p NULL. + */ +void chFDDInit(FullDuplexDriver *sd, + uint8_t *ib, size_t isize, qnotify_t inotify, + uint8_t *ob, size_t osize, qnotify_t onotify) { + + chDbgCheck((sd != NULL) && (ib != NULL) && (ob != NULL) && + (isize > 0) && (osize > 0), "chFDDInit"); + + sd->vmt = &vmt; + chEvtInit(&sd->d1.ievent); + chEvtInit(&sd->d1.oevent); + chEvtInit(&sd->d2.sevent); + sd->d2.flags = SD_NO_ERROR; + chIQInit(&sd->d2.iqueue, ib, isize, inotify); + chOQInit(&sd->d2.oqueue, ob, osize, onotify); +} + +/** + * @brief Handles incoming data. + * @details This function must be called from the input interrupt service + * routine in order to enqueue incoming data and generate the + * related events. + * @param[in] sd pointer to a @p FullDuplexDriver structure + * @param[in] b the byte to be written in the driver's Input Queue + */ +void chFDDIncomingDataI(FullDuplexDriver *sd, uint8_t b) { + + if (chIQPutI(&sd->d2.iqueue, b) < Q_OK) + chFDDAddFlagsI(sd, SD_OVERRUN_ERROR); + else + chEvtBroadcastI(&sd->d1.ievent); +} + +/** + * @brief Handles outgoing data. + * @details Must be called from the output interrupt service routine in order + * to get the next byte to be transmitted. + * + * @param[in] sd pointer to a @p FullDuplexDriver structure + * @return The byte value read from the driver's output queue. + * @retval Q_EMPTY if the queue is empty (the lower driver usually disables + * the interrupt source when this happens). + */ +msg_t chFDDRequestDataI(FullDuplexDriver *sd) { + + msg_t b = chOQGetI(&sd->d2.oqueue); + if (b < Q_OK) + chEvtBroadcastI(&sd->d1.oevent); + return b; +} + +/** + * @brief Handles communication events/errors. + * @details Must be called from the I/O interrupt service routine in order to + * notify I/O conditions as errors, signals change etc. + * + * @param[in] sd pointer to a @p FullDuplexDriver structure + * @param[in] mask condition flags to be added to the mask + */ +void chFDDAddFlagsI(FullDuplexDriver *sd, dflags_t mask) { + + sd->d2.flags |= mask; + chEvtBroadcastI(&sd->d2.sevent); +} + +/** + * @brief Returns and clears the errors mask associated to the driver. + * + * @param[in] sd pointer to a @p FullDuplexDriver structure + * @return The condition flags modified since last time this function was + * invoked. + */ +dflags_t chFDDGetAndClearFlags(FullDuplexDriver *sd) { + dflags_t mask; + + mask = sd->d2.flags; + sd->d2.flags = SD_NO_ERROR; + return mask; +} +#endif /* CH_USE_SERIAL_FULLDUPLEX */ + +/** @} */ diff --git a/os/kernel/src/chsys.c b/os/kernel/src/chsys.c new file mode 100644 index 000000000..217e2f2da --- /dev/null +++ b/os/kernel/src/chsys.c @@ -0,0 +1,129 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +static WORKING_AREA(idle_thread_wa, IDLE_THREAD_STACK_SIZE); + +/** + * @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) { + + while (TRUE) { + port_wait_for_interrupt(); + IDLE_LOOP_HOOK(); + } +} + +/** + * @brief ChibiOS/RT initialization. + * @details After executing this function the current instructions stream + * becomes the main thread. + * + * @note Interrupts should be still disabled when @p chSysInit() is invoked + * and are internally enabled. + * @note The main thread is created with priority @p NORMALPRIO. + */ +void chSysInit(void) { + static Thread mainthread; + + port_init(); + scheduler_init(); + vt_init(); +#if CH_USE_HEAP + heap_init(); +#endif +#if CH_DBG_ENABLE_TRACE + trace_init(); +#endif + + /* + * Now this instructions flow becomes the main thread. + */ + (currp = init_thread(&mainthread, NORMALPRIO))->p_state = PRCURR; + chSysEnable(); + + /* + * 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); +} + +/** + * @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_TIME_QUANTUM macro, the round robin interval. + */ +void chSysTimerHandlerI(void) { + +#if CH_USE_ROUNDROBIN + /* running thread has not used up quantum yet? */ + if (rlist.r_preempt > 0) + /* decrement remaining quantum */ + rlist.r_preempt--; +#endif +#if CH_DBG_THREADS_PROFILING + currp->p_time++; +#endif + chVTDoTickI(); +} + +#if CH_USE_NESTED_LOCKS && !CH_OPTIMIZE_SPEED +void chSysLock(void) { + + chDbgAssert(currp->p_locks >= 0, + "chSysLock(), #1", + "negative nesting counter"); + if (currp->p_locks++ == 0) + port_lock(); +} + +void chSysUnlock(void) { + + chDbgAssert(currp->p_locks > 0, + "chSysUnlock(), #1", + "non-positive nesting counter"); + if (--currp->p_locks == 0) + port_unlock(); +} +#endif /* CH_USE_NESTED_LOCKS && !CH_OPTIMIZE_SPEED */ + +/** @} */ diff --git a/os/kernel/src/chthreads.c b/os/kernel/src/chthreads.c new file mode 100644 index 000000000..f8bb3b869 --- /dev/null +++ b/os/kernel/src/chthreads.c @@ -0,0 +1,381 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 + * @{ + */ + +#include + +/* + * Initializes a thread structure. + */ +Thread *init_thread(Thread *tp, tprio_t prio) { + + tp->p_flags = P_MEM_MODE_STATIC; + tp->p_prio = prio; + tp->p_state = PRSUSPENDED; +#if CH_USE_NESTED_LOCKS + tp->p_locks = 0; +#endif +#if CH_DBG_THREADS_PROFILING + tp->p_time = 0; +#endif +#if CH_USE_MUTEXES + /* realprio is the thread's own, non-inherited, priority */ + tp->p_realprio = prio; + tp->p_mtxlist = NULL; +#endif +#if CH_USE_WAITEXIT + tp->p_waiting = NULL; +#endif +#if CH_USE_MESSAGES + queue_init(&tp->p_msgqueue); +#endif +#if CH_USE_EVENTS + tp->p_epending = 0; +#endif + THREAD_EXT_INIT(tp); + return tp; +} + +#if CH_DBG_FILL_THREADS +static void memfill(uint8_t *startp, uint8_t *endp, uint8_t v) { + + while (startp < endp) + *startp++ = v; +} +#endif + +/** + * @brief Initializes a new thread. + * @details The new thread is initialized but not inserted in the ready list, + * the initial state is @p PRSUSPENDED. + * + * @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 structure allocated for the + * thread into the working space area. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + * @note This function can be invoked from within an interrupt handler even if + * it is not an I-Class API because it does not touch any critical kernel + * data structure. + */ +Thread *chThdInit(void *wsp, size_t size, tprio_t prio, tfunc_t pf, void *arg) { + /* Thread structure is layed out in the lower part of the thread workspace */ + Thread *tp = wsp; + + chDbgCheck((wsp != NULL) && (size >= THD_WA_SIZE(0)) && + (prio <= HIGHPRIO) && (pf != NULL), + "chThdInit"); +#if CH_DBG_FILL_THREADS + memfill((uint8_t *)wsp, (uint8_t *)wsp + sizeof(Thread), THREAD_FILL_VALUE); + memfill((uint8_t *)wsp + sizeof(Thread), + (uint8_t *)wsp + size, STACK_FILL_VALUE); +#endif + SETUP_CONTEXT(wsp, size, pf, arg); + return init_thread(tp, prio); +} + +/** + * @brief Creates a new thread into a static memory area. + * + * @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 structure allocated for the + * thread into the working space area. + * @note A thread can terminate by calling @p chThdExit() or by simply + * returning from its main function. + */ +Thread *chThdCreateStatic(void *wsp, size_t size, + tprio_t prio, tfunc_t pf, void *arg) { + + return chThdResume(chThdInit(wsp, size, prio, pf, arg)); +} + +#if CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_HEAP +/** + * @brief Creates a new thread allocating the memory from the 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 structure allocated for the + * thread into the working space area. + * @retval NULL if the memory cannot be allocated. + * @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. + * @note The function is available only if the @p CH_USE_DYNAMIC, + * @p CH_USE_HEAP and @p CH_USE_WAITEXIT options are enabled + * in @p chconf.h. + */ +Thread *chThdCreateFromHeap(size_t size, tprio_t prio, tfunc_t pf, void *arg) { + void *wsp; + Thread *tp; + + wsp = chHeapAlloc(size); + if (wsp == NULL) + return NULL; + tp = chThdInit(wsp, size, prio, pf, arg); + tp->p_flags = P_MEM_MODE_HEAP; + return chThdResume(tp); +} +#endif /* CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_HEAP */ + +#if CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_MEMPOOLS +/** + * @brief Creates a new thread allocating the memory from the specified Memory + * Pool. + * + * @param[in] mp the memory pool + * @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 structure allocated for the + * thread into the working space area or @p NULL if the memory cannot + * be allocated. + * @retval NULL if the memory pool is empty. + * @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. + * @note The function is available only if the @p CH_USE_DYNAMIC, + * @p CH_USE_MEMPOOLS and @p CH_USE_WAITEXIT options are enabled + * in @p chconf.h. + */ +Thread *chThdCreateFromMemoryPool(MemoryPool *mp, tprio_t prio, + tfunc_t pf, void *arg) { + void *wsp; + Thread *tp; + + chDbgCheck(mp != NULL, "chThdCreateFromMemoryPool"); + + wsp = chPoolAlloc(mp); + if (wsp == NULL) + return NULL; + tp = chThdInit(wsp, mp->mp_object_size, prio, pf, arg); + tp->p_flags = P_MEM_MODE_MEMPOOL; + tp->p_mpool = mp; + return chThdResume(tp); +} +#endif /* CH_USE_DYNAMIC && CH_USE_WAITEXIT && CH_USE_MEMPOOLS */ + +/** + * @brief Changes the running thread priority level then reschedules if + * necessary. + * + * @param[in] newprio the new priority level of the running thread + * @return The old priority level. + * @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. + */ +tprio_t chThdSetPriority(tprio_t newprio) { + tprio_t oldprio; + + chDbgCheck((newprio >= LOWPRIO) && (newprio <= HIGHPRIO), + "chThdSetPriority"); + + chSysLock(); +#if CH_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. + * + * @param[in] tp the pointer to the thread + * @return The pointer to the thread. + * @note This call is supposed to resume threads created with @p chThdInit(). + * It should not be used on threads suspended using @p chThdSuspend(). + */ +Thread *chThdResume(Thread *tp) { + + chSysLock(); + chDbgAssert(tp->p_state == PRSUSPENDED, + "chThdResume(), #1", + "thread not in PRSUSPENDED state"); + chSchWakeupS(tp, RDY_OK); + chSysUnlock(); + return tp; +} + +/** + * @brief Requests a thread termination. + * + * @param[in] tp the pointer to the thread + * @note The thread is not termitated but a termination request is added to + * its @p p_flags field. The thread can read this status by + * invoking @p chThdShouldTerminate() and then terminate cleanly. + */ +void chThdTerminate(Thread *tp) { + + chSysLock(); + tp->p_flags |= P_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 accepted but interpreted + * as a normal time specification not as an immediate timeout + * specification. + * . + */ +void chThdSleep(systime_t time) { + + chDbgCheck(time != TIME_INFINITE, "chThdSleep"); + + chSysLock(); + chThdSleepS(time); + chSysUnlock(); +} + +/** + * @brief Suspends the invoking thread until the system time arrives to the + * specified value. + * + * @param[in] time the absolute system time + */ +void chThdSleepUntil(systime_t time) { + + chSysLock(); + if ((time -= chTimeNow()) > 0) + chThdSleepS(time); + chSysUnlock(); +} + +/** + * @brief Terminates the current thread by specifying an exit status code. + * + * @param[in] msg the thread exit code. The code can be retrieved by using + * @p chThdWait(). + */ +void chThdExit(msg_t msg) { + Thread *tp = currp; + + chSysLock(); + tp->p_exitcode = msg; + THREAD_EXT_EXIT(tp); +#if CH_USE_WAITEXIT + if (tp->p_waiting != NULL) + chSchReadyI(tp->p_waiting); +#endif + chSchGoSleepS(PREXIT); +} + +#if CH_USE_WAITEXIT +/** + * @brief Blocks the execution of the invoking thread until the specified + * thread terminates then the exit code is returned. + * @details 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 chThdInit() 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. + * . + * @param[in] tp the thread pointer + * @return The exit code from the terminated thread + * @note After invoking @p chThdWait() the thread pointer becomes invalid and + * must not be used as parameter for further system calls. + * @note The function is available only if the @p CH_USE_WAITEXIT + * option is enabled in @p chconf.h. + * @note Only one thread can be waiting for another thread at any time. You + * should imagine the threads as having a reference counter that is set + * to one when the thread is created, chThdWait() decreases the reference + * and the memory is freed when the counter reaches zero. In the current + * implementation there is no real reference counter in the thread + * structure but it is a planned extension. + */ +msg_t chThdWait(Thread *tp) { + msg_t msg; + + chDbgCheck(tp != NULL, "chThdWait"); + + chSysLock(); + + chDbgAssert(tp != currp, "chThdWait(), #1", "waiting self"); + chDbgAssert(tp->p_waiting == NULL, "chThdWait(), #2", "some other thread waiting"); + + if (tp->p_state != PREXIT) { + tp->p_waiting = currp; + chSchGoSleepS(PRWAIT); + } + msg = tp->p_exitcode; +#if !CH_USE_DYNAMIC + chSysUnlock(); + return msg; +#else /* CH_USE_DYNAMIC */ + + /* Returning memory.*/ + tmode_t mode = tp->p_flags & P_MEM_MODE_MASK; + chSysUnlock(); + + switch (mode) { +#if CH_USE_HEAP + case P_MEM_MODE_HEAP: + chHeapFree(tp); + break; +#endif +#if CH_USE_MEMPOOLS + case P_MEM_MODE_MEMPOOL: + chPoolFree(tp->p_mpool, tp); + break; +#endif + } + return msg; +#endif /* CH_USE_DYNAMIC */ +} +#endif /* CH_USE_WAITEXIT */ + +/** @} */ diff --git a/os/kernel/src/chvt.c b/os/kernel/src/chvt.c new file mode 100644 index 000000000..c1d8734ec --- /dev/null +++ b/os/kernel/src/chvt.c @@ -0,0 +1,116 @@ +/* + ChibiOS/RT - Copyright (C) 2006-2007 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 related code. + * @addtogroup Time + * @{ + */ + +#include + +VTList vtlist; + +/** + * @brief Virtual Timers initialization. + * + * @note Internal use only. + */ +void vt_init(void) { + + vtlist.vt_next = vtlist.vt_prev = (void *)&vtlist; + vtlist.vt_time = (systime_t)-1; + vtlist.vt_systime = 0; +} + +/** + * @brief Enables a virtual timer. + * + * @param[out] vtp the @p VirtualTimer structure pointer + * @param[in] time the number of time ticks, the value @p TIME_INFINITE is not + * allowed. The value @p TIME_IMMEDIATE is allowed but + * interpreted as a normal time specification not as an + * immediate timeout specification. + * @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 + * @note The associated function is invoked by an interrupt handler within + * the I-Locked state, see @ref system_states. + */ +void chVTSetI(VirtualTimer *vtp, systime_t time, vtfunc_t vtfunc, void *par) { + VirtualTimer *p; + + chDbgCheck((vtp != NULL) && (vtfunc != NULL) && (time != TIME_INFINITE), + "chVTSetI"); + + vtp->vt_par = par; + vtp->vt_func = vtfunc; + p = vtlist.vt_next; + while (p->vt_time < time) { + time -= p->vt_time; + p = p->vt_next; + } + + vtp->vt_prev = (vtp->vt_next = p)->vt_prev; + vtp->vt_prev->vt_next = p->vt_prev = vtp; + vtp->vt_time = time; + if (p != (void *)&vtlist) + p->vt_time -= time; +} + +/** + * @brief Disables a Virtual Timer. + * + * @param[in] vtp the @p VirtualTimer structure pointer + * @note The timer MUST be active when this function is invoked. + */ +void chVTResetI(VirtualTimer *vtp) { + + chDbgCheck(vtp != NULL, "chVTResetI"); + chDbgAssert(vtp->vt_func != NULL, + "chVTResetI(), #1", + "timer not set or already triggered"); + + if (vtp->vt_next != (void *)&vtlist) + vtp->vt_next->vt_time += vtp->vt_time; + vtp->vt_prev->vt_next = vtp->vt_next; + vtp->vt_next->vt_prev = vtp->vt_prev; + vtp->vt_func = NULL; +} + +/** + * @brief Checks if the current system time is within the specified time window. + * + * @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. + * @note When start==end then the function returns always true because the + * whole time range is specified. + */ +bool_t chTimeIsWithin(systime_t start, systime_t end) { + + systime_t time = chTimeNow(); + return end > start ? (time >= start) && (time < end) : + (time >= start) || (time < end); +} + +/** @} */ -- cgit v1.2.3