modules/base/callout/callout.c

   1 /*
   2  * Copyright (c) <2014>, Olivier Matz <zer0@droids-corp.org>
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *
   8  *     * Redistributions of source code must retain the above copyright
   9  *       notice, this list of conditions and the following disclaimer.
  10  *     * Redistributions in binary form must reproduce the above copyright
  11  *       notice, this list of conditions and the following disclaimer in the
  12  *       documentation and/or other materials provided with the distribution.
  13  *     * Neither the name of the University of California, Berkeley nor the
  14  *       names of its contributors may be used to endorse or promote products
  15  *       derived from this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
  18  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  19  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  20  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
  21  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  22  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  23  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  24  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  25  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  26  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  27  */
  28
  29 /* Inspired from Intel DPDK rte_timer library */
  30 /*-
  31  * Copyright (c) <2010>, Intel Corporation
  32  * All rights reserved.
  33  *
  34  * Redistribution and use in source and binary forms, with or without
  35  * modification, are permitted provided that the following conditions
  36  * are met:
  37  *
  38  * - Redistributions of source code must retain the above copyright
  39  *   notice, this list of conditions and the following disclaimer.
  40  *
  41  * - Redistributions in binary form must reproduce the above copyright
  42  *   notice, this list of conditions and the following disclaimer in
  43  *   the documentation and/or other materials provided with the
  44  *   distribution.
  45  *
  46  * - Neither the name of Intel Corporation nor the names of its
  47  *   contributors may be used to endorse or promote products derived
  48  *   from this software without specific prior written permission.
  49  *
  50  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  51  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  52  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  53  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  54  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  55  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  56  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  57  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  58  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
  59  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  60  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
  61  * OF THE POSSIBILITY OF SUCH DAMAGE.
  62  */
  63
  64 #include <string.h>
  65 #include <stdio.h>
  66 #include <stdint.h>
  67 #include <inttypes.h>
  68
  69 #include <aversive.h>
  70 #include <aversive/pgmspace.h>
  71 #include <aversive/queue.h>
  72
  73 #include "callout.h"
  74
  75 /* allow to browse a list while modifying the current element */
  76 #define LIST_FOREACH_SAFE(cur, next, head, field)                       \
  77         for ((cur) = LIST_FIRST((head)),                                \
  78                      (next) = ((cur) ? LIST_NEXT((cur), field) : NULL); \
  79              (cur);                                                     \
  80              (cur) = (next),                                            \
  81                      (next) = ((cur) ? LIST_NEXT((cur), field) : NULL))
  82
  83 #ifdef CALLOUT_STATS
  84 /* called with irq locked */
  85 #define CALLOUT_STAT_ADD(cm, field, x) do {     \
  86         cm->stats.field += x;                   \
  87         } while(0)
  88 #else
  89 #define CALLOUT_STAT_ADD(cm, field, x) do { } while(0)
  90 #endif
  91
  92 #ifdef CALLOUT_DEBUG
  93 #define callout_dprintf_P(fmt, ...) printf_P(PSTR("%s(): " fmt), __FUNCTION__, \
  94                                          __VA_ARGS__)
  95 #else
  96 #define callout_dprintf_P(...) do { } while (0)
  97 #endif
  98
  99 /* Initialize a callout manager */
 100 void
 101 callout_mgr_init(struct callout_mgr *cm, get_time_t *get_time)
 102 {
 103         memset(cm, 0, sizeof(*cm));
 104         cm->get_time = get_time;
 105         LIST_INIT(&cm->sched_list);
 106 }
 107
 108 /* Initialize the timer handle tim for use */
 109 void
 110 callout_init(struct callout *tim, callout_cb_t f, void *arg, uint8_t priority)
 111 {
 112         memset(tim, 0, sizeof(*tim));
 113         tim->f = f;
 114         tim->arg = arg;
 115         tim->priority = priority;
 116 }
 117
 118 /*
 119  * Add a timer in the scheduled list (timer must not already be in a list). The
 120  * timers are sorted in the list according the expire time (the closer timers
 121  * first).
 122  *
 123  * called with irq locked
 124  */
 125 static void
 126 callout_add_in_sched_list(struct callout_mgr *cm, struct callout *tim)
 127 {
 128         struct callout *t, *prev_t;
 129
 130         callout_dprintf_P("cm=%p tim=%p\r\n", cm, tim);
 131         tim->state = CALLOUT_STATE_SCHEDULED;
 132
 133         /* list is empty */
 134         if (LIST_EMPTY(&cm->sched_list)) {
 135                 LIST_INSERT_HEAD(&cm->sched_list, tim, next);
 136                 return;
 137         }
 138
 139         /* 'tim' expires before first entry */
 140         t = LIST_FIRST(&cm->sched_list);
 141         if ((int16_t)(tim->expire - t->expire) <= 0) {
 142                 LIST_INSERT_HEAD(&cm->sched_list, tim, next);
 143                 return;
 144         }
 145
 146         /* find an element that will expire after 'tim' */
 147         LIST_FOREACH(t, &cm->sched_list, next) {
 148                 if ((int16_t)(tim->expire - t->expire) <= 0) {
 149                         LIST_INSERT_BEFORE(t, tim, next);
 150                         return;
 151                 }
 152                 prev_t = t;
 153         }
 154
 155         /* not found, insert at the end of the list */
 156         LIST_INSERT_AFTER(prev_t, tim, next);
 157 }
 158
 159 /*
 160  * Add a timer in the local expired list (timer must not already be in a
 161  * list). The timers are sorted in the list according to the priority (high
 162  * priority first).
 163  *
 164  * called with irq locked
 165  */
 166 static void
 167 callout_add_in_expired_list(struct callout_mgr *cm,
 168         struct callout_list *expired_list, struct callout *tim)
 169 {
 170         struct callout *t, *prev_t;
 171
 172         (void)cm; /* avoid warning if debug is disabled */
 173
 174         callout_dprintf_P("cm=%p tim=%p\r\n", cm, tim);
 175         tim->state = CALLOUT_STATE_EXPIRED;
 176
 177         /* list is empty */
 178         if (LIST_EMPTY(expired_list)) {
 179                 LIST_INSERT_HEAD(expired_list, tim, next);
 180                 return;
 181         }
 182
 183         /* 'tim' has a higher prio */
 184         t = LIST_FIRST(expired_list);
 185         if (tim->priority >= t->priority) {
 186                 LIST_INSERT_HEAD(expired_list, tim, next);
 187                 return;
 188         }
 189
 190         /* find an element that will expire after 'tim' */
 191         LIST_FOREACH(t, expired_list, next) {
 192                 if (tim->priority >= t->priority) {
 193                         LIST_INSERT_BEFORE(t, tim, next);
 194                         return;
 195                 }
 196                 prev_t = t;
 197         }
 198
 199         /* not found, insert at the end of the list */
 200         LIST_INSERT_AFTER(prev_t, tim, next);
 201 }
 202
 203 /*
 204  * del from list (timer must be in a list)
 205  */
 206 static void
 207 callout_del(struct callout_mgr *cm, struct callout *tim)
 208 {
 209         (void)cm; /* avoid warning if debug is disabled */
 210         callout_dprintf_P("cm=%p tim=%p\r\n", cm, tim);
 211         LIST_REMOVE(tim, next);
 212 }
 213
 214 /* Reset and start the timer associated with the timer handle tim */
 215 static int
 216 __callout_schedule(struct callout_mgr *cm, struct callout *tim,
 217         uint16_t expire)
 218 {
 219         uint8_t flags;
 220
 221         callout_dprintf_P("cm=%p tim=%p expire=%d\r\n",
 222                           cm, tim, expire);
 223
 224         IRQ_LOCK(flags);
 225         CALLOUT_STAT_ADD(cm, schedule, 1);
 226
 227         /* remove it from list */
 228         if (tim->state != CALLOUT_STATE_STOPPED) {
 229                 /* stats */
 230                 if (tim->state == CALLOUT_STATE_SCHEDULED)
 231                         CALLOUT_STAT_ADD(cm, cur_scheduled, -1);
 232                 else if (tim->state == CALLOUT_STATE_EXPIRED)
 233                         CALLOUT_STAT_ADD(cm, cur_expired, -1);
 234                 if (tim->state == CALLOUT_STATE_RUNNING)
 235                         CALLOUT_STAT_ADD(cm, cur_running, -1);
 236
 237                 callout_del(cm, tim);
 238         }
 239
 240         tim->expire = expire;
 241         CALLOUT_STAT_ADD(cm, cur_scheduled, 1);
 242         callout_add_in_sched_list(cm, tim);
 243         IRQ_UNLOCK(flags);
 244
 245         return 0;
 246 }
 247
 248 /* Reset and start the timer associated with the timer handle tim */
 249 int
 250 callout_schedule(struct callout_mgr *cm, struct callout *tim,
 251         uint16_t ticks)
 252 {
 253         return __callout_schedule(cm, tim, cm->get_time() + ticks);
 254 }
 255
 256 /* Reset and start the timer associated with the timer handle tim */
 257 int
 258 callout_reschedule(struct callout_mgr *cm, struct callout *tim,
 259         uint16_t ticks)
 260 {
 261         return __callout_schedule(cm, tim, tim->expire + ticks);
 262 }
 263
 264 /* Stop the timer associated with the timer handle tim */
 265 void
 266 callout_stop(struct callout_mgr *cm, struct callout *tim)
 267 {
 268         uint8_t flags;
 269
 270         callout_dprintf_P("cm=%p tim=%p\r\n", cm, tim);
 271
 272         IRQ_LOCK(flags);
 273         if (tim->state != CALLOUT_STATE_STOPPED) {
 274
 275                 /* stats */
 276                 if (tim->state == CALLOUT_STATE_SCHEDULED)
 277                         CALLOUT_STAT_ADD(cm, cur_scheduled, -1);
 278                 else if (tim->state == CALLOUT_STATE_EXPIRED)
 279                         CALLOUT_STAT_ADD(cm, cur_expired, -1);
 280                 if (tim->state == CALLOUT_STATE_RUNNING)
 281                         CALLOUT_STAT_ADD(cm, cur_running, -1);
 282                 CALLOUT_STAT_ADD(cm, stop, 1);
 283
 284                 /* remove it from list */
 285                 callout_del(cm, tim);
 286                 tim->state = CALLOUT_STATE_STOPPED;
 287         }
 288         IRQ_UNLOCK(flags);
 289 }
 290
 291 /* must be called periodically, run all timer that expired */
 292 void callout_manage(struct callout_mgr *cm)
 293 {
 294         struct callout_list expired_list;
 295         struct callout_list reschedule_list;
 296         struct callout *tim, *tim_next;
 297         uint16_t cur_time;
 298         uint8_t old_prio;
 299         int16_t diff;
 300
 301         CALLOUT_STAT_ADD(cm, manage, 1);
 302         callout_dprintf_P("cm=%p\r\n", cm);
 303
 304         /* maximize the number of self-recursions */
 305         if (cm->nb_recursion >= CALLOUT_MAX_RECURSION) {
 306                 CALLOUT_STAT_ADD(cm, max_recursion, 1);
 307                 return;
 308         }
 309
 310         cli();
 311         cm->nb_recursion++;
 312         LIST_INIT(&expired_list);
 313         LIST_INIT(&reschedule_list);
 314         cur_time = cm->get_time();
 315         old_prio = cm->cur_priority;
 316
 317         /* move all expired timers in a local list */
 318         LIST_FOREACH_SAFE(tim, tim_next, &cm->sched_list, next) {
 319
 320                 diff = cur_time - tim->expire;
 321
 322                 /* check the expiration time (tasks are sorted) */
 323                 if (diff < 0)
 324                         break;
 325
 326                 callout_dprintf_P("cm=%p diff=%d\r\n", cm, diff);
 327
 328                 /* check the priority, if it's too low, inc stats */
 329                 if (tim->priority <= cm->cur_priority) {
 330                         if (diff < 16484)
 331                                 CALLOUT_STAT_ADD(cm, delayed, 1);
 332                         else {
 333                                 /* reschedule to avoid an overflow */
 334                                 CALLOUT_STAT_ADD(cm, hard_delayed, 1);
 335                                 LIST_REMOVE(tim, next);
 336                                 tim->expire = cur_time;
 337                                 LIST_INSERT_HEAD(&reschedule_list, tim, next);
 338                         }
 339                         continue;
 340                 }
 341
 342                 LIST_REMOVE(tim, next);
 343                 callout_add_in_expired_list(cm, &expired_list, tim);
 344                 CALLOUT_STAT_ADD(cm, cur_scheduled, -1);
 345                 CALLOUT_STAT_ADD(cm, cur_expired, 1);
 346         }
 347
 348         /* reschedule hard_delayed timers, this does not happen usually */
 349         while (!LIST_EMPTY(&reschedule_list)) {
 350                 tim = LIST_FIRST(&reschedule_list);
 351                 LIST_REMOVE(tim, next);
 352                 callout_add_in_sched_list(cm, tim);
 353         }
 354
 355         /* for each timer of 'expired' list, execute callback */
 356         while (!LIST_EMPTY(&expired_list)) {
 357                 tim = LIST_FIRST(&expired_list);
 358                 LIST_REMOVE(tim, next);
 359
 360                 /* execute callback function */
 361                 CALLOUT_STAT_ADD(cm, cur_expired, -1);
 362                 CALLOUT_STAT_ADD(cm, cur_running, 1);
 363                 tim->state = CALLOUT_STATE_RUNNING;
 364                 cm->cur_priority = tim->priority;
 365                 sei();
 366                 tim->f(cm, tim, tim->arg);
 367                 cli();
 368         }
 369
 370         cm->cur_priority = old_prio;
 371         cm->nb_recursion--;
 372         sei();
 373 }
 374
 375 /* set the current priority level */
 376 uint8_t callout_mgr_set_prio(struct callout_mgr *cm, uint8_t new_prio)
 377 {
 378         uint8_t old_prio;
 379
 380         old_prio = cm->cur_priority;
 381         if (new_prio <= old_prio)
 382                 return old_prio;
 383
 384         cm->cur_priority = new_prio;
 385         return old_prio;
 386 }
 387
 388 /* restore the current priority level */
 389 void callout_mgr_restore_prio(struct callout_mgr *cm, uint8_t old_prio)
 390 {
 391         cm->cur_priority = old_prio;
 392 }
 393
 394 /* dump statistics about timers */
 395 void callout_dump_stats(struct callout_mgr *cm)
 396 {
 397 #ifdef CALLOUT_STATS
 398         printf_P(PSTR("Timer statistics:\r\n"));
 399         printf_P(PSTR("  schedule = %"PRIu32"\r\n"), cm->stats.schedule);
 400         printf_P(PSTR("  stop = %"PRIu32"\r\n"), cm->stats.stop);
 401         printf_P(PSTR("  manage = %"PRIu32"\r\n"), cm->stats.manage);
 402         printf_P(PSTR("  max_recursion = %"PRIu32"\r\n"), cm->stats.max_recursion);
 403         printf_P(PSTR("  delayed = %"PRIu32"\r\n"), cm->stats.delayed);
 404         printf_P(PSTR("  hard_delayed = %"PRIu32"\r\n"), cm->stats.hard_delayed);
 405
 406         printf_P(PSTR("  cur_scheduled = %u\r\n"), cm->stats.cur_scheduled);
 407         printf_P(PSTR("  cur_expired = %u\r\n"), cm->stats.cur_expired);
 408         printf_P(PSTR("  cur_running = %u\r\n"), cm->stats.cur_running);
 409 #else
 410         printf_P(PSTR("No timer statistics, CALLOUT_STATS is disabled\r\n"));
 411 #endif
 412 }