timer.c 11.6 KB
Newer Older
Stefan Reif's avatar
Stefan Reif committed
1 2 3
#include "timer.h"

#include <assert.h>
Stefan Reif's avatar
Stefan Reif committed
4 5 6 7 8
#include <errno.h>
#include <pthread.h>
#include <sched.h>
#include <stdatomic.h>
#include <stdbool.h>
Stefan Reif's avatar
Stefan Reif committed
9 10
#include <stdlib.h>
#include <string.h>
Stefan Reif's avatar
Stefan Reif committed
11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
#include <stdio.h>
#include <time.h>
#include <limits.h>
#include <linux/futex.h>
#include <sys/time.h>
#include <sys/syscall.h>
#include <sys/prctl.h>
#include <unistd.h>

typedef prrtTimerDate    TimerDate;
typedef prrtTimerTaskFun TimerTaskFun;
typedef prrtTimerTaskArg TimerTaskArg;

typedef unsigned long long TimerDateUDiff_t;
#define TimerDateUDiff_MAX ULLONG_MAX

#define NSEC_PER_SEC 1000000000
Stefan Reif's avatar
Stefan Reif committed
28

Stefan Reif's avatar
Stefan Reif committed
29 30 31 32
static inline void timer_date_make_inf(TimerDate *td)
{
	td->tv_sec = td->tv_nsec = 0;
}
Stefan Reif's avatar
Stefan Reif committed
33

Stefan Reif's avatar
Stefan Reif committed
34
static inline bool timer_date_is_inf(const TimerDate *td)
Stefan Reif's avatar
Stefan Reif committed
35
{
Stefan Reif's avatar
Stefan Reif committed
36 37
	return !td->tv_sec && !td->tv_nsec;
}
Stefan Reif's avatar
Stefan Reif committed
38

Stefan Reif's avatar
Stefan Reif committed
39 40 41 42 43
static inline TimerDateUDiff_t timer_duration_finite(const TimerDate *t1, const TimerDate *t2)
{
	// assume that t1 and t2 are finite
	return (t2->tv_sec - t1->tv_sec) * NSEC_PER_SEC + (t2->tv_nsec - t1->tv_nsec);
}
Stefan Reif's avatar
Stefan Reif committed
44

Stefan Reif's avatar
Stefan Reif committed
45 46 47 48 49 50
static inline void timer_date_add(TimerDate *td, TimerDateUDiff_t nsec)
{
	assert(td->tv_nsec >= 0 && "negative time");
	while (td->tv_nsec >= NSEC_PER_SEC) {
		td->tv_sec++;
		td->tv_nsec -= NSEC_PER_SEC;
Stefan Reif's avatar
Stefan Reif committed
51 52
	}

Stefan Reif's avatar
Stefan Reif committed
53 54 55 56 57 58 59
	td->tv_sec += nsec / NSEC_PER_SEC;
	td->tv_nsec += nsec % NSEC_PER_SEC;

	if (td->tv_nsec >= NSEC_PER_SEC) {
		td->tv_sec++;
		td->tv_nsec -= NSEC_PER_SEC;
	}
Stefan Reif's avatar
Stefan Reif committed
60 61
}

Stefan Reif's avatar
Stefan Reif committed
62
static inline void timer_date_sub(TimerDate *td, TimerDateUDiff_t nsec)
Stefan Reif's avatar
Stefan Reif committed
63
{
Stefan Reif's avatar
Stefan Reif committed
64 65 66 67 68 69 70 71 72
	assert(td->tv_nsec >= 0 && "negative time");
	td->tv_sec -= nsec / NSEC_PER_SEC;
	nsec %= NSEC_PER_SEC;
	if ((TimerDateUDiff_t) td->tv_nsec < nsec) {
		td->tv_sec--;
		td->tv_nsec += NSEC_PER_SEC;
	}
	td->tv_nsec -= nsec;
}
Stefan Reif's avatar
Stefan Reif committed
73

