select.c

   1 /* based on code from OpenBSC */
   2
   3 #include <stdio.h>
   4 #include <string.h>
   5 #include <stdarg.h>
   6 #include <unistd.h>
   7 #include <stdlib.h>
   8 #include <fcntl.h>
   9 #include <sys/time.h>
  10 #include "macro.h"
  11 #include "select.h"
  12
  13 static int maxfd = 0;
  14 static int unregistered;
  15 static struct lcr_fd *fd_first = NULL;
  16 static struct timeval *nearest_timer(struct timeval *select_timer, int *work);
  17 static int next_work(void);
  18
  19 int _register_fd(struct lcr_fd *fd, int when, int (*cb)(struct lcr_fd *fd, unsigned int what, void *instance, int index), void *instance, int index, const char *func)
  20 {
  21         int flags;
  22
  23         if (fd->inuse)
  24                 FATAL("FD that is registered in function %s is already in use\n", func);
  25 //      printf("registering fd %d  %s\n", fd->fd, func);
  26
  27         /* make FD nonblocking */
  28         flags = fcntl(fd->fd, F_GETFL);
  29         if (flags < 0)
  30                 FATAL("Failed to F_GETFL\n");
  31         flags |= O_NONBLOCK;
  32         flags = fcntl(fd->fd, F_SETFL, flags);
  33         if (flags < 0)
  34                 FATAL("Failed to F_SETFL O_NONBLOCK\n");
  35
  36         /* Register FD */
  37         if (fd->fd > maxfd)
  38                 maxfd = fd->fd;
  39
  40         /* append to list */
  41         fd->inuse = 1;
  42         fd->when = when;
  43         fd->cb = cb;
  44         fd->cb_instance = instance;
  45         fd->cb_index = index;
  46         fd->next = fd_first;
  47         fd_first = fd;
  48
  49         return 0;
  50 }
  51
  52 void _unregister_fd(struct lcr_fd *fd, const char *func)
  53 {
  54         struct lcr_fd **lcr_fdp;
  55
  56         /* find pointer to fd */
  57         lcr_fdp = &fd_first;
  58         while(*lcr_fdp) {
  59                 if (*lcr_fdp == fd)
  60                         break;
  61                 lcr_fdp = &((*lcr_fdp)->next);
  62         }
  63         if (!*lcr_fdp) {
  64                 FATAL("FD unregistered in function %s not in list\n", func);
  65         }
  66
  67         /* remove fd from list */
  68         fd->inuse = 0;
  69         *lcr_fdp = fd->next;
  70         unregistered = 1;
  71 }
  72
  73
  74 int select_main(int polling, int *global_change, void (*lock)(void), void (*unlock)(void))
  75 {
  76         struct lcr_fd *lcr_fd;
  77         fd_set readset, writeset, exceptset;
  78         int work = 0, temp, rc;
  79         struct timeval no_time = {0, 0};
  80         struct timeval select_timer, *timer;
  81
  82         /* goto again;
  83          *
  84          * this ensures that select is only called until:
  85          * - no work event exists
  86          * - and no timeout occurred
  87          *
  88          * if no future timeout exists, select will wait infinit.
  89          */
  90
  91 printf("-"); fflush(stdout);
  92 again:
  93 printf("1"); fflush(stdout);
  94         /* process all work events */
  95         if (next_work()) {
  96 printf("2"); fflush(stdout);
  97                 work = 1;
  98                 goto again;
  99         }
 100
 101         /* process timer events and get timeout for next timer event */
 102         temp = 0;
 103         timer = nearest_timer(&select_timer, &temp);
 104 printf("3"); fflush(stdout);
 105         if (temp) {
 106 printf("4"); fflush(stdout);
 107                 work = 1;
 108                 goto again;
 109         }
 110         if (polling)
 111                 timer = &no_time;
 112 //#warning TESTING
 113 //      if (!timer)
 114 //              printf("wait till infinity ..."); fflush(stdout);
 115
 116         FD_ZERO(&readset);
 117         FD_ZERO(&writeset);
 118         FD_ZERO(&exceptset);
 119
 120 printf("5"); fflush(stdout);
 121         /* prepare read and write fdsets */
 122         lcr_fd = fd_first;
 123         while(lcr_fd) {
 124                 if (lcr_fd->when & LCR_FD_READ)
 125                         FD_SET(lcr_fd->fd, &readset);
 126                 if (lcr_fd->when & LCR_FD_WRITE)
 127                         FD_SET(lcr_fd->fd, &writeset);
 128                 if (lcr_fd->when & LCR_FD_EXCEPT)
 129                         FD_SET(lcr_fd->fd, &exceptset);
 130                 lcr_fd = lcr_fd->next;
 131         }
 132 printf("6"); fflush(stdout);
 133
 134         if (unlock)
 135                 unlock();
 136         rc = select(maxfd+1, &readset, &writeset, &exceptset, timer);
 137         if (lock)
 138                 lock();
 139 //#warning TESTING
 140 //      if (!timer)
 141 //              printf("interrupted.\n");
 142         if (rc < 0)
 143                 return 0;
 144         if (global_change && *global_change) {
 145                 *global_change = 0;
 146                 return 1;
 147         }
 148 printf("7"); fflush(stdout);
 149
 150         /* fire timers */
 151 #if 0
 152         bsc_update_timers();
 153 #endif
 154
 155         /* call registered callback functions */
 156 restart:
 157 printf("8"); fflush(stdout);
 158         unregistered = 0;
 159         lcr_fd = fd_first;
 160         while(lcr_fd) {
 161                 int flags = 0;
 162
 163                 if (FD_ISSET(lcr_fd->fd, &readset)) {
 164                         flags |= LCR_FD_READ;
 165                         FD_CLR(lcr_fd->fd, &readset);
 166                 }
 167                 if (FD_ISSET(lcr_fd->fd, &writeset)) {
 168                         flags |= LCR_FD_WRITE;
 169                         FD_CLR(lcr_fd->fd, &writeset);
 170                 }
 171                 if (FD_ISSET(lcr_fd->fd, &exceptset)) {
 172                         flags |= LCR_FD_EXCEPT;
 173                         FD_CLR(lcr_fd->fd, &exceptset);
 174                 }
 175                 if (flags) {
 176 printf("9"); fflush(stdout);
 177                         work = 1;
 178                         lcr_fd->cb(lcr_fd, flags, lcr_fd->cb_instance, lcr_fd->cb_index);
 179                         if (unregistered)
 180                                 goto restart;
 181 printf("-"); fflush(stdout);
 182                         return 1;
 183                 }
 184                 lcr_fd = lcr_fd->next;
 185         }
 186         return work;
 187 }
 188
 189
 190 static struct lcr_timer *timer_first = NULL;
 191
 192 int _add_timer(struct lcr_timer *timer, int (*cb)(struct lcr_timer *timer, void *instance, int index), void *instance, int index, const char *func)
 193 {
 194         if (timer->inuse) {
 195                 FATAL("timer that is registered in function %s is already in use\n", func);
 196         }
 197
 198 #if 0
 199         struct lcr_timer *test = timer_first;
 200         while(test) {
 201                 if (test == timer)
 202                         FATAL("Timer already in list %s\n", func);
 203                 test = test->next;
 204         }
 205 #endif
 206
 207         timer->inuse = 1;
 208         timer->active = 0;
 209         timer->timeout.tv_sec = 0;
 210         timer->timeout.tv_usec = 0;
 211         timer->cb = cb;
 212         timer->cb_instance = instance;
 213         timer->cb_index = index;
 214         timer->next = timer_first;
 215         timer_first = timer;
 216
 217         return 0;
 218 }
 219
 220 void _del_timer(struct lcr_timer *timer, const char *func)
 221 {
 222         struct lcr_timer **lcr_timerp;
 223
 224         /* find pointer to timer */
 225         lcr_timerp = &timer_first;
 226         while(*lcr_timerp) {
 227                 if (*lcr_timerp == timer)
 228                         break;
 229                 lcr_timerp = &((*lcr_timerp)->next);
 230         }
 231         if (!*lcr_timerp) {
 232                 FATAL("timer deleted in function %s not in list\n", func);
 233         }
 234
 235         /* remove timer from list */
 236         timer->inuse = 0;
 237         *lcr_timerp = timer->next;
 238 }
 239
 240 void schedule_timer(struct lcr_timer *timer, int seconds, int microseconds)
 241 {
 242         struct timeval current_time;
 243
 244         if (!timer->inuse) {
 245                 FATAL("Timer not added\n");
 246         }
 247
 248         gettimeofday(&current_time, NULL);
 249         unsigned long long currentTime = current_time.tv_sec * MICRO_SECONDS + current_time.tv_usec;
 250         currentTime += seconds * MICRO_SECONDS + microseconds;
 251         timer->timeout.tv_sec = currentTime / MICRO_SECONDS;
 252         timer->timeout.tv_usec = currentTime % MICRO_SECONDS;
 253         timer->active = 1;
 254 }
 255
 256 void unsched_timer(struct lcr_timer *timer)
 257 {
 258         timer->active = 0;
 259 }
 260
 261 /* if a timeout is reached, process timer, if not, return timer value for select */
 262 static struct timeval *nearest_timer(struct timeval *select_timer, int *work)
 263 {
 264         struct timeval current;
 265         struct timeval *nearest = NULL;
 266         struct lcr_timer *lcr_timer, *lcr_nearest = NULL;
 267
 268         /* find nearest timer, or NULL, if no timer active */
 269         lcr_timer = timer_first;
 270         while(lcr_timer) {
 271                 if (lcr_timer->active && (!nearest || TIME_SMALLER(&lcr_timer->timeout, nearest))) {
 272                         nearest = &lcr_timer->timeout;
 273                         lcr_nearest = lcr_timer;
 274                 }
 275                 lcr_timer = lcr_timer->next;
 276         }
 277
 278         select_timer->tv_sec = 0;
 279         select_timer->tv_usec = 0;
 280
 281         if (!nearest)
 282                 return NULL; /* wait until infinity */
 283
 284         gettimeofday(&current, NULL);
 285         unsigned long long nearestTime = nearest->tv_sec * MICRO_SECONDS + nearest->tv_usec;
 286         unsigned long long currentTime = current.tv_sec * MICRO_SECONDS + current.tv_usec;
 287
 288         if (nearestTime > currentTime) {
 289                 select_timer->tv_sec = (nearestTime - currentTime) / MICRO_SECONDS;
 290                 select_timer->tv_usec = (nearestTime - currentTime) % MICRO_SECONDS;
 291                 return select_timer;
 292         } else {
 293                 lcr_nearest->active = 0;
 294                 (*lcr_nearest->cb)(lcr_nearest, lcr_nearest->cb_instance, lcr_nearest->cb_index);
 295                 /* don't wait so we can process the queues, indicate "work=1" */
 296                 select_timer->tv_sec = 0;
 297                 select_timer->tv_usec = 0;
 298                 *work = 1;
 299                 return select_timer;
 300         }
 301 }
 302
 303
 304 static struct lcr_work *work_first = NULL; /* chain of work */
 305 static struct lcr_work *first_event = NULL, *last_event = NULL; /* chain of active events */
 306
 307 #ifdef DEBUG_WORK
 308 void show_chain(const char *func)
 309 {
 310         struct lcr_work *work = first_event;
 311         printf("chain:%s\n", func);
 312         while(work) {
 313                 printf("%p - %p - %p\n", work->prev_event, work, work->next_event);
 314                 work = work->next_event;
 315         }
 316 }
 317 #endif
 318
 319 int _add_work(struct lcr_work *work, int (*cb)(struct lcr_work *work, void *instance, int index), void *instance, int index, const char *func)
 320 {
 321         if (work->inuse) {
 322                 FATAL("work that is registered in function %s is already in use\n", func);
 323         }
 324
 325 #ifdef DEBUG_WORK
 326         printf("add work %p from function %s\n", work, func);
 327         show_chain("before add");
 328 #endif
 329         work->inuse = 1;
 330         work->active = 0;
 331         work->cb = cb;
 332         work->cb_instance = instance;
 333         work->cb_index = index;
 334         work->next = work_first;
 335         work_first = work;
 336 #ifdef DEBUG_WORK
 337         show_chain("after add");
 338 #endif
 339
 340         return 0;
 341 }
 342
 343 void _del_work(struct lcr_work *work, const char *func)
 344 {
 345         struct lcr_work **lcr_workp;
 346
 347 #ifdef DEBUG_WORK
 348         show_chain("before detach");
 349 #endif
 350         if (work->active) {
 351                 /* first event removed */
 352                 if (!work->prev_event)
 353                         first_event = work->next_event;
 354                 else
 355                         work->prev_event->next_event = work->next_event;
 356                 /* last event removed */
 357                 if (!work->next_event)
 358                         last_event = work->prev_event;
 359                 else
 360                         work->next_event->prev_event = work->prev_event;
 361         }
 362 #ifdef DEBUG_WORK
 363         show_chain("after detach");
 364 #endif
 365
 366         /* find pointer to work */
 367         lcr_workp = &work_first;
 368         while(*lcr_workp) {
 369                 if (*lcr_workp == work)
 370                         break;
 371                 lcr_workp = &((*lcr_workp)->next);
 372         }
 373         if (!*lcr_workp) {
 374                 FATAL("work deleted by '%s' not in list\n", func);
 375         }
 376
 377         /* remove work from list */
 378         work->inuse = 0;
 379         *lcr_workp = work->next;
 380 #ifdef DEBUG_WORK
 381         show_chain("after delete");
 382 #endif
 383 }
 384
 385 void trigger_work(struct lcr_work *work)
 386 {
 387         if (!work->inuse) {
 388                 FATAL("Work not added\n");
 389         }
 390
 391         /* event already triggered */
 392         if (work->active)
 393                 return;
 394
 395 #ifdef DEBUG_WORK
 396         show_chain("before trigger");
 397 #endif
 398         /* append to tail of chain */
 399         if (last_event)
 400                 last_event->next_event = work;
 401         work->prev_event = last_event;
 402         work->next_event = NULL;
 403         last_event = work;
 404         if (!first_event)
 405                 first_event = work;
 406 #ifdef DEBUG_WORK
 407         show_chain("after trigger");
 408 #endif
 409
 410         work->active = 1;
 411 }
 412
 413 /* get first work and remove from event chain */
 414 static int next_work(void)
 415 {
 416         struct lcr_work *lcr_work;
 417
 418         if (!first_event)
 419                 return 0;
 420
 421 #ifdef DEBUG_WORK
 422         show_chain("before next_work");
 423 #endif
 424         if (!first_event->inuse) {
 425                 FATAL("Work not added\n");
 426         }
 427
 428         /* detach from event chain */
 429         lcr_work = first_event;
 430         first_event = lcr_work->next_event;
 431         if (!first_event)
 432                 last_event = NULL;
 433         else
 434                 first_event->prev_event = NULL;
 435
 436 #ifdef DEBUG_WORK
 437         show_chain("after next_work");
 438 #endif
 439         lcr_work->active = 0;
 440
 441         (*lcr_work->cb)(lcr_work, lcr_work->cb_instance, lcr_work->cb_index);
 442
 443         return 1;
 444 }
 445