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 printf("0"); fflush(stdout);
 180                         if (unregistered)
 181                                 goto restart;
 182 printf("-"); fflush(stdout);
 183                         return 1;
 184                 }
 185                 lcr_fd = lcr_fd->next;
 186         }
 187         return work;
 188 }
 189
 190
 191 static struct lcr_timer *timer_first = NULL;
 192
 193 int _add_timer(struct lcr_timer *timer, int (*cb)(struct lcr_timer *timer, void *instance, int index), void *instance, int index, const char *func)
 194 {
 195         if (timer->inuse) {
 196                 FATAL("timer that is registered in function %s is already in use\n", func);
 197         }
 198
 199 #if 0
 200         struct lcr_timer *test = timer_first;
 201         while(test) {
 202                 if (test == timer)
 203                         FATAL("Timer already in list %s\n", func);
 204                 test = test->next;
 205         }
 206 #endif
 207
 208         timer->inuse = 1;
 209         timer->active = 0;
 210         timer->timeout.tv_sec = 0;
 211         timer->timeout.tv_usec = 0;
 212         timer->cb = cb;
 213         timer->cb_instance = instance;
 214         timer->cb_index = index;
 215         timer->next = timer_first;
 216         timer_first = timer;
 217
 218         return 0;
 219 }
 220
 221 void _del_timer(struct lcr_timer *timer, const char *func)
 222 {
 223         struct lcr_timer **lcr_timerp;
 224
 225         /* find pointer to timer */
 226         lcr_timerp = &timer_first;
 227         while(*lcr_timerp) {
 228                 if (*lcr_timerp == timer)
 229                         break;
 230                 lcr_timerp = &((*lcr_timerp)->next);
 231         }
 232         if (!*lcr_timerp) {
 233                 FATAL("timer deleted in function %s not in list\n", func);
 234         }
 235
 236         /* remove timer from list */
 237         timer->inuse = 0;
 238         *lcr_timerp = timer->next;
 239 }
 240
 241 void schedule_timer(struct lcr_timer *timer, int seconds, int microseconds)
 242 {
 243         struct timeval current_time;
 244
 245         if (!timer->inuse) {
 246                 FATAL("Timer not added\n");
 247         }
 248
 249         gettimeofday(&current_time, NULL);
 250         unsigned long long currentTime = current_time.tv_sec * MICRO_SECONDS + current_time.tv_usec;
 251         currentTime += seconds * MICRO_SECONDS + microseconds;
 252         timer->timeout.tv_sec = currentTime / MICRO_SECONDS;
 253         timer->timeout.tv_usec = currentTime % MICRO_SECONDS;
 254         timer->active = 1;
 255 }
 256
 257 void unsched_timer(struct lcr_timer *timer)
 258 {
 259         timer->active = 0;
 260 }
 261
 262 /* if a timeout is reached, process timer, if not, return timer value for select */
 263 static struct timeval *nearest_timer(struct timeval *select_timer, int *work)
 264 {
 265         struct timeval current;
 266         struct timeval *nearest = NULL;
 267         struct lcr_timer *lcr_timer, *lcr_nearest = NULL;
 268
 269         /* find nearest timer, or NULL, if no timer active */
 270         lcr_timer = timer_first;
 271         while(lcr_timer) {
 272                 if (lcr_timer->active && (!nearest || TIME_SMALLER(&lcr_timer->timeout, nearest))) {
 273                         nearest = &lcr_timer->timeout;
 274                         lcr_nearest = lcr_timer;
 275                 }
 276                 lcr_timer = lcr_timer->next;
 277         }
 278
 279         select_timer->tv_sec = 0;
 280         select_timer->tv_usec = 0;
 281
 282         if (!nearest)
 283                 return NULL; /* wait until infinity */
 284
 285         gettimeofday(&current, NULL);
 286         unsigned long long nearestTime = nearest->tv_sec * MICRO_SECONDS + nearest->tv_usec;
 287         unsigned long long currentTime = current.tv_sec * MICRO_SECONDS + current.tv_usec;
 288
 289         if (nearestTime > currentTime) {
 290                 select_timer->tv_sec = (nearestTime - currentTime) / MICRO_SECONDS;
 291                 select_timer->tv_usec = (nearestTime - currentTime) % MICRO_SECONDS;
 292                 return select_timer;
 293         } else {
 294                 lcr_nearest->active = 0;
 295                 (*lcr_nearest->cb)(lcr_nearest, lcr_nearest->cb_instance, lcr_nearest->cb_index);
 296                 /* don't wait so we can process the queues, indicate "work=1" */
 297                 select_timer->tv_sec = 0;
 298                 select_timer->tv_usec = 0;
 299                 *work = 1;
 300                 return select_timer;
 301         }
 302 }
 303
 304
 305 static struct lcr_work *work_first = NULL; /* chain of work */
 306 static struct lcr_work *first_event = NULL, *last_event = NULL; /* chain of active events */
 307
 308 #ifdef DEBUG_WORK
 309 void show_chain(const char *func)
 310 {
 311         struct lcr_work *work = first_event;
 312         printf("chain:%s\n", func);
 313         while(work) {
 314                 printf("%p - %p - %p\n", work->prev_event, work, work->next_event);
 315                 work = work->next_event;
 316         }
 317 }
 318 #endif
 319
 320 int _add_work(struct lcr_work *work, int (*cb)(struct lcr_work *work, void *instance, int index), void *instance, int index, const char *func)
 321 {
 322         if (work->inuse) {
 323                 FATAL("work that is registered in function %s is already in use\n", func);
 324         }
 325
 326 #ifdef DEBUG_WORK
 327         printf("add work %p from function %s\n", work, func);
 328         show_chain("before add");
 329 #endif
 330         work->inuse = 1;
 331         work->active = 0;
 332         work->cb = cb;
 333         work->cb_instance = instance;
 334         work->cb_index = index;
 335         work->next = work_first;
 336         work_first = work;
 337 #ifdef DEBUG_WORK
 338         show_chain("after add");
 339 #endif
 340
 341         return 0;
 342 }
 343
 344 void _del_work(struct lcr_work *work, const char *func)
 345 {
 346         struct lcr_work **lcr_workp;
 347
 348 #ifdef DEBUG_WORK
 349         show_chain("before detach");
 350 #endif
 351         if (work->active) {
 352                 /* first event removed */
 353                 if (!work->prev_event)
 354                         first_event = work->next_event;
 355                 else
 356                         work->prev_event->next_event = work->next_event;
 357                 /* last event removed */
 358                 if (!work->next_event)
 359                         last_event = work->prev_event;
 360                 else
 361                         work->next_event->prev_event = work->prev_event;
 362         }
 363 #ifdef DEBUG_WORK
 364         show_chain("after detach");
 365 #endif
 366
 367         /* find pointer to work */
 368         lcr_workp = &work_first;
 369         while(*lcr_workp) {
 370                 if (*lcr_workp == work)
 371                         break;
 372                 lcr_workp = &((*lcr_workp)->next);
 373         }
 374         if (!*lcr_workp) {
 375                 FATAL("work deleted by '%s' not in list\n", func);
 376         }
 377
 378         /* remove work from list */
 379         work->inuse = 0;
 380         *lcr_workp = work->next;
 381 #ifdef DEBUG_WORK
 382         show_chain("after delete");
 383 #endif
 384 }
 385
 386 void trigger_work(struct lcr_work *work)
 387 {
 388         if (!work->inuse) {
 389                 FATAL("Work not added\n");
 390         }
 391
 392         /* event already triggered */
 393         if (work->active)
 394                 return;
 395
 396 #ifdef DEBUG_WORK
 397         show_chain("before trigger");
 398 #endif
 399         /* append to tail of chain */
 400         if (last_event)
 401                 last_event->next_event = work;
 402         work->prev_event = last_event;
 403         work->next_event = NULL;
 404         last_event = work;
 405         if (!first_event)
 406                 first_event = work;
 407 #ifdef DEBUG_WORK
 408         show_chain("after trigger");
 409 #endif
 410
 411         work->active = 1;
 412 }
 413
 414 /* get first work and remove from event chain */
 415 static int next_work(void)
 416 {
 417         struct lcr_work *lcr_work;
 418
 419         if (!first_event)
 420                 return 0;
 421
 422 #ifdef DEBUG_WORK
 423         show_chain("before next_work");
 424 #endif
 425         if (!first_event->inuse) {
 426                 FATAL("Work not added\n");
 427         }
 428
 429         /* detach from event chain */
 430         lcr_work = first_event;
 431         first_event = lcr_work->next_event;
 432         if (!first_event)
 433                 last_event = NULL;
 434         else
 435                 first_event->prev_event = NULL;
 436
 437 #ifdef DEBUG_WORK
 438         show_chain("after next_work");
 439 #endif
 440         lcr_work->active = 0;
 441
 442         (*lcr_work->cb)(lcr_work, lcr_work->cb_instance, lcr_work->cb_index);
 443
 444         return 1;
 445 }
 446