Stefan Reif's avatar
Stefan Reif committed
74 75 76 77 78 79
static inline bool timer_date_is_lt(const TimerDate *ta, const TimerDate *tb)
{
	if (timer_date_is_inf(ta))
		return false;
	if (timer_date_is_inf(tb))
		return true;
Stefan Reif's avatar
Stefan Reif committed
80

Stefan Reif's avatar
Stefan Reif committed
81
	// TODO: is integer overflow relevant here?
Stefan Reif's avatar
Stefan Reif committed
82

Stefan Reif's avatar
Stefan Reif committed
83 84 85 86
	if (ta->tv_sec < tb->tv_sec)
		return true;
	if (ta->tv_sec > tb->tv_sec)
		return false;
Stefan Reif's avatar
Stefan Reif committed
87

Stefan Reif's avatar
Stefan Reif committed
88
	if (ta->tv_nsec < tb->tv_nsec)
Stefan Reif's avatar
Stefan Reif committed
89
		return true;
Stefan Reif's avatar
Stefan Reif committed
90
	if (ta->tv_nsec > tb->tv_nsec)
Stefan Reif's avatar
Stefan Reif committed
91 92
		return false;

Stefan Reif's avatar
Stefan Reif committed
93
	return false;
Stefan Reif's avatar
Stefan Reif committed
94 95
}

Stefan Reif's avatar
Stefan Reif committed
96 97 98 99
static inline bool timer_date_eq(const TimerDate *ta, const TimerDate *tb)
{
	return ta->tv_sec == tb->tv_sec && ta->tv_nsec == tb->tv_nsec;
}
Stefan Reif's avatar
Stefan Reif committed
100

Stefan Reif's avatar
Stefan Reif committed
101
static inline TimerDateUDiff_t timer_measure_clock_precision_round(void)
Stefan Reif's avatar
Stefan Reif committed
102
{
Stefan Reif's avatar
Stefan Reif committed
103 104 105 106 107 108 109
	TimerDate a,b;
	clock_gettime(CLOCK_REALTIME, &a);
	do {
		clock_gettime(CLOCK_REALTIME, &b);
	} while (0 == timer_duration_finite(&a, &b));
	return timer_duration_finite(&a, &b);
}
Stefan Reif's avatar
Stefan Reif committed
110

Stefan Reif's avatar
Stefan Reif committed
111 112 113 114 115 116 117 118
static TimerDateUDiff_t timer_measure_clock_precision(void)
{
	const unsigned int ROUNDS = 10;
	TimerDateUDiff_t sum = 0;
	for (int r = 0; r < ROUNDS; r++)
		sum += timer_measure_clock_precision_round();
	return (sum + ROUNDS / 2) / ROUNDS;
}
Stefan Reif's avatar
Stefan Reif committed
119

Stefan Reif's avatar
Stefan Reif committed
120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
typedef struct prrtTimerNode {
	_Atomic(struct prrtTimerNode *) next;
	atomic_bool                     done;
	TimerDate                       date;
	TimerTaskArg                    arg;
	TimerTaskFun                    fun;
} TimerNode;

#define OSP_WINDOW_SIZE 8

struct prrtTimer {
	pthread_t            worker;
	atomic_bool          alive;
	atomic_int           wait;
	_Atomic(TimerNode *) new;
	_Atomic(TimerNode *) old;
Stefan Reif's avatar
timer  
Stefan Reif committed
136
	_Atomic(TimerNode *) cur;
Stefan Reif's avatar
Stefan Reif committed
137 138 139 140 141 142 143 144 145
	TimerDateUDiff_t     precision;
	TimerDateUDiff_t     lcp;
	TimerDateUDiff_t     osp;
	TimerDateUDiff_t     osp_window[OSP_WINDOW_SIZE];
	unsigned int         osp_idx;
};

typedef struct prrtTimer Timer;

Stefan Reif's avatar
timer  
Stefan Reif committed
146
static bool timer_date_is_due(Timer *self, const TimerDate *when, const TimerDate *now)
Stefan Reif's avatar
Stefan Reif committed
147 148 149 150
{
	// TODO: use self->precision to check whether now and *when are similar enough
	(void) self;
	return timer_date_is_lt(when, now);
Stefan Reif's avatar
Stefan Reif committed
151 152
}

