l4/pkg/ankh/lib/lwip/arch/sys_arch.c

   1 /*
   2  * Copyright (c) 2001-2003 Swedish Institute of Computer Science.
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without modification,
   6  * are permitted provided that the following conditions are met:
   7  *
   8  * 1. Redistributions of source code must retain the above copyright notice,
   9  *    this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright notice,
  11  *    this list of conditions and the following disclaimer in the documentation
  12  *    and/or other materials provided with the distribution.
  13  * 3. The name of the author may not be used to endorse or promote products
  14  *    derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
  17  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  18  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
  19  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  20  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
  21  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
  24  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
  25  * OF SUCH DAMAGE.
  26  *
  27  * This file is part of the lwIP TCP/IP stack.
  28  *
  29  * Author: Adam Dunkels <adam@sics.se>
  30  *
  31  */
  32
  33 /*
  34  * Wed Apr 17 16:05:29 EDT 2002 (James Roth)
  35  *
  36  *  - Fixed an unlikely sys_thread_new() race condition.
  37  *
  38  *  - Made current_thread() work with threads which where
  39  *    not created with sys_thread_new().  This includes
  40  *    the main thread and threads made with pthread_create().
  41  *
  42  *  - Catch overflows where more than SYS_MBOX_SIZE messages
  43  *    are waiting to be read.  The sys_mbox_post() routine
  44  *    will block until there is more room instead of just
  45  *    leaking messages.
  46  */
  47 #include "lwip/debug.h"
  48
  49 #include <string.h>
  50 #include <sys/time.h>
  51 #include <sys/types.h>
  52 #include <stdlib.h>
  53 #include <unistd.h>
  54 #include <pthread.h>
  55
  56 #include "lwip/sys.h"
  57 #include "lwip/opt.h"
  58 #include "lwip/stats.h"
  59
  60 #define UMAX(a, b)      ((a) > (b) ? (a) : (b))
  61
  62 static struct timeval starttime;
  63
  64 #if !NO_SYS
  65
  66 static struct sys_thread *threads = NULL;
  67 static pthread_mutex_t threads_mutex = PTHREAD_MUTEX_INITIALIZER;
  68
  69 struct sys_mbox_msg {
  70   struct sys_mbox_msg *next;
  71   void *msg;
  72 };
  73
  74 #define SYS_MBOX_SIZE 128
  75
  76 struct sys_mbox {
  77   int first, last;
  78   void *msgs[SYS_MBOX_SIZE];
  79   struct sys_sem *not_empty;
  80   struct sys_sem *not_full;
  81   struct sys_sem *mutex;
  82   int wait_send;
  83 };
  84
  85 struct sys_sem {
  86   unsigned int c;
  87   pthread_cond_t cond;
  88   pthread_mutex_t mutex;
  89 };
  90
  91 struct sys_thread {
  92   struct sys_thread *next;
  93   pthread_t pthread;
  94 };
  95
  96 #if SYS_LIGHTWEIGHT_PROT
  97 static pthread_mutex_t lwprot_mutex = PTHREAD_MUTEX_INITIALIZER;
  98 static pthread_t lwprot_thread = (pthread_t)0xDEAD;
  99 static int lwprot_count = 0;
 100 #endif /* SYS_LIGHTWEIGHT_PROT */
 101
 102 static struct sys_sem *sys_sem_new_internal(u8_t count);
 103 static void sys_sem_free_internal(struct sys_sem *sem);
 104
 105 static u32_t cond_wait(pthread_cond_t * cond, pthread_mutex_t * mutex,
 106                        u32_t timeout);
 107
 108 /*-----------------------------------------------------------------------------------*/
 109 static struct sys_thread *
 110 introduce_thread(pthread_t id)
 111 {
 112   struct sys_thread *thread;
 113
 114   thread = (struct sys_thread *)malloc(sizeof(struct sys_thread));
 115
 116   if (thread != NULL) {
 117     pthread_mutex_lock(&threads_mutex);
 118     thread->next = threads;
 119     thread->pthread = id;
 120     threads = thread;
 121     pthread_mutex_unlock(&threads_mutex);
 122   }
 123
 124   return thread;
 125 }
 126 /*-----------------------------------------------------------------------------------*/
 127 sys_thread_t
 128 sys_thread_new(const char *name, lwip_thread_fn function, void *arg, int stacksize, int prio)
 129 {
 130   int code;
 131   pthread_t tmp;
 132   struct sys_thread *st = NULL;
 133   LWIP_UNUSED_ARG(name);
 134   LWIP_UNUSED_ARG(stacksize);
 135   LWIP_UNUSED_ARG(prio);
 136
 137   code = pthread_create(&tmp,
 138                         NULL,
 139                         (void *(*)(void *))
 140                         function,
 141                         arg);
 142
 143   if (0 == code) {
 144     st = introduce_thread(tmp);
 145   }
 146
 147   if (NULL == st) {
 148     LWIP_DEBUGF(SYS_DEBUG, ("sys_thread_new: pthread_create %d, st = 0x%lx",
 149                        code, (unsigned long)st));
 150     abort();
 151   }
 152   return st;
 153 }
 154 /*-----------------------------------------------------------------------------------*/
 155 err_t
 156 sys_mbox_new(struct sys_mbox **mb, int size)
 157 {
 158   struct sys_mbox *mbox;
 159   LWIP_UNUSED_ARG(size);
 160
 161   mbox = (struct sys_mbox *)malloc(sizeof(struct sys_mbox));
 162   if (mbox == NULL) {
 163     return ERR_MEM;
 164   }
 165   mbox->first = mbox->last = 0;
 166   mbox->not_empty = sys_sem_new_internal(0);
 167   mbox->not_full = sys_sem_new_internal(0);
 168   mbox->mutex = sys_sem_new_internal(1);
 169   mbox->wait_send = 0;
 170
 171   SYS_STATS_INC_USED(mbox);
 172   *mb = mbox;
 173   return ERR_OK;
 174 }
 175 /*-----------------------------------------------------------------------------------*/
 176 void
 177 sys_mbox_free(struct sys_mbox **mb)
 178 {
 179   if ((mb != NULL) && (*mb != SYS_MBOX_NULL)) {
 180     struct sys_mbox *mbox = *mb;
 181     SYS_STATS_DEC(mbox.used);
 182     sys_arch_sem_wait(&mbox->mutex, 0);
 183
 184     sys_sem_free_internal(mbox->not_empty);
 185     sys_sem_free_internal(mbox->not_full);
 186     sys_sem_free_internal(mbox->mutex);
 187     mbox->not_empty = mbox->not_full = mbox->mutex = NULL;
 188     /*  LWIP_DEBUGF("sys_mbox_free: mbox 0x%lx\n", mbox); */
 189     free(mbox);
 190   }
 191 }
 192 /*-----------------------------------------------------------------------------------*/
 193 err_t
 194 sys_mbox_trypost(struct sys_mbox **mb, void *msg)
 195 {
 196   u8_t first;
 197   struct sys_mbox *mbox;
 198   LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
 199   mbox = *mb;
 200
 201   sys_arch_sem_wait(&mbox->mutex, 0);
 202
 203   LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_trypost: mbox %p msg %p\n",
 204                           (void *)mbox, (void *)msg));
 205
 206   if ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) {
 207     sys_sem_signal(&mbox->mutex);
 208     return ERR_MEM;
 209   }
 210
 211   mbox->msgs[mbox->last % SYS_MBOX_SIZE] = msg;
 212
 213   if (mbox->last == mbox->first) {
 214     first = 1;
 215   } else {
 216     first = 0;
 217   }
 218
 219   mbox->last++;
 220
 221   if (first) {
 222     sys_sem_signal(&mbox->not_empty);
 223   }
 224
 225   sys_sem_signal(&mbox->mutex);
 226
 227   return ERR_OK;
 228 }
 229 /*-----------------------------------------------------------------------------------*/
 230 void
 231 sys_mbox_post(struct sys_mbox **mb, void *msg)
 232 {
 233   u8_t first;
 234   struct sys_mbox *mbox;
 235   LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
 236   mbox = *mb;
 237
 238   sys_arch_sem_wait(&mbox->mutex, 0);
 239
 240   LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_post: mbox %p msg %p\n", (void *)mbox, (void *)msg));
 241
 242   while ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) {
 243     mbox->wait_send++;
 244     sys_sem_signal(&mbox->mutex);
 245     sys_arch_sem_wait(&mbox->not_full, 0);
 246     sys_arch_sem_wait(&mbox->mutex, 0);
 247     mbox->wait_send--;
 248   }
 249
 250   mbox->msgs[mbox->last % SYS_MBOX_SIZE] = msg;
 251
 252   if (mbox->last == mbox->first) {
 253     first = 1;
 254   } else {
 255     first = 0;
 256   }
 257
 258   mbox->last++;
 259
 260   if (first) {
 261     sys_sem_signal(&mbox->not_empty);
 262   }
 263
 264   sys_sem_signal(&mbox->mutex);
 265 }
 266 /*-----------------------------------------------------------------------------------*/
 267 u32_t
 268 sys_arch_mbox_tryfetch(struct sys_mbox **mb, void **msg)
 269 {
 270   struct sys_mbox *mbox;
 271   LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
 272   mbox = *mb;
 273
 274   sys_arch_sem_wait(&mbox->mutex, 0);
 275
 276   if (mbox->first == mbox->last) {
 277     sys_sem_signal(&mbox->mutex);
 278     return SYS_MBOX_EMPTY;
 279   }
 280
 281   if (msg != NULL) {
 282     LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_tryfetch: mbox %p msg %p\n", (void *)mbox, *msg));
 283     *msg = mbox->msgs[mbox->first % SYS_MBOX_SIZE];
 284   }
 285   else{
 286     LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_tryfetch: mbox %p, null msg\n", (void *)mbox));
 287   }
 288
 289   mbox->first++;
 290
 291   if (mbox->wait_send) {
 292     sys_sem_signal(&mbox->not_full);
 293   }
 294
 295   sys_sem_signal(&mbox->mutex);
 296
 297   return 0;
 298 }
 299 /*-----------------------------------------------------------------------------------*/
 300 u32_t
 301 sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout)
 302 {
 303   u32_t time_needed = 0;
 304   struct sys_mbox *mbox;
 305   LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL));
 306   mbox = *mb;
 307
 308   /* The mutex lock is quick so we don't bother with the timeout
 309      stuff here. */
 310   sys_arch_sem_wait(&mbox->mutex, 0);
 311
 312   while (mbox->first == mbox->last) {
 313     sys_sem_signal(&mbox->mutex);
 314
 315     /* We block while waiting for a mail to arrive in the mailbox. We
 316        must be prepared to timeout. */
 317     if (timeout != 0) {
 318       time_needed = sys_arch_sem_wait(&mbox->not_empty, timeout);
 319
 320       if (time_needed == SYS_ARCH_TIMEOUT) {
 321         return SYS_ARCH_TIMEOUT;
 322       }
 323     } else {
 324       sys_arch_sem_wait(&mbox->not_empty, 0);
 325     }
 326
 327     sys_arch_sem_wait(&mbox->mutex, 0);
 328   }
 329
 330   if (msg != NULL) {
 331     LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_fetch: mbox %p msg %p\n", (void *)mbox, *msg));
 332     *msg = mbox->msgs[mbox->first % SYS_MBOX_SIZE];
 333   }
 334   else{
 335     LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_fetch: mbox %p, null msg\n", (void *)mbox));
 336   }
 337
 338   mbox->first++;
 339
 340   if (mbox->wait_send) {
 341     sys_sem_signal(&mbox->not_full);
 342   }
 343
 344   sys_sem_signal(&mbox->mutex);
 345
 346   return time_needed;
 347 }
 348 /*-----------------------------------------------------------------------------------*/
 349 static struct sys_sem *
 350 sys_sem_new_internal(u8_t count)
 351 {
 352   struct sys_sem *sem;
 353
 354   sem = (struct sys_sem *)malloc(sizeof(struct sys_sem));
 355   if (sem != NULL) {
 356     sem->c = count;
 357     pthread_cond_init(&(sem->cond), NULL);
 358     pthread_mutex_init(&(sem->mutex), NULL);
 359   }
 360   return sem;
 361 }
 362 /*-----------------------------------------------------------------------------------*/
 363 err_t
 364 sys_sem_new(struct sys_sem **sem, u8_t count)
 365 {
 366   SYS_STATS_INC_USED(sem);
 367   *sem = sys_sem_new_internal(count);
 368   if (*sem == NULL) {
 369     return ERR_MEM;
 370   }
 371   return ERR_OK;
 372 }
 373 /*-----------------------------------------------------------------------------------*/
 374 static u32_t
 375 cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex, u32_t timeout)
 376 {
 377   int tdiff;
 378   unsigned long sec, usec;
 379   struct timeval rtime1, rtime2;
 380   struct timespec ts;
 381   int retval;
 382
 383   if (timeout > 0) {
 384     /* Get a timestamp and add the timeout value. */
 385     gettimeofday(&rtime1, NULL);
 386     sec = rtime1.tv_sec;
 387     usec = rtime1.tv_usec;
 388     usec += timeout % 1000 * 1000;
 389     sec += (int)(timeout / 1000) + (int)(usec / 1000000);
 390     usec = usec % 1000000;
 391     ts.tv_nsec = usec * 1000;
 392     ts.tv_sec = sec;
 393
 394     retval = pthread_cond_timedwait(cond, mutex, &ts);
 395
 396     if (retval == ETIMEDOUT) {
 397       return SYS_ARCH_TIMEOUT;
 398     } else {
 399       /* Calculate for how long we waited for the cond. */
 400       gettimeofday(&rtime2, NULL);
 401       tdiff = (rtime2.tv_sec - rtime1.tv_sec) * 1000 +
 402         (rtime2.tv_usec - rtime1.tv_usec) / 1000;
 403
 404       if (tdiff <= 0) {
 405         return 0;
 406       }
 407
 408       return tdiff;
 409     }
 410   } else {
 411     pthread_cond_wait(cond, mutex);
 412     return SYS_ARCH_TIMEOUT;
 413   }
 414 }
 415 /*-----------------------------------------------------------------------------------*/
 416 u32_t
 417 sys_arch_sem_wait(struct sys_sem **s, u32_t timeout)
 418 {
 419   u32_t time_needed = 0;
 420   struct sys_sem *sem;
 421   LWIP_ASSERT("invalid sem", (s != NULL) && (*s != NULL));
 422   sem = *s;
 423
 424   pthread_mutex_lock(&(sem->mutex));
 425   while (sem->c <= 0) {
 426     if (timeout > 0) {
 427       time_needed = cond_wait(&(sem->cond), &(sem->mutex), timeout);
 428
 429       if (time_needed == SYS_ARCH_TIMEOUT) {
 430         pthread_mutex_unlock(&(sem->mutex));
 431         return SYS_ARCH_TIMEOUT;
 432       }
 433       /*      pthread_mutex_unlock(&(sem->mutex));
 434               return time_needed; */
 435     } else {
 436       cond_wait(&(sem->cond), &(sem->mutex), 0);
 437     }
 438   }
 439   sem->c--;
 440   pthread_mutex_unlock(&(sem->mutex));
 441   return time_needed;
 442 }
 443 /*-----------------------------------------------------------------------------------*/
 444 void
 445 sys_sem_signal(struct sys_sem **s)
 446 {
 447   struct sys_sem *sem;
 448   LWIP_ASSERT("invalid sem", (s != NULL) && (*s != NULL));
 449   sem = *s;
 450
 451   pthread_mutex_lock(&(sem->mutex));
 452   sem->c++;
 453
 454   if (sem->c > 1) {
 455     sem->c = 1;
 456   }
 457
 458   pthread_cond_broadcast(&(sem->cond));
 459   pthread_mutex_unlock(&(sem->mutex));
 460 }
 461 /*-----------------------------------------------------------------------------------*/
 462 static void
 463 sys_sem_free_internal(struct sys_sem *sem)
 464 {
 465   pthread_cond_destroy(&(sem->cond));
 466   pthread_mutex_destroy(&(sem->mutex));
 467   free(sem);
 468 }
 469 /*-----------------------------------------------------------------------------------*/
 470 void
 471 sys_sem_free(struct sys_sem **sem)
 472 {
 473   if ((sem != NULL) && (*sem != SYS_SEM_NULL)) {
 474     SYS_STATS_DEC(sem.used);
 475     sys_sem_free_internal(*sem);
 476   }
 477 }
 478 #endif /* !NO_SYS */
 479 /*-----------------------------------------------------------------------------------*/
 480 u32_t
 481 sys_now(void)
 482 {
 483   struct timeval tv;
 484   u32_t sec, usec, msec;
 485   gettimeofday(&tv, NULL);
 486
 487   sec = (u32_t)(tv.tv_sec - starttime.tv_sec);
 488   usec = (u32_t)(tv.tv_usec - starttime.tv_usec);
 489   msec = sec * 1000 + usec / 1000;
 490
 491   return msec;
 492 }
 493 /*-----------------------------------------------------------------------------------*/
 494 void
 495 sys_init(void)
 496 {
 497   gettimeofday(&starttime, NULL);
 498 }
 499 /*-----------------------------------------------------------------------------------*/
 500 #if SYS_LIGHTWEIGHT_PROT
 501 /** sys_prot_t sys_arch_protect(void)
 502
 503 This optional function does a "fast" critical region protection and returns
 504 the previous protection level. This function is only called during very short
 505 critical regions. An embedded system which supports ISR-based drivers might
 506 want to implement this function by disabling interrupts. Task-based systems
 507 might want to implement this by using a mutex or disabling tasking. This
 508 function should support recursive calls from the same task or interrupt. In
 509 other words, sys_arch_protect() could be called while already protected. In
 510 that case the return value indicates that it is already protected.
 511
 512 sys_arch_protect() is only required if your port is supporting an operating
 513 system.
 514 */
 515 sys_prot_t
 516 sys_arch_protect(void)
 517 {
 518     /* Note that for the UNIX port, we are using a lightweight mutex, and our
 519      * own counter (which is locked by the mutex). The return code is not actually
 520      * used. */
 521     if (lwprot_thread != pthread_self())
 522     {
 523         /* We are locking the mutex where it has not been locked before *
 524         * or is being locked by another thread */
 525         pthread_mutex_lock(&lwprot_mutex);
 526         lwprot_thread = pthread_self();
 527         lwprot_count = 1;
 528     }
 529     else
 530         /* It is already locked by THIS thread */
 531         lwprot_count++;
 532     return 0;
 533 }
 534 /*-----------------------------------------------------------------------------------*/
 535 /** void sys_arch_unprotect(sys_prot_t pval)
 536
 537 This optional function does a "fast" set of critical region protection to the
 538 value specified by pval. See the documentation for sys_arch_protect() for
 539 more information. This function is only required if your port is supporting
 540 an operating system.
 541 */
 542 void
 543 sys_arch_unprotect(sys_prot_t pval)
 544 {
 545     LWIP_UNUSED_ARG(pval);
 546     if (lwprot_thread == pthread_self())
 547     {
 548         if (--lwprot_count == 0)
 549         {
 550             lwprot_thread = (pthread_t) 0xDEAD;
 551             pthread_mutex_unlock(&lwprot_mutex);
 552         }
 553     }
 554 }
 555 #endif /* SYS_LIGHTWEIGHT_PROT */
 556
 557 /*-----------------------------------------------------------------------------------*/
 558
 559 #ifndef MAX_JIFFY_OFFSET
 560 #define MAX_JIFFY_OFFSET ((~0U >> 1)-1)
 561 #endif
 562
 563 #ifndef HZ
 564 #define HZ 100
 565 #endif
 566
 567 u32_t
 568 sys_jiffies(void)
 569 {
 570     struct timeval tv;
 571     unsigned long sec;
 572     long usec;
 573
 574     gettimeofday(&tv,NULL);
 575     sec = tv.tv_sec - starttime.tv_sec;
 576     usec = tv.tv_usec;
 577
 578     if (sec >= (MAX_JIFFY_OFFSET / HZ))
 579       return MAX_JIFFY_OFFSET;
 580     usec += 1000000L / HZ - 1;
 581     usec /= 1000000L / HZ;
 582     return HZ * sec + usec;
 583 }
 584
 585 #if PPP_DEBUG
 586
 587 #include <stdarg.h>
 588
 589 void ppp_trace(int level, const char *format, ...)
 590 {
 591     va_list args;
 592
 593     (void)level;
 594     va_start(args, format);
 595     vprintf(format, args);
 596     va_end(args);
 597 }
 598 #endif