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 again:
  92         /* process all work events */
  93         if (next_work()) {
  94                 work = 1;
  95                 goto again;
  96         }
  97
  98         /* process timer events and get timeout for next timer event */
  99         temp = 0;
 100         timer = nearest_timer(&select_timer, &temp);
 101         if (temp) {
 102                 work = 1;
 103                 goto again;
 104         }
 105         if (polling)
 106                 timer = &no_time;
 107 //#warning TESTING
 108 //      if (!timer)
 109 //              printf("wait till infinity ..."); fflush(stdout);
 110
 111         FD_ZERO(&readset);
 112         FD_ZERO(&writeset);
 113         FD_ZERO(&exceptset);
 114
 115         /* prepare read and write fdsets */
 116         lcr_fd = fd_first;
 117         while(lcr_fd) {
 118                 if (lcr_fd->when & LCR_FD_READ)
 119                         FD_SET(lcr_fd->fd, &readset);
 120                 if (lcr_fd->when & LCR_FD_WRITE)
 121                         FD_SET(lcr_fd->fd, &writeset);
 122                 if (lcr_fd->when & LCR_FD_EXCEPT)
 123                         FD_SET(lcr_fd->fd, &exceptset);
 124                 lcr_fd = lcr_fd->next;
 125         }
 126
 127         if (unlock)
 128                 unlock();
 129         rc = select(maxfd+1, &readset, &writeset, &exceptset, timer);
 130         if (lock)
 131                 lock();
 132 //#warning TESTING
 133 //      if (!timer)
 134 //              printf("interrupted.\n");
 135         if (rc < 0)
 136                 return 0;
 137         if (global_change && *global_change) {
 138                 *global_change = 0;
 139                 return 1;
 140         }
 141
 142         /* fire timers */
 143 #if 0
 144         bsc_update_timers();
 145 #endif
 146
 147         /* call registered callback functions */
 148 restart:
 149         unregistered = 0;
 150         lcr_fd = fd_first;
 151         while(lcr_fd) {
 152                 int flags = 0;
 153
 154                 if (FD_ISSET(lcr_fd->fd, &readset)) {
 155                         flags |= LCR_FD_READ;
 156                         FD_CLR(lcr_fd->fd, &readset);
 157                 }
 158                 if (FD_ISSET(lcr_fd->fd, &writeset)) {
 159                         flags |= LCR_FD_WRITE;
 160                         FD_CLR(lcr_fd->fd, &writeset);
 161                 }
 162                 if (FD_ISSET(lcr_fd->fd, &exceptset)) {
 163                         flags |= LCR_FD_EXCEPT;
 164                         FD_CLR(lcr_fd->fd, &exceptset);
 165                 }
 166                 if (flags) {
 167                         work = 1;
 168                         lcr_fd->cb(lcr_fd, flags, lcr_fd->cb_instance, lcr_fd->cb_index);
 169                         if (unregistered)
 170                                 goto restart;
 171                         return 1;
 172                 }
 173                 lcr_fd = lcr_fd->next;
 174         }
 175         return work;
 176 }
 177
 178
 179 static struct lcr_timer *timer_first = NULL;
 180
 181 int _add_timer(struct lcr_timer *timer, int (*cb)(struct lcr_timer *timer, void *instance, int index), void *instance, int index, const char *func)
 182 {
 183         if (timer->inuse) {
 184                 FATAL("timer that is registered in function %s is already in use\n", func);
 185         }
 186
 187 #if 0
 188         struct lcr_timer *test = timer_first;
 189         while(test) {
 190                 if (test == timer)
 191                         FATAL("Timer already in list %s\n", func);
 192                 test = test->next;
 193         }
 194 #endif
 195
 196         timer->inuse = 1;
 197         timer->active = 0;
 198         timer->timeout.tv_sec = 0;
 199         timer->timeout.tv_usec = 0;
 200         timer->cb = cb;
 201         timer->cb_instance = instance;
 202         timer->cb_index = index;
 203         timer->next = timer_first;
 204         timer_first = timer;
 205
 206         return 0;
 207 }
 208
 209 void _del_timer(struct lcr_timer *timer, const char *func)
 210 {
 211         struct lcr_timer **lcr_timerp;
 212
 213         /* find pointer to timer */
 214         lcr_timerp = &timer_first;
 215         while(*lcr_timerp) {
 216                 if (*lcr_timerp == timer)
 217                         break;
 218                 lcr_timerp = &((*lcr_timerp)->next);
 219         }
 220         if (!*lcr_timerp) {
 221                 FATAL("timer deleted in function %s not in list\n", func);
 222         }
 223
 224         /* remove timer from list */
 225         timer->inuse = 0;
 226         *lcr_timerp = timer->next;
 227 }
 228
 229 void schedule_timer(struct lcr_timer *timer, int seconds, int microseconds)
 230 {
 231         struct timeval current_time;
 232
 233         if (!timer->inuse) {
 234                 FATAL("Timer not added\n");
 235         }
 236
 237         gettimeofday(&current_time, NULL);
 238         unsigned long long currentTime = current_time.tv_sec * MICRO_SECONDS + current_time.tv_usec;
 239         currentTime += seconds * MICRO_SECONDS + microseconds;
 240         timer->timeout.tv_sec = currentTime / MICRO_SECONDS;
 241         timer->timeout.tv_usec = currentTime % MICRO_SECONDS;
 242         timer->active = 1;
 243 }
 244
 245 void unsched_timer(struct lcr_timer *timer)
 246 {
 247         timer->active = 0;
 248 }
 249
 250 /* if a timeout is reached, process timer, if not, return timer value for select */
 251 static struct timeval *nearest_timer(struct timeval *select_timer, int *work)
 252 {
 253         struct timeval current;
 254         struct timeval *nearest = NULL;
 255         struct lcr_timer *lcr_timer, *lcr_nearest = NULL;
 256
 257         /* find nearest timer, or NULL, if no timer active */
 258         lcr_timer = timer_first;
 259         while(lcr_timer) {
 260                 if (lcr_timer->active && (!nearest || TIME_SMALLER(&lcr_timer->timeout, nearest))) {
 261                         nearest = &lcr_timer->timeout;
 262                         lcr_nearest = lcr_timer;
 263                 }
 264                 lcr_timer = lcr_timer->next;
 265         }
 266
 267         select_timer->tv_sec = 0;
 268         select_timer->tv_usec = 0;
 269
 270         if (!nearest)
 271                 return NULL; /* wait until infinity */
 272
 273         gettimeofday(&current, NULL);
 274         unsigned long long nearestTime = nearest->tv_sec * MICRO_SECONDS + nearest->tv_usec;
 275         unsigned long long currentTime = current.tv_sec * MICRO_SECONDS + current.tv_usec;
 276
 277         if (nearestTime > currentTime) {
 278                 select_timer->tv_sec = (nearestTime - currentTime) / MICRO_SECONDS;
 279                 select_timer->tv_usec = (nearestTime - currentTime) % MICRO_SECONDS;
 280                 return select_timer;
 281         } else {
 282                 lcr_nearest->active = 0;
 283                 (*lcr_nearest->cb)(lcr_nearest, lcr_nearest->cb_instance, lcr_nearest->cb_index);
 284                 /* don't wait so we can process the queues, indicate "work=1" */
 285                 select_timer->tv_sec = 0;
 286                 select_timer->tv_usec = 0;
 287                 *work = 1;
 288                 return select_timer;
 289         }
 290 }
 291
 292
 293 static struct lcr_work *work_first = NULL; /* chain of work */
 294 static struct lcr_work *first_event = NULL, *last_event = NULL; /* chain of active events */
 295
 296 #ifdef DEBUG_WORK
 297 void show_chain(const char *func)
 298 {
 299         struct lcr_work *work = first_event;
 300         printf("chain:%s\n", func);
 301         while(work) {
 302                 printf("%p - %p - %p\n", work->prev_event, work, work->next_event);
 303                 work = work->next_event;
 304         }
 305 }
 306 #endif
 307
 308 int _add_work(struct lcr_work *work, int (*cb)(struct lcr_work *work, void *instance, int index), void *instance, int index, const char *func)
 309 {
 310         if (work->inuse) {
 311                 FATAL("work that is registered in function %s is already in use\n", func);
 312         }
 313
 314 #ifdef DEBUG_WORK
 315         printf("add work %p from function %s\n", work, func);
 316         show_chain("before add");
 317 #endif
 318         work->inuse = 1;
 319         work->active = 0;
 320         work->cb = cb;
 321         work->cb_instance = instance;
 322         work->cb_index = index;
 323         work->next = work_first;
 324         work_first = work;
 325 #ifdef DEBUG_WORK
 326         show_chain("after add");
 327 #endif
 328
 329         return 0;
 330 }
 331
 332 void _del_work(struct lcr_work *work, const char *func)
 333 {
 334         struct lcr_work **lcr_workp;
 335
 336 #ifdef DEBUG_WORK
 337         show_chain("before detach");
 338 #endif
 339         if (work->active) {
 340                 /* first event removed */
 341                 if (!work->prev_event)
 342                         first_event = work->next_event;
 343                 else
 344                         work->prev_event->next_event = work->next_event;
 345                 /* last event removed */
 346                 if (!work->next_event)
 347                         last_event = work->prev_event;
 348                 else
 349                         work->next_event->prev_event = work->prev_event;
 350         }
 351 #ifdef DEBUG_WORK
 352         show_chain("after detach");
 353 #endif
 354
 355         /* find pointer to work */
 356         lcr_workp = &work_first;
 357         while(*lcr_workp) {
 358                 if (*lcr_workp == work)
 359                         break;
 360                 lcr_workp = &((*lcr_workp)->next);
 361         }
 362         if (!*lcr_workp) {
 363                 FATAL("work deleted by '%s' not in list\n", func);
 364         }
 365
 366         /* remove work from list */
 367         work->inuse = 0;
 368         *lcr_workp = work->next;
 369 #ifdef DEBUG_WORK
 370         show_chain("after delete");
 371 #endif
 372 }
 373
 374 void _trigger_work(struct lcr_work *work, const char *func)
 375 {
 376         if (!work->inuse) {
 377                 FATAL("Work not added, (called from func %s)\n", func);
 378         }
 379
 380         /* event already triggered */
 381         if (work->active)
 382                 return;
 383
 384 #ifdef DEBUG_WORK
 385         show_chain("before trigger");
 386 #endif
 387         /* append to tail of chain */
 388         if (last_event)
 389                 last_event->next_event = work;
 390         work->prev_event = last_event;
 391         work->next_event = NULL;
 392         last_event = work;
 393         if (!first_event)
 394                 first_event = work;
 395 #ifdef DEBUG_WORK
 396         show_chain("after trigger");
 397 #endif
 398
 399         work->active = 1;
 400 }
 401
 402 /* get first work and remove from event chain */
 403 static int next_work(void)
 404 {
 405         struct lcr_work *lcr_work;
 406
 407         if (!first_event)
 408                 return 0;
 409
 410 #ifdef DEBUG_WORK
 411         show_chain("before next_work");
 412 #endif
 413         if (!first_event->inuse) {
 414                 FATAL("Work not added\n");
 415         }
 416
 417         /* detach from event chain */
 418         lcr_work = first_event;
 419         first_event = lcr_work->next_event;
 420         if (!first_event)
 421                 last_event = NULL;
 422         else
 423                 first_event->prev_event = NULL;
 424
 425 #ifdef DEBUG_WORK
 426         show_chain("after next_work");
 427 #endif
 428         lcr_work->active = 0;
 429
 430         (*lcr_work->cb)(lcr_work, lcr_work->cb_instance, lcr_work->cb_index);
 431
 432         return 1;
 433 }
 434