Stefan Reif's avatar
Stefan Reif committed
153
static void compute_sleep_end(Timer *self, TimerDate *out, const TimerDate *end)
Stefan Reif's avatar
Stefan Reif committed
154
{
Stefan Reif's avatar
Stefan Reif committed
155 156 157
	*out = *end;
	if (timer_date_is_inf(end))
		return;
Stefan Reif's avatar
Stefan Reif committed
158

Stefan Reif's avatar
Stefan Reif committed
159 160 161
	// is is okay when the computed sleep end is in the past
	timer_date_sub(out, self->osp + self->lcp + 2 * self->precision);
}
Stefan Reif's avatar
Stefan Reif committed
162

Stefan Reif's avatar
Stefan Reif committed
163 164 165 166 167 168
static inline void learn_osp(Timer *self, TimerDateUDiff_t new)
{
	unsigned int idx = self->osp_idx;
	TimerDateUDiff_t old = self->osp_window[idx];
	(void) old;
	self->osp_window[idx] = new;
Stefan Reif's avatar
Stefan Reif committed
169

Stefan Reif's avatar
Stefan Reif committed
170 171 172 173 174 175
	TimerDateUDiff_t max = self->osp_window[0];
	for (int i = 1; i < OSP_WINDOW_SIZE; i++)
		max = self->osp_window[i] > max ? self->osp_window[i] : max;
	self->osp = max;

	self->osp_idx = (idx + 1) & (OSP_WINDOW_SIZE - 1);
Stefan Reif's avatar
Stefan Reif committed
176 177
}

Stefan Reif's avatar
Stefan Reif committed
178
static inline void learn_lcp(Timer *self, TimerDateUDiff_t new)
Stefan Reif's avatar
Stefan Reif committed
179
{
Stefan Reif's avatar
Stefan Reif committed
180 181
	TimerDateUDiff_t old = self->lcp;
	self->lcp = (3 * old + new) / 4;
Stefan Reif's avatar
Stefan Reif committed
182 183
}

Stefan Reif's avatar
Stefan Reif committed
184 185 186
#define futex(...) syscall(SYS_futex, __VA_ARGS__)

static void timer_wait_imprecise(Timer *self, const TimerDate *end)
Stefan Reif's avatar
Stefan Reif committed
187
{
Stefan Reif's avatar
Stefan Reif committed
188 189 190
	bool forever = timer_date_is_inf(end);
	futex(&self->wait, FUTEX_WAIT_BITSET|FUTEX_PRIVATE_FLAG|FUTEX_CLOCK_REALTIME, 1, forever ? NULL : end, NULL, FUTEX_BITSET_MATCH_ANY);
}
Stefan Reif's avatar
Stefan Reif committed
191

Stefan Reif's avatar
Stefan Reif committed
192 193 194 195 196 197
static void timer_wake_worker(PrrtTimer *self, bool force)
{
	int one = 1;
	if (atomic_compare_exchange_strong(&self->wait, &one, 0) || force)
		futex(&self->wait, FUTEX_WAKE|FUTEX_PRIVATE_FLAG, 1, NULL, NULL, 0);
}
Stefan Reif's avatar
Stefan Reif committed
198

