fred-tutorial-app/periodic_task.c at main · fred-framework/fred-tutorial-app · GitHub

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
118
119
120
121
122
123
124
125
126
127
128
129
/*
 * Fred for Linux. Experimental support.
 *
 * Copyright (C) 2018-2021, Marco Pagani, ReTiS Lab.
 * <marco.pag(at)outlook.com>
 *
 * This program 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.
*/

#include <time.h>
#include "periodic_task.h"

// Returns
//  1 if t1 > t2
// -1 if t1 < t2
//  0 if t1 == t2
static inline
int time_compare_(const struct timespec *t1, const struct timespec *t2)
{
	if (t1->tv_sec > t2->tv_sec)
		return 1;
	if (t1->tv_sec < t2->tv_sec)
		return -1;

	if (t1->tv_nsec > t2->tv_nsec)
		return 1;
	if (t1->tv_nsec < t2->tv_nsec)
		return -1;

	return 0;
}

static inline
void time_add_ms_(struct timespec *t, int ms)
{
	t->tv_sec += ms / 1000;
	t->tv_nsec += (ms % 1000) * 1000000;

	if (t->tv_nsec > 1000000000) {
		t->tv_nsec -= 1000000000;
		t->tv_sec += 1;
	}
}

static inline
void time_copy_(struct timespec *td, const struct timespec *ts)
{
	td->tv_sec = ts->tv_sec;
	td->tv_nsec = ts->tv_nsec;
}

static inline
void set_period_(struct periodic_task *self)
{
	struct timespec t_now;

	// Grab task's start time and compute next activation
	clock_gettime(CLOCK_MONOTONIC, &t_now);
	time_copy_(&self->next_activation, &t_now);

	time_add_ms_(&self->next_activation, self->period_ms);
}

static inline
void wait_for_period_(struct periodic_task *self)
{
	struct timespec t_now;
	int overrun;

	// Sleep until next activation
	clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &self->next_activation, NULL);

	// Compute next activation time (based on start time)
	clock_gettime(CLOCK_MONOTONIC, &t_now);
	do {
		time_add_ms_(&self->next_activation, self->period_ms);
		overrun = time_compare_(&t_now, &self->next_activation);
	} while (overrun == 1);
}

static inline
void *periodic_body_(void *self)
{
	struct periodic_task *task;
	int retval;

	task = (struct periodic_task *)self;

	// Set first period
	set_period_(task);

	while (1) {
		// Call the task body function
		retval = task->body(task->args);
		if (retval)
			break;

		// wait until end of current period and set next period
		wait_for_period_(task);
	}

	return NULL;
}

int periodic_task_init(struct periodic_task *self, int (*body)(void *), void *args,
						int period_ms)
{
	if (!body || period_ms <= 0)
		return -1;

	self->body = body;
	self->args = args;
	self->period_ms = period_ms;

	return 0;
}

int periodic_task_start(struct periodic_task *self)
{
	return pthread_create(&self->tid, NULL, periodic_body_, self);
}

int periodic_task_join(struct periodic_task *self)
{
	return pthread_join(self->tid, NULL);
}