261 lines6.2 KB

Newer

Older

Text Raw Blame

9 months ago		Split down into component parts	37	1	#include "asleep.h"
				2	#include "task.h"
				3	#include <assert.h>
				4	#include <stdio.h>
				5	#include <stdlib.h>
				6	#include <time.h>
				7	#include <errno.h>
				8	#include <math.h>
				9	#include <string.h>
				10	#include "timespec_utils.h"
				11
				12
				13	typedef struct Loop_Timer {
				14	struct timespec when;
				15	Future *who;
				16	} Loop_Timer;
				17
				18	typedef struct _ASleep {
				19	pthread_mutex_t mutex;
				20	unsigned users;
				21	Loop_Timer *sleepers;
				22	int lo;
				23	int hi;
				24	int max;
				25	pthread_cond_t cond;
				26	bool sleeploop_cancel;
				27	pthread_t sleeploop;
				28	bool started;
				29	} _ASleep;
				30
				31	static _ASleep asleep;
				32
				33	static void ASleep_Sleeper(void param);
				34
				35	static void ASleep_ctor(
				36	_ASleep *me
				37	){
				38	int r;
				39
				40	me->sleepers = NULL;
				41	me->lo = 0;
				42	me->hi = 0;
				43	me->max = 0;
				44
				45	r = pthread_mutex_init(&me->mutex, NULL);
				46	assert(r == 0);
				47	r = pthread_cond_init(&me->cond, NULL);
				48	assert(r == 0);
				49	me->sleeploop_cancel = false;
				50	r = pthread_create(&me->sleeploop, NULL, ASleep_Sleeper, me);
				51	assert(r == 0);
				52	me->started = true;
				53	}
				54
				55
				56
				57	static void ASleep_dtor(
				58	_ASleep *me
				59	){
				60	int r;
				61	me->started = false;
				62	me->sleeploop_cancel = true;
				63	r = pthread_cond_signal(&me->cond);
				64	assert(r == 0);
65	r = pthread_join(me->sleeploop, NULL);
66	assert(r == 0);
67	r = pthread_cond_destroy(&me->cond);
68	assert(r == 0);
69	pthread_mutex_destroy(&me->mutex);
70	assert(r == 0);
71	free(me->sleepers);
72	}
73
74
75	void ASleep_StartSystem(){
76	ASleep_ctor(&asleep);
77	}
78
79
80	void ASleep_StopSystem(){
81	ASleep_dtor(&asleep);
82	}
83
84
85	static void ASleep_AddSleeper(
86	_ASleep *me,
87	struct timespec when,
88	Future *who
89	){
90	int r = pthread_mutex_lock(&me->mutex);
91	assert(r == 0);
92
93	int nsleepers;
94	if (me->lo <= me->hi){
95	int nsleepers = me->hi - me->lo;
96	// ensure there's always an empty slot so that lo == hi always means no sleepers
97	if (nsleepers+1 >= me->max) {
98	me->max = (me->max == 0) ? 4 : me->max * 2;
99	me->sleepers = realloc(me->sleepers, me->max * sizeof(Loop_Timer));
100	assert(me->sleepers);
101	}
102	} else {
103	nsleepers = me->hi + me->max - me->lo;
104	// ensure there's always an empty slot so that lo == hi always means no sleepers
105	if (nsleepers+1 >= me->max) {
106	int oldmax = me->max;
107	me->max = (me->max == 0) ? 4 : me->max * 2;
108	me->sleepers = realloc(me->sleepers, me->max * sizeof(Loop_Timer));
109	assert(me->sleepers);
110	memmove(&me->sleepers[me->lo], &me->lo + me->max - oldmax, sizeof(me->sleepers) oldmax - me->lo);
111	me->lo += me->max - oldmax;
112	}
113	}
114
115	// insertion sort - we're assuming there's not going to be many sleepers active at once
116	int i;
117	int nexti;
118	for (i = me->hi; i != me->lo; i = nexti){
119	nexti = i-1;
120	if (i < 0){
121	nexti += me->max;
122	}
123	if (timespec_lte(me->sleepers[nexti].when, when)){
124	break;
125	}
126	me->sleepers[i] = me->sleepers[nexti];
127	}
128	me->sleepers[i].when = when;
129	me->sleepers[i].who = who;
130	me->hi += 1;
131	if (me->hi >= me->max){
132	me->hi -= me->max;
133	}
134
135	r = pthread_cond_signal(&me->cond);
136	assert(r == 0);
137
138	r = pthread_mutex_unlock(&me->mutex);
139	assert(r == 0);
140	}
141
142
143	static void ASleep_RemoveSleeper(
144	_ASleep *me,
145	struct timespec when,
146	Future *who
147	){
148	int r = pthread_mutex_lock(&me->mutex);
149	assert(r == 0);
150
151	int i;
152	for (i = me->lo; i != me->hi; ){
153	if (timespec_eq(me->sleepers[i].when, when) && me->sleepers[i].who == who){
154	int previ = i;
155	i += 1;
156	if (i >= me->max){
157	i -= me->max;
158	}
159	for (; i != me->hi; ){
160	me->sleepers[previ] = me->sleepers[i];
161	previ = i;
162	i += 1;
163	if (i >= me->max){
164	i -= me->max;
165	}
166	}
167	me->hi = previ;
168	break;
169	}
170	i += 1;
171	if (i >= me->max){
172	i -= me->max;
173	}
174	}
175
176	r = pthread_cond_signal(&me->cond);
177	assert(r == 0);
178
179	r = pthread_mutex_unlock(&me->mutex);
180	assert(r == 0);
181	}
182
183
184	static void *ASleep_Sleeper(
185	void *param
186	){
187	_ASleep me = (_ASleep )param;
188	int r;
189
190	while (!me->sleeploop_cancel){
191	r = pthread_mutex_lock(&me->mutex);
192	assert(r == 0);
193	if (me->lo != me->hi){
194	struct timespec when = me->sleepers[0].when;
195
196	r = pthread_mutex_unlock(&me->mutex);
197	assert(r == 0);
198	r = pthread_cond_timedwait(&me->cond, &me->mutex, &when);
199	assert(r == 0 \|\| r == ETIMEDOUT);
200	} else {
201	r = pthread_mutex_unlock(&me->mutex);
202	assert(r == 0);
203	r = pthread_cond_wait(&me->cond, &me->mutex);
204	assert(r == 0);
205	}
206	// issue any due timers
207	struct timespec now;
208	r = clock_gettime(CLOCK_REALTIME, &now);
209	assert(r == 0);
210	while(me->lo != me->hi && timespec_lte(me->sleepers[me->lo].when, now)) {
211	Future *fut = me->sleepers[me->lo].who;
212	me->lo += 1;
213	if (me->lo >= me->max){
214	me->lo -= me->max;
215	}
216	r = pthread_mutex_unlock(&me->mutex);
217	assert(r == 0);
218	Future_SetResult(fut, false, NULL);
219	r = pthread_mutex_lock(&me->mutex);
220	assert(r == 0);
221	r = clock_gettime(CLOCK_REALTIME, &now);
222	assert(r == 0);
223	}
224	r = pthread_mutex_unlock(&me->mutex);
225	assert(r == 0);
226	}
227
228	return NULL;
229	}
230
231
232	// value is a pointer to a float delay in seconds
233	bool ASleep(
234	float delay,
235	void **value
236	){
237	assert(current_task && asleep.started);
238	if (delay < 0){
239	delay = 0;
240	}
241	float secs = floorf(delay);
242	float nsecs = (delay - secs) * 1e9;
243	struct timespec endtime;
244	clock_gettime(CLOCK_REALTIME, &endtime);
245	endtime.tv_nsec += (int)nsecs;
246	if (endtime.tv_nsec > 1000000000){
247	endtime.tv_sec += 1;
248	endtime.tv_nsec -= 1000000000;
249	}
250	endtime.tv_sec += (int)secs;
251
252	Future fut;
253	Future_ctor(&fut);
254	ASleep_AddSleeper(&asleep, endtime, &fut);
255	bool res = Future_Await(&fut, value);
256	ASleep_RemoveSleeper(&asleep, endtime, &fut);
257	Future_dtor(&fut);
258
259	return res;
260	}
261