Stefan Reif's avatar
Stefan Reif committed
199 200 201
static void *timer_worker_loop(void *arg)
{
	PrrtTimer *self = (PrrtTimer *) arg;
Stefan Reif's avatar
Stefan Reif committed
202

Stefan Reif's avatar
Stefan Reif committed
203 204 205 206 207 208 209 210
	bool slept, learned;

	while (1) {
		loop:;
		atomic_store(&self->wait, 1);
		TimerNode *task = atomic_load(&self->new);
		assert(task != NULL && "task list contains NULL node");

Stefan Reif's avatar
timer  
Stefan Reif committed
211 212
		atomic_store(&self->cur, task);

Stefan Reif's avatar
Stefan Reif committed
213 214 215 216 217
		if (timer_date_is_inf(&task->date)) {
			if (!atomic_load_explicit(&self->alive, memory_order_acquire)) {
				if (task == atomic_load(&self->new))
					break;
			}
Stefan Reif's avatar
Stefan Reif committed
218 219
		}

Stefan Reif's avatar
Stefan Reif committed
220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248
		TimerDate sleep_end;
		compute_sleep_end(self, &sleep_end, &task->date);

		TimerDate now, td1, td2;
		TimerDateUDiff_t oversleep = 0;
		slept = false;
		learned = false;

		clock_gettime(CLOCK_REALTIME, &now);

		if (timer_date_is_lt(&now, &sleep_end)) {
			slept = true;
			timer_wait_imprecise(self, &sleep_end);

			clock_gettime(CLOCK_REALTIME, &td1);

			if (timer_date_is_lt(&td1, &sleep_end))
				goto loop;

			oversleep = timer_duration_finite(&sleep_end, &td1);

			TimerDateUDiff_t avail = timer_duration_finite(&td1, &task->date);
			if (timer_date_is_lt(&td1, &task->date) && avail >= 2 * self->lcp) {
				learn_osp(self, oversleep);
				learned = true;
				clock_gettime(CLOCK_REALTIME, &td2);
				learn_lcp(self, timer_duration_finite(&td1, &td2));
			}
		}
Stefan Reif's avatar
Stefan Reif committed
249

Stefan Reif's avatar
Stefan Reif committed
250
		atomic_store(&self->wait, 0);
Stefan Reif's avatar
Stefan Reif committed
251

Stefan Reif's avatar
Stefan Reif committed
252 253 254 255 256 257
		while (true) {
			if (timer_date_is_due(self, &task->date, &now))
				break;
			if (task != atomic_load(&self->new))
				goto loop;
			clock_gettime(CLOCK_REALTIME, &now);
Stefan Reif's avatar
Stefan Reif committed
258 259
		}

Stefan Reif's avatar
Stefan Reif committed
260 261 262
		if (!task->done) {
			task->fun(task->arg);
			task->done = true;
Stefan Reif's avatar
Stefan Reif committed
263 264
		}

Stefan Reif's avatar
Stefan Reif committed
265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280
		TimerNode *next = atomic_load(&task->next);
		TimerNode *temp = task;
		atomic_compare_exchange_strong(&self->new, &temp, next);

		if (slept && !learned) {
			clock_gettime(CLOCK_REALTIME, &td1);
			learn_osp(self, oversleep);
			clock_gettime(CLOCK_REALTIME, &td2);
			learn_lcp(self, timer_duration_finite(&td1, &td2));
		}

		if (!slept) {
			for (int i = 0; i < OSP_WINDOW_SIZE; i++)
				self->osp_window[i] = self->osp_window[i] / 4 * 3;
			self->osp = self->osp / 4 * 3;
		}
Stefan Reif's avatar
Stefan Reif committed
281 282
	}

Stefan Reif's avatar
Stefan Reif committed
283
	return self;
Stefan Reif's avatar
Stefan Reif committed
284 285
}

Stefan Reif's avatar
Stefan Reif committed
286 287

