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         if ((_i) < (h)->used) {                                         \
  97                 heap_swap(h, _i, (h)->used);                            \
  98                 heap_sift_down(h, _i, cmp);                             \
  99                 heap_sift(h, _i, cmp);                                  \
 100         }                                                               \
 101 } while (0)
 102
 103 #define heap_pop(h, d, cmp)                                             \
 104 ({                                                                      \
 105         bool _r = (h)->used;                                            \
 106         if (_r) {                                                       \
 107                 (d) = (h)->data[0];                                     \
 108                 heap_del(h, 0, cmp);                                    \
 109         }                                                               \
 110         _r;                                                             \
 111 })
 112
 113 #define heap_peek(h)    ((h)->used ? &(h)->data[0] : NULL)
 114 #define heap_full(h)    ((h)->used == (h)->size)
 115 #define heap_empty(h)   ((h)->used == 0)
 116
 117 #define heap_resort(heap, cmp)                                          \
 118 do {                                                                    \
 119         ssize_t _i;                                                     \
 120         for (_i = (ssize_t) (heap)->used / 2 -  1; _i >= 0; --_i)       \
 121                 heap_sift(heap, _i, cmp);                               \
 122 } while (0)
 123
 124 struct pending_timer {
 125         struct timer_list       *timer;
 126         unsigned long           expires;
 127 };
 128
 129 static inline bool pending_timer_cmp(struct pending_timer a,
 130                                      struct pending_timer b)
 131 {
 132         return a.expires < b.expires;
 133 }
 134
 135 static DECLARE_HEAP(struct pending_timer) pending_timers;
 136
 137 static pthread_mutex_t  timer_lock = PTHREAD_MUTEX_INITIALIZER;
 138 static pthread_cond_t   timer_cond = PTHREAD_COND_INITIALIZER;
 139 static pthread_cond_t   timer_running_cond = PTHREAD_COND_INITIALIZER;
 140 static unsigned long    timer_seq;
 141
 142 static inline bool timer_running(void)
 143 {
 144         return timer_seq & 1;
 145 }
 146
 147 static ssize_t timer_idx(struct timer_list *timer)
 148 {
 149         size_t i;
 150
 151         for (i = 0; i < pending_timers.used; i++)
 152                 if (pending_timers.data[i].timer == timer)
 153                         return i;
 154
 155         return -1;
 156 }
 157
 158 int del_timer(struct timer_list *timer)
 159 {
 160         ssize_t idx;
 161
 162         pthread_mutex_lock(&timer_lock);
 163         idx = timer_idx(timer);
 164         if (idx >= 0)
 165                 heap_del(&pending_timers, idx, pending_timer_cmp);
 166
 167         timer->pending = false;
 168         pthread_mutex_unlock(&timer_lock);
 169
 170         return idx >= 0;
 171 }
 172
 173 void flush_timers(void)
 174 {
 175         unsigned long seq;
 176
 177         pthread_mutex_lock(&timer_lock);
 178         seq = timer_seq;
 179         while (timer_running() && seq == timer_seq)
 180                 pthread_cond_wait(&timer_running_cond, &timer_lock);
 181
 182         pthread_mutex_unlock(&timer_lock);
 183 }
 184
 185 int del_timer_sync(struct timer_list *timer)
 186 {
 187         unsigned long seq;
 188         ssize_t idx;
 189
 190         pthread_mutex_lock(&timer_lock);
 191         idx = timer_idx(timer);
 192         if (idx >= 0)
 193                 heap_del(&pending_timers, idx, pending_timer_cmp);
 194
 195         timer->pending = false;
 196
 197         seq = timer_seq;
 198         while (timer_running() && seq == timer_seq)
 199                 pthread_cond_wait(&timer_running_cond, &timer_lock);
 200         pthread_mutex_unlock(&timer_lock);
 201
 202         return idx >= 0;
 203 }
 204
 205 int mod_timer(struct timer_list *timer, unsigned long expires)
 206 {
 207         ssize_t idx;
 208
 209         pthread_mutex_lock(&timer_lock);
 210         timer->expires = expires;
 211         timer->pending = true;
 212         idx = timer_idx(timer);
 213
 214         if (idx >= 0 &&
 215             pending_timers.data[idx].expires == expires)
 216                 goto out;
 217
 218         if (idx >= 0) {
 219                 pending_timers.data[idx].expires = expires;
 220
 221                 heap_sift_down(&pending_timers, idx, pending_timer_cmp);
 222                 heap_sift(&pending_timers, idx, pending_timer_cmp);
 223         } else {
 224                 if (heap_full(&pending_timers)) {
 225                         pending_timers.size *= 2;
 226                         pending_timers.data =
 227                                 realloc(pending_timers.data,
 228                                         pending_timers.size *
 229                                         sizeof(struct pending_timer));
 230
 231                         BUG_ON(!pending_timers.data);
 232                 }
 233
 234                 heap_add(&pending_timers,
 235                          ((struct pending_timer) {
 236                                 .timer = timer,
 237                                 .expires = expires,
 238                          }),
 239                          pending_timer_cmp);
 240         }
 241
 242         pthread_cond_signal(&timer_cond);
 243 out:
 244         pthread_mutex_unlock(&timer_lock);
 245
 246         return idx >= 0;
 247 }
 248
 249 static bool timer_thread_stop = false;
 250
 251 static int timer_thread(void *arg)
 252 {
 253         struct pending_timer *p;
 254         struct timespec ts;
 255         unsigned long now;
 256         int ret;
 257
 258         pthread_mutex_lock(&timer_lock);
 259
 260         while (!timer_thread_stop) {
 261                 now = jiffies;
 262                 p = heap_peek(&pending_timers);
 263
 264                 if (!p) {
 265                         pthread_cond_wait(&timer_cond, &timer_lock);
 266                         continue;
 267                 }
 268
 269                 if (time_after_eq(now, p->expires)) {
 270                         struct timer_list *timer = p->timer;
 271
 272                         heap_del(&pending_timers, 0, pending_timer_cmp);
 273                         BUG_ON(!timer_pending(timer));
 274                         timer->pending = false;
 275
 276                         timer_seq++;
 277                         BUG_ON(!timer_running());
 278
 279                         pthread_mutex_unlock(&timer_lock);
 280                         timer->function(timer);
 281                         pthread_mutex_lock(&timer_lock);
 282
 283                         timer_seq++;
 284                         pthread_cond_broadcast(&timer_running_cond);
 285                         continue;
 286                 }
 287
 288
 289                 ret = clock_gettime(CLOCK_REALTIME, &ts);
 290                 BUG_ON(ret);
 291
 292                 ts = timespec_add_ns(ts, jiffies_to_nsecs(p->expires - now));
 293
 294                 pthread_cond_timedwait(&timer_cond, &timer_lock, &ts);
 295         }
 296
 297         pthread_mutex_unlock(&timer_lock);
 298
 299         return 0;
 300 }
 301
 302 struct task_struct *timer_task;
 303
 304 __attribute__((constructor(103)))
 305 static void timers_init(void)
 306 {
 307         heap_init(&pending_timers, 64);
 308         BUG_ON(!pending_timers.data);
 309
 310         timer_task = kthread_run(timer_thread, NULL, "timers");
 311         BUG_ON(IS_ERR(timer_task));
 312 }
 313
 314 __attribute__((destructor(103)))
 315 static void timers_cleanup(void)
 316 {
 317         get_task_struct(timer_task);
 318
 319         pthread_mutex_lock(&timer_lock);
 320         timer_thread_stop = true;
 321         pthread_cond_signal(&timer_cond);
 322         pthread_mutex_unlock(&timer_lock);
 323
 324         int ret = kthread_stop(timer_task);
 325         BUG_ON(ret);
 326
 327         put_task_struct(timer_task);
 328         timer_task = NULL;
 329 }