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
|
/*
ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
/**
* @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) {
chDbgAssert(tr < &nil.threads[NIL_CFG_NUM_THREADS],
"pointer out of range");
/* 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++;
}
}
/** @} */
|