PrrtTimer *PrrtTimer_create(unsigned int core)
Stefan Reif's avatar
Stefan Reif committed
288
{
Stefan Reif's avatar
Stefan Reif committed
289
	int err;
Stefan Reif's avatar
Stefan Reif committed
290

Stefan Reif's avatar
Stefan Reif committed
291 292 293
	PrrtTimer *self = malloc(sizeof(PrrtTimer));
	if (!self)
		return NULL;
Stefan Reif's avatar
Stefan Reif committed
294

Stefan Reif's avatar
Stefan Reif committed
295 296 297 298 299 300 301 302 303 304 305 306 307 308
	// create dummy node
	TimerNode *node = malloc(sizeof(TimerNode));
	if (!node) {
		free(self);
		return NULL;
	}
	timer_date_make_inf(&node->date);
	atomic_store_explicit(&node->done, false, memory_order_relaxed);
	atomic_store_explicit(&node->next, NULL,  memory_order_relaxed);

	atomic_store_explicit(&self->alive, true, memory_order_relaxed);
	atomic_store_explicit(&self->wait,  0,    memory_order_relaxed);
	atomic_store_explicit(&self->new,   node, memory_order_relaxed);
	atomic_store_explicit(&self->old,   node, memory_order_relaxed);
Stefan Reif's avatar
timer  
Stefan Reif committed
309
	atomic_store_explicit(&self->cur,   node, memory_order_relaxed);
Stefan Reif's avatar
Stefan Reif committed
310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328

	self->precision = timer_measure_clock_precision();
	for (int i = 0; i < OSP_WINDOW_SIZE; i++)
		self->osp_window[i] = self->precision;
	self->osp = self->precision;
	self->lcp = self->precision;
	self->osp_idx = 0;

	atomic_thread_fence(memory_order_release);

	// start worker thread
	pthread_attr_t attr;
	err = pthread_attr_init(&attr);
	if (err) {
		free(node);
		free(self);
		errno = err;
		return NULL;
	}
Stefan Reif's avatar
Stefan Reif committed
329

Stefan Reif's avatar
Stefan Reif committed
330 331 332
	cpu_set_t set;
	CPU_ZERO(&set);
	CPU_SET(core, &set);
Stefan Reif's avatar
Stefan Reif committed
333

Stefan Reif's avatar
Stefan Reif committed
334 335 336 337 338 339 340 341
	err = pthread_attr_setaffinity_np(&attr, sizeof(set), &set);
	if (err) {
		free(node);
		free(self);
		pthread_attr_destroy(&attr);
		errno = err;
		return NULL;
	}
Stefan Reif's avatar
Stefan Reif committed
342

Stefan Reif's avatar
Stefan Reif committed
343 344 345 346 347 348 349 350
	err = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
	if (err) {
		free(node);
		free(self);
		pthread_attr_destroy(&attr);
		errno = err;
		return NULL;
	}
Stefan Reif's avatar
Stefan Reif committed
351

Stefan Reif's avatar
Stefan Reif committed
352 353 354 355 356 357 358 359
	err = pthread_create(&self->worker, &attr, timer_worker_loop, self);
	if (err) {
		free(node);
		free(self);
		pthread_attr_destroy(&attr);
		errno = err;
		return NULL;
	}
Stefan Reif's avatar
Stefan Reif committed
360

Stefan Reif's avatar
Stefan Reif committed
361 362 363 364 365 366
	pthread_attr_destroy(&attr);
	return self;
}

int PrrtTimer_submit(PrrtTimer *self, const TimerDate *when, const PrrtTimerTask *what)
{
Stefan Reif's avatar
timer  
Stefan Reif committed
367
	TimerNode *iter, *stop, *next, *hold;
Stefan Reif's avatar
Stefan Reif committed
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382
	TimerNode *node = malloc(sizeof(TimerNode));
	if (!node)
		return -1;

	node->done = false;
	node->arg = what->arg;
	node->fun = what->fun;
	node->date = *when;

	// fix the date, if needed
	if (timer_date_is_inf(&node->date))
		timer_date_add(&node->date, 1);

	iter = atomic_load(&self->old);
	stop = atomic_load(&self->new);
Stefan Reif's avatar
timer  
Stefan Reif committed
383 384
	hold = atomic_load(&self->cur);
	while (iter != stop && iter != hold) {
Stefan Reif's avatar
Stefan Reif committed
385 386 387 388 389
		next = iter->next;
		assert(iter->done && "cleanup task that is not marked as done");
		free(iter);
		iter = next;
	}
Stefan Reif's avatar
timer  
Stefan Reif committed
390
	atomic_store(&self->old, iter);
Stefan Reif's avatar
Stefan Reif committed
391 392 393 394 395 396 397 398 399 400 401 402

	_Atomic(TimerNode *) *addr = &self->old;
	while (1) {
		iter = atomic_load(addr);
		assert(iter && "unexpected NULL pointer in task list");

		// make sure every date is unique
		if (timer_date_eq(&node->date, &iter->date)) {
			timer_date_add(&node->date, 1);
			if (timer_date_is_inf(&node->date))
				timer_date_add(&node->date, 1);
		} else if (timer_date_is_lt(&node->date, &iter->date)) {
Stefan Reif's avatar
Stefan Reif committed
403
			break;
Stefan Reif's avatar
Stefan Reif committed
404
		}
Stefan Reif's avatar
Stefan Reif committed
405 406 407
		addr = &iter->next;
	}

Stefan Reif's avatar
Stefan Reif committed
408 409
	atomic_store(&node->next, iter);
	atomic_store(addr, node);
Stefan Reif's avatar
Stefan Reif committed
410

Stefan Reif's avatar
Stefan Reif committed
411
	TimerNode *tail = atomic_load(&self->new);
Stefan Reif's avatar
timer  
Stefan Reif committed
412
	if (timer_date_is_lt(&node->date, &tail->date) || (addr == &tail->next && atomic_load(&tail->done))) {
Stefan Reif's avatar
Stefan Reif committed
413 414 415
		atomic_store(&self->new, node);
		timer_wake_worker(self, false);
	}
Stefan Reif's avatar
Stefan Reif committed
416

Stefan Reif's avatar
Stefan Reif committed
417
	return -1;
Stefan Reif's avatar
Stefan Reif committed
418 419
}

