1 /*
2 * Copyright (c) 2006-2021, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2021-04-27 flybreak the first version.
9 */
10
11 #include <arm-tpl.h>
12 #include "tpl.h"
13 #include <new>
14 #include <cstdint>
15 #include <stdatomic.h>
16
arm_tpl_cv()17 arm_tpl_cv::arm_tpl_cv()
18 {
19 s = rt_sem_create("semxs", 0, RT_IPC_FLAG_PRIO);
20 if (s == nullptr)
21 RT_ASSERT(0);
22 h = rt_sem_create("semxh", 0, RT_IPC_FLAG_PRIO);
23 if (h == nullptr)
24 {
25 rt_sem_delete(s);
26 RT_ASSERT(0);
27 }
28 x = rt_mutex_create("mutx", RT_IPC_FLAG_PRIO);
29 if (x == nullptr)
30 {
31 rt_sem_delete(s);
32 rt_sem_delete(h);
33 RT_ASSERT(0);
34 }
35 }
36
~arm_tpl_cv()37 arm_tpl_cv::~arm_tpl_cv()
38 {
39 rt_mutex_delete(x);
40 rt_sem_delete(h);
41 rt_sem_delete(s);
42 }
43
wait(rt_mutex_t lock,bool recursive)44 void arm_tpl_cv::wait(rt_mutex_t lock, bool recursive)
45 {
46 while (rt_mutex_take(x, ARM_TPL_MAX_DELAY) != 0);
47 rt_sem_release(s);
48 rt_mutex_release(x);
49 if (recursive)
50 rt_mutex_release(lock);
51 else
52 rt_mutex_release(lock);
53 while (rt_sem_take(h, ARM_TPL_MAX_DELAY) != 0);
54 if (recursive)
55 while (rt_mutex_take(lock, ARM_TPL_MAX_DELAY) != 0);
56 else
57 while (rt_mutex_take(lock, ARM_TPL_MAX_DELAY) != 0);
58 }
59
timedwait(rt_mutex_t lock,bool recursive,unsigned int timeout_ms)60 int arm_tpl_cv::timedwait(rt_mutex_t lock, bool recursive, unsigned int timeout_ms)
61 {
62 int result = 0;
63 while (rt_mutex_take(x, ARM_TPL_MAX_DELAY) != 0);
64 rt_sem_release(s);
65 rt_mutex_release(x);
66 if (recursive)
67 rt_mutex_release(lock);
68 else
69 rt_mutex_release(lock);
70 if (rt_sem_take(h, rt_tick_from_millisecond(timeout_ms)) != 0)
71 {
72 while (rt_mutex_take(x, ARM_TPL_MAX_DELAY) != 0);
73 if (rt_sem_take(h, 0) != 0)
74 {
75 if (rt_sem_take(s, 0) != 0)
76 result = -1;
77 else
78 result = 1;
79 }
80 rt_mutex_release(x);
81 }
82 if (recursive)
83 while (rt_mutex_take(lock, ARM_TPL_MAX_DELAY) != 0);
84 else
85 while (rt_mutex_take(lock, ARM_TPL_MAX_DELAY) != 0);
86 return result;
87 }
88
signal()89 void arm_tpl_cv::signal()
90 {
91 while (rt_mutex_take(x, ARM_TPL_MAX_DELAY) != 0);
92 if (rt_sem_take(s, 0) == 0)
93 rt_sem_release(h);
94 rt_mutex_release(x);
95 }
96
broadcast()97 void arm_tpl_cv::broadcast()
98 {
99 while (rt_mutex_take(x, ARM_TPL_MAX_DELAY) != 0);
100 auto count = s->value;
101 for (auto i = 0; i < count; i++)
102 {
103 while (rt_sem_take(s, ARM_TPL_MAX_DELAY) != 0);
104 rt_sem_release(h);
105 }
106 rt_mutex_release(x);
107 }
108
check_create(volatile __ARM_TPL_condvar_t * __vcv)109 static int check_create(volatile __ARM_TPL_condvar_t *__vcv)
110 {
111 if (__vcv->data == 0)
112 {
113 uintptr_t cv_new;
114 cv_new = reinterpret_cast<uintptr_t>(new arm_tpl_cv());
115 if (cv_new == 0)
116 {
117 return -1;
118 }
119 uintptr_t cv_null = 0;
120 if (!atomic_compare_exchange_strong(&__vcv->data, &cv_null, cv_new))
121 delete reinterpret_cast<arm_tpl_cv *>(cv_new);
122 }
123 return 0;
124 }
125
__ARM_TPL_condvar_wait(__ARM_TPL_condvar_t * __cv,__ARM_TPL_mutex_t * __m)126 extern "C" int __ARM_TPL_condvar_wait(__ARM_TPL_condvar_t *__cv, __ARM_TPL_mutex_t *__m)
127 {
128 volatile __ARM_TPL_condvar_t *__vcv = __cv;
129 if (check_create(__vcv) != 0)
130 return -1;
131 struct arm_tpl_mutex_struct *tmutex = (struct arm_tpl_mutex_struct *)(__m->data);
132 ((arm_tpl_cv *) __vcv->data)->wait(tmutex->mutex, tmutex->type == RECURSIVE);
133 return 0;
134 }
135
__ARM_TPL_condvar_timedwait(__ARM_TPL_condvar_t * __cv,__ARM_TPL_mutex_t * __m,__ARM_TPL_timespec_t * __ts)136 extern "C" int __ARM_TPL_condvar_timedwait(__ARM_TPL_condvar_t *__cv,
137 __ARM_TPL_mutex_t *__m,
138 __ARM_TPL_timespec_t *__ts)
139 {
140 volatile __ARM_TPL_condvar_t *__vcv = __cv;
141 if (check_create(__vcv) != 0)
142 return -1;
143 __ARM_TPL_timespec_t now;
144 if (__ARM_TPL_clock_realtime(&now) != 0)
145 return -1;
146 struct arm_tpl_mutex_struct *tmutex = (struct arm_tpl_mutex_struct *)(__m->data);
147 unsigned int timeout_ms = (__ts->tv_sec - now.tv_sec) * 1000 + (__ts->tv_nsec - now.tv_nsec) / 1000000;
148 if (((arm_tpl_cv *) __vcv->data)->timedwait(tmutex->mutex, tmutex->type == RECURSIVE, timeout_ms) < 0)
149 return -1;
150 return 0;
151 }
152
__ARM_TPL_condvar_signal(__ARM_TPL_condvar_t * __cv)153 extern "C" int __ARM_TPL_condvar_signal(__ARM_TPL_condvar_t *__cv)
154 {
155 volatile __ARM_TPL_condvar_t *__vcv = __cv;
156 if (__vcv->data != 0)
157 ((arm_tpl_cv *) __vcv->data)->signal();
158 return 0;
159 }
160
__ARM_TPL_condvar_broadcast(__ARM_TPL_condvar_t * __cv)161 extern "C" int __ARM_TPL_condvar_broadcast(__ARM_TPL_condvar_t *__cv)
162 {
163 volatile __ARM_TPL_condvar_t *__vcv = __cv;
164 if (__vcv->data != 0)
165 ((arm_tpl_cv *) __vcv->data)->broadcast();
166 return 0;
167 }
168
__ARM_TPL_condvar_destroy(__ARM_TPL_condvar_t * __cv)169 extern "C" int __ARM_TPL_condvar_destroy(__ARM_TPL_condvar_t *__cv)
170 {
171 volatile __ARM_TPL_condvar_t *__vcv = __cv;
172 if (__vcv->data != 0)
173 {
174 delete (arm_tpl_cv *) __vcv->data;
175 __vcv->data = 0;
176 }
177 return 0;
178 }
179