openwrt/package/openwrt/libshared/linux_timer.c

   1 /*
   2  * Copyright 2004, Broadcom Corporation
   3  * All Rights Reserved.
   4  *
   5  * THIS SOFTWARE IS OFFERED "AS IS", AND BROADCOM GRANTS NO WARRANTIES OF ANY
   6  * KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE. BROADCOM
   7  * SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
   8  * FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE.
   9  *
  10  * Low resolution timer interface linux specific implementation.
  11  *
  12  * $Id$
  13  */
  14
  15 /*
  16 * debug facilities
  17 */
  18 #define TIMER_DEBUG     0
  19 #if TIMER_DEBUG
  20 #define TIMERDBG(fmt, args...) printf("%s: " fmt "\n" , __FUNCTION__ , ## args)
  21 #else
  22 #define TIMERDBG(fmt, args...)
  23 #endif
  24
  25
  26 /*
  27  * POSIX timer support for Linux. Taken from linux_timer.c in upnp
  28  */
  29
  30 #define __USE_GNU
  31
  32
  33 #include <stdlib.h>         // for malloc, free, etc.
  34 #include <string.h>         // for memset, strncasecmp, etc.
  35 #include <assert.h>         // for assert, of course.
  36 #include <signal.h>         // for sigemptyset, etc.
  37 #include <stdio.h>          // for printf, etc.
  38 #include <sys/time.h>
  39 #include <time.h>
  40
  41 /* define TIMER_PROFILE to enable code which guages how accurate the timer functions are.
  42    For each expiring timer the code will print the expected time interval and the actual time interval.
  43 #define TIMER_PROFILE
  44 */
  45 #undef TIMER_PROFILE
  46
  47 /*
  48 timer_cancel( ) - cancel a timer
  49 timer_connect( ) - connect a user routine to the timer signal
  50 timer_create( ) - allocate a timer using the specified clock for a timing base (POSIX)
  51 timer_delete( ) - remove a previously created timer (POSIX)
  52 timer_gettime( ) - get the remaining time before expiration and the reload value (POSIX)
  53 timer_getoverrun( ) - return the timer expiration overrun (POSIX)
  54 timer_settime( ) - set the time until the next expiration and arm timer (POSIX)
  55 nanosleep( ) - suspend the current task until the time interval elapses (POSIX)
  56 */
  57
  58 #define MS_PER_SEC 1000
  59 #define US_PER_SEC 1000000
  60 #define US_PER_MS  1000
  61 #define UCLOCKS_PER_SEC 1000000
  62
  63 typedef void (*event_callback_t)(timer_t, int);
  64
  65 #ifndef TIMESPEC_TO_TIMEVAL
  66 # define TIMESPEC_TO_TIMEVAL(tv, ts) {                                   \
  67         (tv)->tv_sec = (ts)->tv_sec;                                    \
  68         (tv)->tv_usec = (ts)->tv_nsec / 1000;                           \
  69 }
  70 #endif
  71
  72 #ifndef TIMEVAL_TO_TIMESPEC
  73 # define TIMEVAL_TO_TIMESPEC(tv, ts) {                                   \
  74         (ts)->tv_sec = (tv)->tv_sec;                                    \
  75         (ts)->tv_nsec = (tv)->tv_usec * 1000;                           \
  76 }
  77 #endif
  78
  79 #define ROUNDUP(x,y) ((((x)+(y)-1)/(y))*(y))
  80
  81 #define timerroundup(t,g) \
  82     do { \
  83         if (!timerisset(t)) (t)->tv_usec=1; \
  84         if ((t)->tv_sec == 0) (t)->tv_usec=ROUNDUP((t)->tv_usec, g); \
  85     } while (0)
  86
  87 typedef long uclock_t;
  88
  89 #define TFLAG_NONE      0
  90 #define TFLAG_CANCELLED (1<<0)
  91 #define TFLAG_DELETED   (1<<1)
  92
  93 struct event {
  94     struct timeval it_interval;
  95     struct timeval it_value;
  96     event_callback_t func;
  97     int arg;
  98     unsigned short flags;
  99     struct event *next;
 100 #ifdef TIMER_PROFILE
 101     uint expected_ms;
 102     uclock_t start;
 103 #endif
 104 };
 105
 106 void timer_cancel(timer_t timerid);
 107
 108 static void alarm_handler(int i);
 109 static void check_event_queue();
 110 static void print_event_queue();
 111 static void check_timer();
 112 #if THIS_FINDS_USE
 113 static int count_queue(struct event *);
 114 #endif
 115
 116 void block_timer();
 117 void unblock_timer();
 118
 119 static struct event *event_queue = NULL;
 120 static struct event *event_freelist;
 121 static uint g_granularity;
 122 static int g_maxevents = 0;
 123
 124 uclock_t uclock()
 125 {
 126     struct timeval tv;
 127
 128     gettimeofday(&tv, NULL);
 129     return ((tv.tv_sec * US_PER_SEC) + tv.tv_usec);
 130 }
 131
 132
 133 void init_event_queue(int n)
 134 {
 135     int i;
 136     struct itimerval tv;
 137
 138     g_maxevents = n;
 139     event_freelist = (struct event *) malloc(n * sizeof(struct event));
 140     memset(event_freelist, 0, n * sizeof(struct event));
 141
 142     for (i = 0; i < (n-1); i++)
 143         event_freelist[i].next = &event_freelist[i+1];
 144
 145     event_freelist[i].next = NULL;
 146
 147     tv.it_interval.tv_sec = 0;
 148     tv.it_interval.tv_usec = 1;
 149     tv.it_value.tv_sec = 0;
 150     tv.it_value.tv_usec = 0;
 151     setitimer (ITIMER_REAL, &tv, 0);
 152     setitimer (ITIMER_REAL, 0, &tv);
 153     g_granularity = tv.it_interval.tv_usec;
 154
 155     signal(SIGALRM, alarm_handler);
 156 }
 157
 158
 159 int clock_gettime(
 160     clockid_t         clock_id, /* clock ID (always CLOCK_REALTIME) */
 161     struct timespec * tp        /* where to store current time */
 162 )
 163 {
 164     struct timeval tv;
 165     int n;
 166
 167
 168     n = gettimeofday(&tv, NULL);
 169     TIMEVAL_TO_TIMESPEC(&tv, tp);
 170
 171     return n;
 172 }
 173
 174
 175 int timer_create(
 176     clockid_t         clock_id, /* clock ID (always CLOCK_REALTIME) */
 177     struct sigevent * evp,      /* user event handler */
 178     timer_t *         pTimer    /* ptr to return value */
 179 )
 180 {
 181     struct event *event;
 182
 183     if (clock_id != CLOCK_REALTIME) {
 184         TIMERDBG("timer_create can only support clock id CLOCK_REALTIME");
 185         exit(1);
 186     }
 187
 188     if (evp != NULL) {
 189         if (evp->sigev_notify != SIGEV_SIGNAL || evp->sigev_signo != SIGALRM) {
 190             TIMERDBG("timer_create can only support signalled alarms using SIGALRM");
 191             exit(1);
 192         }
 193     }
 194
 195     event = event_freelist;
 196     if (event == NULL) {
 197         print_event_queue();
 198     }
 199     assert(event != NULL);
 200
 201     event->flags = TFLAG_NONE;
 202
 203     event_freelist = event->next;
 204     event->next = NULL;
 205
 206     check_event_queue();
 207
 208     *pTimer = (timer_t) event;
 209
 210     return 0;
 211 }
 212
 213 int timer_delete(
 214     timer_t timerid /* timer ID */
 215 )
 216 {
 217     struct event *event = (struct event *) timerid;
 218
 219     if (event->flags & TFLAG_DELETED) {
 220         TIMERDBG("Cannot delete a deleted event");
 221         return 1;
 222     }
 223
 224     timer_cancel(timerid);
 225
 226     event->flags |= TFLAG_DELETED;
 227
 228     event->next = event_freelist;
 229     event_freelist = event;
 230
 231     return 0;
 232 }
 233
 234 int timer_connect
 235 (
 236     timer_t     timerid, /* timer ID */
 237     void (*routine)(timer_t, int), /* user routine */
 238     int         arg      /* user argument */
 239 )
 240 {
 241     struct event *event = (struct event *) timerid;
 242
 243     assert(routine != NULL);
 244     event->func = routine;
 245     event->arg = arg;
 246
 247     return 0;
 248 }
 249
 250
 251 int timer_settime
 252 (
 253     timer_t                   timerid, /* timer ID */
 254     int                       flags,   /* absolute or relative */
 255     const struct itimerspec * value,   /* time to be set */
 256     struct itimerspec *       ovalue   /* previous time set (NULL=no result) */
 257 )
 258 {
 259     struct itimerval itimer;
 260     struct event *event = (struct event *) timerid;
 261     struct event **ppevent;
 262
 263     TIMESPEC_TO_TIMEVAL(&event->it_interval, &value->it_interval);
 264     TIMESPEC_TO_TIMEVAL(&event->it_value, &value->it_value);
 265
 266     /* if .it_value is zero, the timer is disarmed */
 267     if (!timerisset(&event->it_value)) {
 268         timer_cancel(timerid);
 269         return 0;
 270     }
 271
 272     block_timer();
 273
 274 #ifdef TIMER_PROFILE
 275     event->expected_ms = (event->it_value.tv_sec * MS_PER_SEC) + (event->it_value.tv_usec / US_PER_MS);
 276     event->start = uclock();
 277 #endif
 278     if (event->next) {
 279         TIMERDBG("calling timer_settime with a timer that is already on the queue.");
 280     }
 281
 282
 283     /* We always want to make sure that the event at the head of the
 284        queue has a timeout greater than the itimer granularity.
 285        Otherwise we end up with the situation that the time remaining
 286        on an itimer is greater than the time at the head of the queue
 287        in the first place. */
 288     timerroundup(&event->it_value, g_granularity);
 289
 290     timerclear(&itimer.it_value);
 291     getitimer(ITIMER_REAL, &itimer);
 292     if (timerisset(&itimer.it_value)) {
 293         // reset the top timer to have an interval equal to the remaining interval
 294         // when the timer was cancelled.
 295         if (event_queue) {
 296             if (timercmp(&(itimer.it_value), &(event_queue->it_value), >)) {
 297                 // it is an error if the amount of time remaining is more than the amount of time
 298                 // requested by the top event.
 299                 //
 300                 TIMERDBG("timer_settime: TIMER ERROR!");
 301
 302             } else {
 303                 // some portion of the top event has already expired.
 304                 // Reset the interval of the top event to remaining
 305                 // time left in that interval.
 306                 //
 307                 event_queue->it_value = itimer.it_value;
 308
 309                 // if we were the earliest timer before now, we are still the earliest timer now.
 310                 // we do not need to reorder the list.
 311             }
 312         }
 313     }
 314
 315     // Now, march down the list, decrementing the new timer by the
 316     // current it_value of each event on the queue.
 317     ppevent = &event_queue;
 318     while (*ppevent) {
 319         if ( timercmp(&(event->it_value), &((*ppevent)->it_value), <) ) {
 320             // if the proposed event will trigger sooner than the next event
 321             // in the queue, we will insert the new event just before the next one.
 322             //
 323             // we also need to adjust the delta value to the next event.
 324             timersub(&((*ppevent)->it_value), &(event->it_value), &((*ppevent)->it_value));
 325             break;
 326         }
 327         // subtract the interval of the next event from the proposed interval.
 328         timersub(&(event->it_value), &((*ppevent)->it_value), &(event->it_value));
 329
 330         ppevent = &((*ppevent)->next);
 331     }
 332
 333     // we have found our proper place in the queue,
 334     // link our new event into the pending event queue.
 335     event->next = *ppevent;
 336     *ppevent = event;
 337
 338     check_event_queue();
 339
 340     // if our new event ended up at the front of the queue, reissue the timer.
 341     if (event == event_queue) {
 342         timerroundup(&event_queue->it_value, g_granularity);
 343         timerclear(&itimer.it_interval);
 344         itimer.it_value = event_queue->it_value;
 345
 346         // we want to be sure to never turn off the timer completely,
 347         // so if the next interval is zero, set it to some small value.
 348         if (!timerisset(&(itimer.it_value)))
 349             itimer.it_value = (struct timeval) { 0, 1 };
 350
 351         assert(!timerisset(&itimer.it_interval));
 352         assert(itimer.it_value.tv_sec > 0 || itimer.it_value.tv_usec >= g_granularity);
 353         assert(event_queue->it_value.tv_sec > 0 || event_queue->it_value.tv_usec >= g_granularity);
 354         setitimer(ITIMER_REAL, &itimer, NULL);
 355         check_timer();
 356     }
 357
 358     event->flags &= ~TFLAG_CANCELLED;
 359
 360     unblock_timer();
 361
 362     return 0;
 363 }
 364
 365 static void check_timer()
 366 {
 367     struct itimerval itimer;
 368
 369     getitimer(ITIMER_REAL, &itimer);
 370     if (timerisset(&itimer.it_interval)) {
 371         TIMERDBG("ERROR timer interval is set.");
 372     }
 373     if (timercmp(&(itimer.it_value), &(event_queue->it_value), >)) {
 374         TIMERDBG("ERROR timer expires later than top event.");
 375     }
 376 }
 377
 378
 379 static void check_event_queue()
 380 {
 381     struct timeval sum;
 382     struct event *event;
 383     int i = 0;
 384
 385 #ifdef notdef
 386     int nfree = 0;
 387     struct event *p;
 388     for (p = event_freelist; p; p = p->next)
 389         nfree++;
 390     printf("%d free events\n", nfree);
 391 #endif
 392
 393     timerclear(&sum);
 394     for (event = event_queue; event; event = event->next) {
 395         if (i > g_maxevents) {
 396             TIMERDBG("timer queue looks like it loops back on itself!");
 397             print_event_queue();
 398             exit(1);
 399         }
 400         i++;
 401     }
 402 }
 403
 404 #if THIS_FINDS_USE
 405 /* The original upnp version has this unused function, so I left it in
 406    to maintain the resemblance. */
 407 static int count_queue(struct event *event_queue)
 408 {
 409     struct event *event;
 410     int i = 0;
 411     for (event = event_queue; event; event = event->next)
 412         i++;
 413     return i;
 414 }
 415 #endif
 416
 417 static void print_event_queue()
 418 {
 419     struct event *event;
 420     int i = 0;
 421
 422     for (event = event_queue; event; event = event->next) {
 423         printf("#%d (0x%x)->0x%x: \t%d sec %d usec\t%p\n",
 424                i++, (unsigned int) event, (unsigned int) event->next, (int) event->it_value.tv_sec, (int) event->it_value.tv_usec, event->func);
 425         if (i > g_maxevents) {
 426             printf("...(giving up)\n");
 427             break;
 428         }
 429     }
 430 }
 431
 432 // The top element of the event queue must have expired.
 433 // Remove that element, run its function, and reset the timer.
 434 // if there is no interval, recycle the event structure.
 435 static void alarm_handler(int i)
 436 {
 437     struct event *event, **ppevent;
 438     struct itimerval itimer;
 439     struct timeval small_interval = { 0, g_granularity/2 };
 440 #ifdef TIMER_PROFILE
 441     uint junk;
 442     uclock_t end;
 443     uint actual;
 444 #endif
 445
 446     block_timer();
 447
 448     // Loop through the event queue and remove the first event plus any
 449     // subsequent events that will expire very soon thereafter (within 'small_interval'}.
 450     //
 451     do {
 452         // remove the top event.
 453         event = event_queue;
 454         event_queue = event_queue->next;
 455         event->next = NULL;
 456
 457 #ifdef TIMER_PROFILE
 458         end = uclock();
 459         actual = ((end-event->start)/((uclock_t)UCLOCKS_PER_SEC/1000));
 460         if (actual < 0)
 461             junk = end;
 462         TIMERDBG("expected %d ms  actual %d ms", event->expected_ms, ((end-event->start)/((uclock_t)UCLOCKS_PER_SEC/1000)));
 463 #endif
 464
 465             // call the event callback function
 466             (*(event->func))((timer_t) event, (int)event->arg);
 467
 468         /* If the event has been cancelled, do NOT put it back on the queue. */
 469         if ( !(event->flags & TFLAG_CANCELLED) ) {
 470
 471             // if the event is a recurring event, reset the timer and
 472             // find its correct place in the sorted list of events.
 473             //
 474             if (timerisset(&event->it_interval)) {
 475                 // event is recurring...
 476                 //
 477                 event->it_value = event->it_interval;
 478 #ifdef TIMER_PROFILE
 479                 event->expected_ms = (event->it_value.tv_sec * MS_PER_SEC) + (event->it_value.tv_usec / US_PER_MS);
 480                 event->start = uclock();
 481 #endif
 482                 timerroundup(&event->it_value, g_granularity);
 483
 484                 // Now, march down the list, decrementing the new timer by the
 485                 // current delta of each event on the queue.
 486                 ppevent = &event_queue;
 487                 while (*ppevent) {
 488                     if ( timercmp(&(event->it_value), &((*ppevent)->it_value), <) ) {
 489                         // if the proposed event will trigger sooner than the next event
 490                         // in the queue, we will insert the new event just before the next one.
 491                         //
 492                         // we also need to adjust the delta value to the next event.
 493                         timersub(&((*ppevent)->it_value), &(event->it_value), &((*ppevent)->it_value));
 494                         break;
 495                     }
 496                     timersub(&(event->it_value), &((*ppevent)->it_value), &(event->it_value));
 497                     ppevent = &((*ppevent)->next);
 498                 }
 499
 500                 // we have found our proper place in the queue,
 501                 // link our new event into the pending event queue.
 502                 event->next = *ppevent;
 503                 *ppevent = event;
 504             } else {
 505                 // there is no interval, so recycle the event structure.
 506                 //timer_delete((timer_t) event);
 507             }
 508         }
 509
 510         check_event_queue();
 511
 512     } while (event_queue && timercmp(&event_queue->it_value, &small_interval, <));
 513
 514     // re-issue the timer...
 515     if (event_queue) {
 516         timerroundup(&event_queue->it_value, g_granularity);
 517
 518         timerclear(&itimer.it_interval);
 519         itimer.it_value = event_queue->it_value;
 520         // we want to be sure to never turn off the timer completely,
 521         // so if the next interval is zero, set it to some small value.
 522         if (!timerisset(&(itimer.it_value)))
 523             itimer.it_value = (struct timeval) { 0, 1 };
 524
 525         setitimer(ITIMER_REAL, &itimer, NULL);
 526         check_timer();
 527     } else {
 528         TIMERDBG("There are no events in the queue - timer not reset.");
 529     }
 530
 531     unblock_timer();
 532 }
 533
 534 static int block_count = 0;
 535
 536 void block_timer()
 537 {
 538     sigset_t set;
 539
 540     if (block_count++ == 0) {
 541         sigemptyset(&set);
 542         sigaddset(&set, SIGALRM);
 543         sigprocmask(SIG_BLOCK, &set, NULL);
 544     }
 545 }
 546
 547 void unblock_timer()
 548 {
 549     sigset_t set;
 550
 551     if (--block_count == 0) {
 552         sigemptyset(&set);
 553         sigaddset(&set, SIGALRM);
 554         sigprocmask(SIG_UNBLOCK, &set, NULL);
 555     }
 556 }
 557
 558 void timer_cancel_all()
 559 {
 560     struct itimerval timeroff = { { 0, 0 }, { 0, 0} };
 561     struct event *event;
 562     struct event **ppevent;
 563
 564     setitimer(ITIMER_REAL, &timeroff, NULL);
 565
 566     ppevent = &event_queue;
 567     while (*ppevent) {
 568         event = *ppevent;
 569         *ppevent = event->next;
 570         event->next = NULL;
 571     }
 572 }
 573
 574
 575
 576 void timer_cancel(timer_t timerid)
 577 {
 578     struct itimerval itimer;
 579     struct itimerval timeroff = { { 0, 0 }, { 0, 0} };
 580     struct event *event = (struct event *) timerid;
 581     struct event **ppevent;
 582
 583     if (event->flags & TFLAG_CANCELLED) {
 584         TIMERDBG("Cannot cancel a cancelled event");
 585         return;
 586     }
 587
 588     block_timer();
 589
 590     ppevent = &event_queue;
 591     while (*ppevent) {
 592         if ( *ppevent == event ) {
 593
 594             /* RACE CONDITION - if the alarm goes off while we are in
 595                this loop, and if the timer we want to cancel is the
 596                next to expire, the alarm will end up firing
 597                after this routine is complete, causing it to go off early. */
 598
 599             /* If the cancelled timer is the next to expire,
 600                we need to do something special to clean up correctly. */
 601             if (event == event_queue && event->next != NULL) {
 602                 timerclear(&itimer.it_value);
 603                 getitimer(ITIMER_REAL, &itimer);
 604
 605                 /* subtract the time that has already passed while waiting for this timer... */
 606                 timersub(&(event->it_value), &(itimer.it_value), &(event->it_value));
 607
 608                 /* and add any remainder to the next timer in the list */
 609                 timeradd(&(event->next->it_value), &(event->it_value), &(event->next->it_value));
 610             }
 611
 612             *ppevent = event->next;
 613             event->next = NULL;
 614
 615             if (event_queue) {
 616                 timerroundup(&event_queue->it_value, g_granularity);
 617                 timerclear(&itimer.it_interval);
 618                 itimer.it_value = event_queue->it_value;
 619
 620                 /* We want to be sure to never turn off the timer
 621                    completely if there are more events on the queue,
 622                    so if the next interval is zero, set it to some
 623                    small value.  */
 624
 625                 if (!timerisset(&(itimer.it_value)))
 626                     itimer.it_value = (struct timeval) { 0, 1 };
 627
 628                 assert(itimer.it_value.tv_sec > 0 || itimer.it_value.tv_usec >= g_granularity);
 629                 assert(event_queue->it_value.tv_sec > 0 || event_queue->it_value.tv_usec >= g_granularity);
 630                 setitimer(ITIMER_REAL, &itimer, NULL);
 631                 check_timer();
 632             } else {
 633                 setitimer(ITIMER_REAL, &timeroff, NULL);
 634             }
 635             break;
 636         }
 637         ppevent = &((*ppevent)->next);
 638     }
 639
 640     event->flags |= TFLAG_CANCELLED;
 641
 642     unblock_timer();
 643 }
 644
 645 /*
 646 * timer related headers
 647 */
 648 #include "bcmtimer.h"
 649
 650 /*
 651 * locally used global variables and constants
 652 */
 653
 654 /*
 655 * Initialize internal resources used in the timer module. It must be called
 656 * before any other timer function calls. The param 'timer_entries' is used
 657 * to pre-allocate fixed number of timer entries.
 658 */
 659 int bcm_timer_module_init(int timer_entries, bcm_timer_module_id *module_id)
 660 {
 661         init_event_queue(timer_entries);
 662         *module_id = (bcm_timer_module_id)event_freelist;
 663         return 0;
 664 }
 665
 666 /*
 667 * Cleanup internal resources used by this timer module. It deletes all
 668 * pending timer entries from the backend timer system as well.
 669 */
 670 int bcm_timer_module_cleanup(bcm_timer_module_id module_id)
 671 {
 672         module_id = 0;
 673         return 0;
 674 }
 675
 676 int bcm_timer_create(bcm_timer_module_id module_id, bcm_timer_id *timer_id)
 677 {
 678         module_id = 0;
 679         return timer_create(CLOCK_REALTIME, NULL, (timer_t *)timer_id);
 680 }
 681
 682 int bcm_timer_delete(bcm_timer_id timer_id)
 683 {
 684         return timer_delete((timer_t)timer_id);
 685 }
 686
 687 int bcm_timer_gettime(bcm_timer_id timer_id, struct itimerspec *timer_spec)
 688 {
 689         return -1;
 690 }
 691
 692 int bcm_timer_settime(bcm_timer_id timer_id, const struct itimerspec *timer_spec)
 693 {
 694         return timer_settime((timer_t)timer_id, 0, timer_spec, NULL);
 695 }
 696
 697 int bcm_timer_connect(bcm_timer_id timer_id, bcm_timer_cb func, int data)
 698 {
 699         return timer_connect((timer_t)timer_id, (void *)func, data);
 700 }
 701
 702 int bcm_timer_cancel(bcm_timer_id timer_id)
 703 {
 704         timer_cancel((timer_t)timer_id);
 705         return 0;
 706 }
 707