Stefan Reif's avatar
timer  
Stefan Reif committed
420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463
static void wake_sleeping_thread(void *arg)
{
	atomic_int *ip = (atomic_int *) arg;
	atomic_store_explicit(ip, 1, memory_order_release);
	futex((int *) ip, FUTEX_WAKE|FUTEX_PRIVATE_FLAG, 1, NULL, NULL, 0);
}

void PrrtTimer_sleep_until(PrrtTimer *self, const TimerDate *end)
{
	atomic_int cond;
	atomic_store_explicit(&cond, 0, memory_order_release);

	TimerDate now;
	TimerDate care = *end;
	PrrtTimerTask what;
	what.fun = wake_sleeping_thread;
	what.arg = &cond;
	timer_date_sub(&care, 2*self->osp);
	clock_gettime(CLOCK_REALTIME, &now);
	if (!timer_date_is_due(self, &care, &now)) {
		PrrtTimer_submit(self, &care, &what);
		while (!atomic_load(&cond)) {
			clock_gettime(CLOCK_REALTIME, &now);
			if (!timer_date_is_due(self, &care, &now))
				//futex(&cond, FUTEX_WAIT_BITSET|FUTEX_PRIVATE_FLAG|FUTEX_CLOCK_REALTIME, 1, care, NULL, FUTEX_BITSET_MATCH_ANY);
				futex(&cond, FUTEX_WAIT|FUTEX_PRIVATE_FLAG, 0, NULL, NULL, 0);
		}
	}

	while (1) {
		clock_gettime(CLOCK_REALTIME, &now);
		if (timer_date_is_due(self, end, &now))
			break;
	}
}

void PrrtTimer_sleep_nanos(PrrtTimer *self, TimerDateUDiff_t nanos)
{
	TimerDate when;
	clock_gettime(CLOCK_REALTIME, &when);
	timer_date_add(&when, nanos);
	PrrtTimer_sleep_until(self, &when);
}

Stefan Reif's avatar
Stefan Reif committed
464
void PrrtTimer_end(PrrtTimer *self)
Stefan Reif's avatar
Stefan Reif committed
465
{
Stefan Reif's avatar
Stefan Reif committed
466 467 468 469 470 471 472 473 474 475
	atomic_store_explicit(&self->alive, false, memory_order_release);
	timer_wake_worker(self, true);
	pthread_join(self->worker, NULL);

	TimerNode *iter = atomic_load(&self->old);
	while (iter) {
		TimerNode *next = atomic_load(&iter->next);
		assert((iter->done || timer_date_is_inf(&iter->date)) && "cleanup task that is not marked as done");
		free(iter);
		iter = next;
Stefan Reif's avatar
Stefan Reif committed
476
	}
Stefan Reif's avatar
Stefan Reif committed
477 478

	free(self);
Stefan Reif's avatar
Stefan Reif committed
479 480
}