1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
|
/*
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio.
This file is part of ChibiOS.
ChibiOS is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.
ChibiOS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* @file osal.c
* @brief OSAL module code.
*
* @addtogroup OSAL
* @{
*/
#include "osal.h"
/*===========================================================================*/
/* Module local definitions. */
/*===========================================================================*/
/*===========================================================================*/
/* Module exported variables. */
/*===========================================================================*/
/*===========================================================================*/
/* Module local types. */
/*===========================================================================*/
/*===========================================================================*/
/* Module local variables. */
/*===========================================================================*/
/*===========================================================================*/
/* Module local functions. */
/*===========================================================================*/
/*===========================================================================*/
/* Module exported functions. */
/*===========================================================================*/
/**
* @brief Dequeues and wakes up one thread from the queue, if any.
*
* @param[in] tqp pointer to the threads queue object
* @param[in] msg the message code
*
* @iclass
*/
void osalThreadDequeueNextI(threads_queue_t *tqp, msg_t msg) {
semaphore_t *sp = &tqp->sem;
if (chSemGetCounterI(&tqp->sem) < (cnt_t)0) {
thread_reference_t tr = nil.threads;
while (true) {
/* Is this thread waiting on this semaphore?*/
if (tr->u1.semp == sp) {
sp->cnt++;
chDbgAssert(NIL_THD_IS_WTSEM(tr), "not waiting");
(void) chSchReadyI(tr, msg);
return;
}
tr++;
chDbgAssert(tr < &nil.threads[NIL_CFG_NUM_THREADS],
"pointer out of range");
}
}
}
/**
* @brief Dequeues and wakes up all threads from the queue.
*
* @param[in] tqp pointer to the threads queue object
* @param[in] msg the message code
*
* @iclass
*/
void osalThreadDequeueAllI(threads_queue_t *tqp, msg_t msg) {
semaphore_t *sp = &tqp->sem;
thread_reference_t tr;
cnt_t cnt;
cnt = sp->cnt;
sp->cnt = (cnt_t)0;
tr = nil.threads;
while (cnt < (cnt_t)0) {
/* Is this thread waiting on this semaphore?*/
if (tr->u1.semp == sp) {
chDbgAssert(NIL_THD_IS_WTSEM(tr), "not waiting");
cnt++;
(void) chSchReadyI(tr, msg);
}
tr++;
chDbgAssert(tr < &nil.threads[NIL_CFG_NUM_THREADS],
"pointer out of range");
}
}
/** @} */
|
