git.sesse.net Git - bcachefs-tools-debian/blob - linux/timer.c

   1
   2 #include <pthread.h>
   3 #include <signal.h>
   4 #include <time.h>
   5
   6 #include <linux/kernel.h>
   7 #include <linux/kthread.h>
   8 #include <linux/timer.h>
   9
  10 /**
  11  * timespec_add_ns - Adds nanoseconds to a timespec
  12  * @a:          pointer to timespec to be incremented
  13  * @ns:         unsigned nanoseconds value to be added
  14  *
  15  * This must always be inlined because its used from the x86-64 vdso,
  16  * which cannot call other kernel functions.
  17  */
  18 static struct timespec timespec_add_ns(struct timespec a, u64 ns)
  19 {
  20         a.tv_nsec       += ns;
  21         a.tv_sec        += a.tv_nsec / NSEC_PER_SEC;
  22         a.tv_nsec       %= NSEC_PER_SEC;
  23         return a;
  24 }
  25
  26 #define DECLARE_HEAP(type)                                              \
  27 struct {                                                                \
  28         size_t size, used;                                              \
  29         type *data;                                                     \
  30 }
  31
  32 #define heap_init(heap, _size)                                          \
  33 ({                                                                      \
  34         size_t _bytes;                                                  \
  35         (heap)->used = 0;                                               \
  36         (heap)->size = (_size);                                         \
  37         _bytes = (heap)->size * sizeof(*(heap)->data);                  \
  38         (heap)->data = malloc(_bytes);                                  \
  39         (heap)->data;                                                   \
  40 })
  41
  42 #define heap_free(heap)                                                 \
  43 do {                                                                    \
  44         kvfree((heap)->data);                                           \
  45         (heap)->data = NULL;                                            \
  46 } while (0)
  47
  48 #define heap_swap(h, i, j)      swap((h)->data[i], (h)->data[j])
  49
  50 #define heap_sift(h, i, cmp)                                            \
  51 do {                                                                    \
  52         size_t _r, _j = i;                                              \
  53                                                                         \
  54         for (; _j * 2 + 1 < (h)->used; _j = _r) {                       \
  55                 _r = _j * 2 + 1;                                        \
  56                 if (_r + 1 < (h)->used &&                               \
  57                     cmp((h)->data[_r], (h)->data[_r + 1]))              \
  58                         _r++;                                           \
  59                                                                         \
  60                 if (cmp((h)->data[_r], (h)->data[_j]))                  \
  61                         break;                                          \
  62                 heap_swap(h, _r, _j);                                   \
  63         }                                                               \
  64 } while (0)
  65
  66 #define heap_sift_down(h, i, cmp)                                       \
  67 do {                                                                    \
  68         while (i) {                                                     \
  69                 size_t p = (i - 1) / 2;                                 \
  70                 if (cmp((h)->data[i], (h)->data[p]))                    \
  71                         break;                                          \
  72                 heap_swap(h, i, p);                                     \
  73                 i = p;                                                  \
  74         }                                                               \
  75 } while (0)
  76
  77 #define heap_add(h, d, cmp)                                             \
  78 ({                                                                      \
  79         bool _r = !heap_full(h);                                        \
  80         if (_r) {                                                       \
  81                 size_t _i = (h)->used++;                                \
  82                 (h)->data[_i] = d;                                      \
  83                                                                         \
  84                 heap_sift_down(h, _i, cmp);                             \
  85                 heap_sift(h, _i, cmp);                                  \
  86         }                                                               \
  87         _r;                                                             \
  88 })
  89
  90 #define heap_del(h, i, cmp)                                             \
  91 do {                                                                    \
  92         size_t _i = (i);                                                \
  93                                                                         \
  94         BUG_ON(_i >= (h)->used);                                        \
  95         (h)->used--;                                                    \
  96         heap_swap(h, _i, (h)->used);                                    \
  97         heap_sift_down(h, _i, cmp);                                     \
  98         heap_sift(h, _i, cmp);                                          \
  99 } while (0)
 100
 101 #define heap_pop(h, d, cmp)                                             \
 102 ({                                                                      \
 103         bool _r = (h)->used;                                            \
 104         if (_r) {                                                       \
 105                 (d) = (h)->data[0];                                     \
 106                 heap_del(h, 0, cmp);                                    \
 107         }                                                               \
 108         _r;                                                             \
 109 })
 110
 111 #define heap_peek(h)    ((h)->used ? &(h)->data[0] : NULL)
 112 #define heap_full(h)    ((h)->used == (h)->size)
 113 #define heap_empty(h)   ((h)->used == 0)
 114
 115 #define heap_resort(heap, cmp)                                          \
 116 do {                                                                    \
 117         ssize_t _i;                                                     \
 118         for (_i = (ssize_t) (heap)->used / 2 -  1; _i >= 0; --_i)       \
 119                 heap_sift(heap, _i, cmp);                               \
 120 } while (0)
 121
 122 struct pending_timer {
 123         struct timer_list       *timer;
 124         unsigned long           expires;
 125 };
 126
 127 static inline bool pending_timer_cmp(struct pending_timer a,
 128                                      struct pending_timer b)
 129 {
 130         return a.expires < b.expires;
 131 }
 132
 133 static DECLARE_HEAP(struct pending_timer) pending_timers;
 134
 135 static pthread_mutex_t  timer_lock = PTHREAD_MUTEX_INITIALIZER;
 136 static pthread_cond_t   timer_cond = PTHREAD_COND_INITIALIZER;
 137 static pthread_cond_t   timer_running_cond = PTHREAD_COND_INITIALIZER;
 138 static unsigned long    timer_seq;
 139
 140 static inline bool timer_running(void)
 141 {
 142         return timer_seq & 1;
 143 }
 144
 145 static ssize_t timer_idx(struct timer_list *timer)
 146 {
 147         size_t i;
 148
 149         for (i = 0; i < pending_timers.used; i++)
 150                 if (pending_timers.data[i].timer == timer)
 151                         return i;
 152
 153         return -1;
 154 }
 155
 156 int del_timer(struct timer_list *timer)
 157 {
 158         ssize_t idx;
 159
 160         pthread_mutex_lock(&timer_lock);
 161         idx = timer_idx(timer);
 162         if (idx >= 0)
 163                 heap_del(&pending_timers, idx, pending_timer_cmp);
 164
 165         timer->pending = false;
 166         pthread_mutex_unlock(&timer_lock);
 167
 168         return idx >= 0;
 169 }
 170
 171 void flush_timers(void)
 172 {
 173         unsigned long seq;
 174
 175         pthread_mutex_lock(&timer_lock);
 176         seq = timer_seq;
 177         while (timer_running() && seq == timer_seq)
 178                 pthread_cond_wait(&timer_running_cond, &timer_lock);
 179
 180         pthread_mutex_unlock(&timer_lock);
 181 }
 182
 183 int del_timer_sync(struct timer_list *timer)
 184 {
 185         unsigned long seq;
 186         ssize_t idx;
 187
 188         pthread_mutex_lock(&timer_lock);
 189         idx = timer_idx(timer);
 190         if (idx >= 0)
 191                 heap_del(&pending_timers, idx, pending_timer_cmp);
 192
 193         timer->pending = false;
 194
 195         seq = timer_seq;
 196         while (timer_running() && seq == timer_seq)
 197                 pthread_cond_wait(&timer_running_cond, &timer_lock);
 198         pthread_mutex_unlock(&timer_lock);
 199
 200         return idx >= 0;
 201 }
 202
 203 int mod_timer(struct timer_list *timer, unsigned long expires)
 204 {
 205         ssize_t idx;
 206
 207         pthread_mutex_lock(&timer_lock);
 208         timer->expires = expires;
 209         timer->pending = true;
 210         idx = timer_idx(timer);
 211
 212         if (idx >= 0 &&
 213             pending_timers.data[idx].expires == expires)
 214                 goto out;
 215
 216         if (idx >= 0) {
 217                 pending_timers.data[idx].expires = expires;
 218
 219                 heap_sift_down(&pending_timers, idx, pending_timer_cmp);
 220                 heap_sift(&pending_timers, idx, pending_timer_cmp);
 221         } else {
 222                 if (heap_full(&pending_timers)) {
 223                         pending_timers.size *= 2;
 224                         pending_timers.data =
 225                                 realloc(pending_timers.data,
 226                                         pending_timers.size *
 227                                         sizeof(struct pending_timer));
 228
 229                         BUG_ON(!pending_timers.data);
 230                 }
 231
 232                 heap_add(&pending_timers,
 233                          ((struct pending_timer) {
 234                                 .timer = timer,
 235                                 .expires = expires,
 236                          }),
 237                          pending_timer_cmp);
 238         }
 239
 240         pthread_cond_signal(&timer_cond);
 241 out:
 242         pthread_mutex_unlock(&timer_lock);
 243
 244         return idx >= 0;
 245 }
 246
 247 static bool timer_thread_stop = false;
 248
 249 static int timer_thread(void *arg)
 250 {
 251         struct pending_timer *p;
 252         struct timespec ts;
 253         unsigned long now;
 254         int ret;
 255
 256         pthread_mutex_lock(&timer_lock);
 257
 258         while (!timer_thread_stop) {
 259                 now = jiffies;
 260                 p = heap_peek(&pending_timers);
 261
 262                 if (!p) {
 263                         pthread_cond_wait(&timer_cond, &timer_lock);
 264                         continue;
 265                 }
 266
 267                 if (time_after_eq(now, p->expires)) {
 268                         struct timer_list *timer = p->timer;
 269
 270                         heap_del(&pending_timers, 0, pending_timer_cmp);
 271                         BUG_ON(!timer_pending(timer));
 272                         timer->pending = false;
 273
 274                         timer_seq++;
 275                         BUG_ON(!timer_running());
 276
 277                         pthread_mutex_unlock(&timer_lock);
 278                         timer->function(timer);
 279                         pthread_mutex_lock(&timer_lock);
 280
 281                         timer_seq++;
 282                         pthread_cond_broadcast(&timer_running_cond);
 283                         continue;
 284                 }
 285
 286
 287                 ret = clock_gettime(CLOCK_REALTIME, &ts);
 288                 BUG_ON(ret);
 289
 290                 ts = timespec_add_ns(ts, jiffies_to_nsecs(p->expires - now));
 291
 292                 pthread_cond_timedwait(&timer_cond, &timer_lock, &ts);
 293         }
 294
 295         pthread_mutex_unlock(&timer_lock);
 296
 297         return 0;
 298 }
 299
 300 struct task_struct *timer_task;
 301
 302 __attribute__((constructor(103)))
 303 static void timers_init(void)
 304 {
 305         heap_init(&pending_timers, 64);
 306         BUG_ON(!pending_timers.data);
 307
 308         timer_task = kthread_run(timer_thread, NULL, "timers");
 309         BUG_ON(IS_ERR(timer_task));
 310 }
 311
 312 __attribute__((destructor(103)))
 313 static void timers_cleanup(void)
 314 {
 315         get_task_struct(timer_task);
 316
 317         pthread_mutex_lock(&timer_lock);
 318         timer_thread_stop = true;
 319         pthread_cond_signal(&timer_cond);
 320         pthread_mutex_unlock(&timer_lock);
 321
 322         int ret = kthread_stop(timer_task);
 323         BUG_ON(ret);
 324
 325         put_task_struct(timer_task);
 326         timer_task = NULL;
 327 }