1 /*
2 * Copyright (C) 2019 - 2020 Intel Corporation
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6 #include <stddef.h>
7 #include <stdint.h>
8 #include <time.h>
9 #include <sys/timerfd.h>
10 #include <usfstl/sched.h>
11 #include <usfstl/loop.h>
12
usfstl_sched_wallclock_handle_fd(struct usfstl_loop_entry * entry)13 void usfstl_sched_wallclock_handle_fd(struct usfstl_loop_entry *entry)
14 {
15 struct usfstl_scheduler *sched;
16 uint64_t v;
17
18 sched = container_of(entry, struct usfstl_scheduler, wallclock.entry);
19
20 USFSTL_ASSERT_EQ((int)read(entry->fd, &v, sizeof(v)), (int)sizeof(v), "%d");
21 sched->wallclock.timer_triggered = 1;
22 }
23
usfstl_sched_wallclock_initialize(struct usfstl_scheduler * sched)24 static void usfstl_sched_wallclock_initialize(struct usfstl_scheduler *sched)
25 {
26 struct timespec now = {};
27
28 USFSTL_ASSERT_EQ(clock_gettime(CLOCK_MONOTONIC, &now), 0, "%d");
29
30 sched->wallclock.start = (uint64_t)now.tv_sec * 1000000000 + now.tv_nsec;
31 sched->wallclock.initialized = 1;
32 }
33
usfstl_sched_wallclock_request(struct usfstl_scheduler * sched,uint64_t time)34 void usfstl_sched_wallclock_request(struct usfstl_scheduler *sched, uint64_t time)
35 {
36 struct itimerspec itimer = {};
37 unsigned int nsec_per_tick = sched->wallclock.nsec_per_tick;
38 uint64_t waketime;
39
40 USFSTL_ASSERT(nsec_per_tick != 0);
41
42 if (!sched->wallclock.initialized)
43 usfstl_sched_wallclock_initialize(sched);
44
45 waketime = sched->wallclock.start + nsec_per_tick * time;
46
47 itimer.it_value.tv_sec = waketime / 1000000000;
48 itimer.it_value.tv_nsec = waketime % 1000000000;
49
50 USFSTL_ASSERT_EQ(timerfd_settime(sched->wallclock.entry.fd,
51 TFD_TIMER_ABSTIME, &itimer, NULL),
52 0, "%d");
53 }
54
usfstl_sched_wallclock_wait(struct usfstl_scheduler * sched)55 void usfstl_sched_wallclock_wait(struct usfstl_scheduler *sched)
56 {
57 sched->wallclock.timer_triggered = 0;
58
59 usfstl_loop_register(&sched->wallclock.entry);
60
61 while (!sched->wallclock.timer_triggered)
62 usfstl_loop_wait_and_handle();
63
64 usfstl_loop_unregister(&sched->wallclock.entry);
65
66 usfstl_sched_set_time(sched, sched->prev_external_sync);
67 }
68
usfstl_sched_wallclock_init(struct usfstl_scheduler * sched,unsigned int ns_per_tick)69 void usfstl_sched_wallclock_init(struct usfstl_scheduler *sched,
70 unsigned int ns_per_tick)
71 {
72 USFSTL_ASSERT(!sched->external_request && !sched->external_wait);
73
74 sched->external_request = usfstl_sched_wallclock_request;
75 sched->external_wait = usfstl_sched_wallclock_wait;
76
77 sched->wallclock.entry.fd = timerfd_create(CLOCK_MONOTONIC, 0);
78 USFSTL_ASSERT(sched->wallclock.entry.fd >= 0);
79
80 sched->wallclock.entry.handler = usfstl_sched_wallclock_handle_fd;
81
82 sched->wallclock.nsec_per_tick = ns_per_tick;
83 }
84
usfstl_sched_wallclock_exit(struct usfstl_scheduler * sched)85 void usfstl_sched_wallclock_exit(struct usfstl_scheduler *sched)
86 {
87 USFSTL_ASSERT(sched->external_request == usfstl_sched_wallclock_request &&
88 sched->external_wait == usfstl_sched_wallclock_wait);
89
90 sched->external_request = NULL;
91 sched->external_wait = NULL;
92 close(sched->wallclock.entry.fd);
93 }
94
_usfstl_sched_wallclock_sync_real(struct usfstl_scheduler * sched)95 static void _usfstl_sched_wallclock_sync_real(struct usfstl_scheduler *sched)
96 {
97 struct timespec now = {};
98 uint64_t nowns;
99
100 USFSTL_ASSERT_EQ(clock_gettime(CLOCK_MONOTONIC, &now), 0, "%d");
101
102 nowns = (uint64_t)now.tv_sec * 1000000000 + now.tv_nsec;
103 nowns -= sched->wallclock.start;
104 usfstl_sched_set_time(sched, nowns / sched->wallclock.nsec_per_tick);
105 }
106
usfstl_sched_wallclock_sync_real(void * data)107 static void usfstl_sched_wallclock_sync_real(void *data)
108 {
109 _usfstl_sched_wallclock_sync_real(data);
110 }
111
usfstl_sched_wallclock_wait_and_handle(struct usfstl_scheduler * sched)112 void usfstl_sched_wallclock_wait_and_handle(struct usfstl_scheduler *sched)
113 {
114 void (*old_fn)(void *);
115 void *old_data;
116
117 if (usfstl_sched_next_pending(sched, NULL))
118 return;
119
120 if (!sched->wallclock.initialized) {
121 /*
122 * At least once at the beginning we should schedule
123 * and initialize from an initial request, not here.
124 */
125 usfstl_loop_wait_and_handle();
126 return;
127 }
128
129 old_fn = g_usfstl_loop_pre_handler_fn;
130 old_data = g_usfstl_loop_pre_handler_fn_data;
131 g_usfstl_loop_pre_handler_fn = usfstl_sched_wallclock_sync_real;
132 g_usfstl_loop_pre_handler_fn_data = sched;
133
134 usfstl_loop_wait_and_handle();
135
136 g_usfstl_loop_pre_handler_fn = old_fn;
137 g_usfstl_loop_pre_handler_fn_data = old_data;
138 }
139