bertos/net/lwip/src/arch/sys_arch.c

   1 #include "cfg/cfg_lwip.h"
   2
   3 #define LOG_LEVEL  3 //INFO
   4 #define LOG_FORMAT 0 //TERSE
   5 #include <cfg/log.h>
   6
   7 #include <drv/timer.h>
   8
   9 #include <cpu/power.h>
  10 #include <cpu/types.h>
  11
  12 #include <arch/sys_arch.h>
  13 #include <lwip/sys.h>
  14
  15 #include <kern/signal.h>
  16 #include <kern/msg.h>
  17 #include <kern/proc.h>
  18 #include <kern/proc_p.h>
  19
  20 #include <struct/heap.h>
  21
  22 #include <mware/event.h>
  23
  24 /****************************************************************************/
  25
  26 /*
  27  * Generic mutex (binary semaphore) implementation
  28  *
  29  * TODO: move this to a different place (i.e., bertos/kern/sem.c).
  30  */
  31 INLINE void mutex_verify(struct Mutex *s)
  32 {
  33         (void)s;
  34         ASSERT(s);
  35         LIST_ASSERT_VALID(&s->wait_queue);
  36         ASSERT((s->count == MUTEX_LOCKED) || (s->count == MUTEX_UNLOCKED));
  37 }
  38
  39 bool mutex_attempt(struct Mutex *s)
  40 {
  41         return cpu_atomic_xchg(&s->count, MUTEX_LOCKED) == MUTEX_UNLOCKED;
  42 }
  43
  44 static NOINLINE void mutex_slowpath_obtain(struct Mutex *s)
  45 {
  46         PROC_ATOMIC(
  47                 mutex_verify(s);
  48                 ADDTAIL(&s->wait_queue, (Node *)current_process)
  49         );
  50         proc_switch();
  51 }
  52
  53 void mutex_obtain(struct Mutex *s)
  54 {
  55         if (UNLIKELY(cpu_atomic_xchg(&s->count, MUTEX_LOCKED)) !=
  56                                 MUTEX_UNLOCKED)
  57                 mutex_slowpath_obtain(s);
  58 }
  59
  60 void mutex_release(struct Mutex *s)
  61 {
  62         Process *proc = NULL;
  63
  64         PROC_ATOMIC(
  65                 mutex_verify(s);
  66                 proc = (Process *)list_remHead(&s->wait_queue);
  67                 if (!proc)
  68                         s->count = 1;
  69         );
  70         if (proc)
  71                 ATOMIC(proc_wakeup(proc));
  72 }
  73
  74 void mutex_init(struct Mutex *s)
  75 {
  76         LIST_INIT(&s->wait_queue);
  77         s->count = 1;
  78 }
  79
  80 /****************************************************************************/
  81
  82 typedef struct SemNode
  83 {
  84         Node node;
  85         Mutex sem;
  86 } SemNode;
  87
  88 #define MAX_SEM_CNT 16
  89
  90 static struct SemNode sem_pool[MAX_SEM_CNT];
  91 static List free_sem;
  92
  93 /**
  94  * Creates and returns a new semaphore.
  95  *
  96  * \param count Specifies the initial state of the semaphore.
  97  * \return The semaphore or SYS_SEM_NULL on error.
  98  */
  99 sys_sem_t sys_sem_new(u8_t count)
 100 {
 101         SemNode *sem;
 102
 103         PROC_ATOMIC(sem = (SemNode *)list_remHead(&free_sem));
 104         if (UNLIKELY(!sem))
 105         {
 106                 LOG_ERR("Out of semaphores!\n");
 107                 return SYS_SEM_NULL;
 108         }
 109
 110         mutex_init(&sem->sem);
 111         // must obtain semaphore depending on the parameter
 112         // NOTE: count == 1 means that the semaphore is unlocked
 113         if (count <= 0)
 114                 mutex_obtain(&sem->sem);
 115         return (sys_sem_t)&sem->sem;
 116 }
 117
 118 /**
 119  * Frees a semaphore created by sys_sem_new.
 120  *
 121  * \param semaphore Mutex to be freed
 122  */
 123 void sys_sem_free(sys_sem_t semaphore)
 124 {
 125         SemNode *sem = containerof(semaphore, SemNode, sem);
 126         PROC_ATOMIC(ADDHEAD(&free_sem, &sem->node));
 127 }
 128
 129 /**
 130  * Signals (or releases) a semaphore.
 131  */
 132 void sys_sem_signal(sys_sem_t sem)
 133 {
 134         mutex_release(sem);
 135 }
 136
 137 /**
 138  * Blocks the thread while waiting for the semaphore to be signaled.
 139  *
 140  * The timeout parameter specifies how many milliseconds the function should block
 141  * before returning; if the function times out, it should return SYS_ARCH_TIMEOUT.
 142  * If timeout=0, then the function should block indefinitely.
 143  * If the function acquires the semaphore, it should return how many milliseconds
 144  * expired while waiting for the semaphore.
 145  * The function may return 0 if the semaphore was immediately available.
 146  */
 147 u32_t sys_arch_sem_wait(sys_sem_t sem, u32_t timeout)
 148 {
 149         ticks_t end, start = timer_clock();
 150
 151         if (timeout == 0)
 152         {
 153                 mutex_obtain(sem);
 154                 return ticks_to_ms(timer_clock() - start);
 155         }
 156
 157         do
 158         {
 159                 cpu_relax();
 160                 end = timer_clock();
 161         } while ((end - start < ms_to_ticks(timeout) && !mutex_attempt(sem)));
 162
 163         return (end - start > ms_to_ticks(timeout)) ?
 164                         SYS_ARCH_TIMEOUT : (u32_t)ticks_to_ms(end - start);
 165 }
 166
 167 /* Mbox functions */
 168
 169 typedef struct IpPort
 170 {
 171         Node node;
 172         MsgPort port;
 173 } IpPort;
 174
 175 #define MAX_PORT_CNT 16
 176 static struct IpPort port_pool[MAX_PORT_CNT];
 177 static List free_port;
 178
 179 typedef struct IpMsg
 180 {
 181         Msg msg;
 182         void *data;
 183 } IpMsg;
 184
 185 #define MAX_MSG_CNT 32
 186 static struct IpMsg msg_pool[MAX_MSG_CNT];
 187 static List free_msg;
 188
 189 // TODO: allocate memory for 'size' messages
 190 sys_mbox_t sys_mbox_new(UNUSED_ARG(int, size))
 191 {
 192         IpPort *port;
 193
 194         PROC_ATOMIC(port = (IpPort *)list_remHead(&free_port));
 195         if (UNLIKELY(!port))
 196         {
 197                 LOG_ERR("Out of message ports!\n");
 198                 return SYS_MBOX_NULL;
 199         }
 200         msg_initPort(&port->port, event_createGeneric());
 201         port->port.event.Ev.Sig.sig_proc = NULL;
 202
 203         return (sys_mbox_t)(&port->port);
 204 }
 205
 206 void sys_mbox_free(sys_mbox_t mbox)
 207 {
 208         IpPort *port = containerof(mbox, IpPort, port);
 209         PROC_ATOMIC(ADDHEAD(&free_port, &port->node));
 210 }
 211
 212 void sys_mbox_post(sys_mbox_t mbox, void *data)
 213 {
 214         sys_mbox_trypost(mbox, data);
 215 }
 216
 217 /*
 218  * Try to post the "msg" to the mailbox. Returns ERR_MEM if this one
 219  * is full, else, ERR_OK if the "msg" is posted.
 220  */
 221 err_t sys_mbox_trypost(sys_mbox_t mbox, void *data)
 222 {
 223         IpMsg *msg;
 224
 225         PROC_ATOMIC(msg = (IpMsg *)list_remHead(&free_msg));
 226         if (UNLIKELY(!msg))
 227         {
 228                 LOG_ERR("out of messages!\n");
 229                 return ERR_MEM;
 230         }
 231         msg->data = data;
 232
 233         msg_lockPort(mbox);
 234         ADDTAIL(&mbox->queue, &msg->msg.link);
 235         msg_unlockPort(mbox);
 236
 237         if (mbox->event.Ev.Sig.sig_proc)
 238                 event_do(&mbox->event);
 239
 240         return ERR_OK;
 241 }
 242
 243 u32_t sys_arch_mbox_fetch(sys_mbox_t mbox, void **data, u32_t timeout)
 244 {
 245         /* Blocks the thread until a message arrives in the mailbox, but does
 246         not block the thread longer than "timeout" milliseconds (similar to
 247         the sys_arch_sem_wait() function). If "timeout" is 0, the thread should
 248         be blocked until a message arrives. The "msg" argument is a result
 249         parameter that is set by the function (i.e., by doing "*msg =
 250         ptr"). The "msg" parameter maybe NULL to indicate that the message
 251         should be dropped.
 252
 253         The return values are the same as for the sys_arch_sem_wait() function:
 254         Number of milliseconds spent waiting or SYS_ARCH_TIMEOUT if there was a
 255         timeout.
 256
 257         Note that a function with a similar name, sys_mbox_fetch(), is
 258         implemented by lwIP.
 259         */
 260
 261         Msg *msg;
 262         ticks_t start = timer_clock();
 263
 264         while (1)
 265         {
 266                 /* Fast path */
 267                 msg = msg_get(mbox);
 268                 if (LIKELY(msg))
 269                         break;
 270
 271                 mbox->event.Ev.Sig.sig_proc = proc_current();
 272                 /* Slow path */
 273                 if (!timeout)
 274                         event_wait(&mbox->event);
 275                 else
 276                 {
 277                         if (!event_waitTimeout(&mbox->event,
 278                                         ms_to_ticks(timeout)))
 279                         {
 280                                 mbox->event.Ev.Sig.sig_proc = NULL;
 281                                 return SYS_ARCH_TIMEOUT;
 282                         }
 283                 }
 284         }
 285         mbox->event.Ev.Sig.sig_proc = NULL;
 286         if (data)
 287                 *data = containerof(msg, IpMsg, msg)->data;
 288
 289         PROC_ATOMIC(ADDHEAD(&free_msg, &msg->link));
 290
 291         return ticks_to_ms(timer_clock() - start);
 292 }
 293
 294 u32_t sys_arch_mbox_tryfetch(sys_mbox_t mbox, void **data)
 295 {
 296         /* This is similar to sys_arch_mbox_fetch, however if a message is not
 297         present in the mailbox, it immediately returns with the code
 298         SYS_MBOX_EMPTY. On success 0 is returned.
 299
 300         To allow for efficient implementations, this can be defined as a
 301         function-like macro in sys_arch.h instead of a normal function. For
 302         example, a naive implementation could be:
 303         #define sys_arch_mbox_tryfetch(mbox,msg) \
 304           sys_arch_mbox_fetch(mbox,msg,1)
 305         although this would introduce unnecessary delays.
 306         */
 307
 308         Msg *msg;
 309
 310         msg = msg_get(mbox);
 311         if (UNLIKELY(!msg))
 312                 return SYS_MBOX_EMPTY;
 313         if (data)
 314                 *data = containerof(msg, IpMsg, msg)->data;
 315         PROC_ATOMIC(ADDHEAD(&free_msg, &msg->link));
 316
 317         return 0;
 318 }
 319
 320 typedef struct ThreadNode
 321 {
 322         Node node;
 323         struct Process *pid;
 324         void (*entry)(void *);
 325         void *arg;
 326         struct sys_timeouts timeout;
 327 } ThreadNode;
 328
 329 #define MAX_THREAD_CNT 8
 330
 331 static ThreadNode thread_pool[MAX_THREAD_CNT];
 332 static List free_thread;
 333 static List used_thread;
 334
 335 static struct sys_timeouts lwip_system_timeouts; // Default timeouts list for lwIP
 336
 337 struct sys_timeouts *sys_arch_timeouts(void)
 338 {
 339         ThreadNode *thread_node;
 340         struct Process *curr_pid = proc_current();
 341
 342         FOREACH_NODE(thread_node, &used_thread)
 343         {
 344                 if (thread_node->pid == curr_pid)
 345                         return &(thread_node->timeout);
 346         }
 347
 348         return &lwip_system_timeouts;
 349 }
 350
 351 static void thread_trampoline(void)
 352 {
 353         ThreadNode *thread_node = (ThreadNode *)proc_currentUserData();
 354
 355         thread_node->entry(thread_node->arg);
 356 }
 357
 358 #if !CONFIG_KERN_HEAP
 359 /*
 360  * NOTE: threads are never destroyed, consequently these stacks are never
 361  * deallocated. So, the stack allocator can be implemented as a simple index
 362  * that is atomically incremented at each allocation.
 363  */
 364 static cpu_stack_t thread_stack[MAX_THREAD_CNT]
 365                         [DEFAULT_THREAD_STACKSIZE / sizeof(cpu_stack_t)]
 366                                 ALIGNED(sizeof(cpu_stack_t));
 367 static int last_stack;
 368 #endif
 369
 370 sys_thread_t sys_thread_new(const char *name, void (* thread)(void *arg),
 371                                 void *arg, int stacksize, int prio)
 372 {
 373         ThreadNode *thread_node;
 374         cpu_stack_t *stackbase;
 375
 376         proc_forbid();
 377         thread_node = (ThreadNode *)list_remHead(&free_thread);
 378         if (UNLIKELY(!thread_node))
 379         {
 380                 proc_permit();
 381                 LOG_ERR("Out of threads!\n");
 382                 return NULL;
 383         }
 384         ADDHEAD(&used_thread, &thread_node->node);
 385         proc_permit();
 386
 387         thread_node->entry = thread;
 388         thread_node->arg = arg;
 389
 390         #if !CONFIG_KERN_HEAP
 391                 ASSERT(stacksize <= DEFAULT_THREAD_STACKSIZE);
 392                 PROC_ATOMIC(stackbase = thread_stack[last_stack++]);
 393         #else
 394                 stackbase = NULL;
 395         #endif
 396         thread_node->pid = proc_new_with_name(name, thread_trampoline,
 397                                 (void *)thread_node, stacksize, stackbase);
 398         if (thread_node->pid == NULL)
 399                 return NULL;
 400
 401         #if CONFIG_KERN_PRI
 402                 proc_setPri(thread_node->pid, prio);
 403         #else
 404                 /* Avoid warnings when priorities are disabled */
 405                 (void) prio;
 406         #endif
 407
 408         return thread_node->pid;
 409 }
 410
 411 void sys_init(void)
 412 {
 413         LIST_INIT(&free_sem);
 414         LIST_INIT(&free_port);
 415         LIST_INIT(&free_msg);
 416         LIST_INIT(&free_thread);
 417         LIST_INIT(&used_thread);
 418
 419         for (int i = 0; i < MAX_SEM_CNT; ++i)
 420                 ADDHEAD(&free_sem, &sem_pool[i].node);
 421
 422         for (int i = 0; i < MAX_PORT_CNT; ++i)
 423                 ADDHEAD(&free_port, &port_pool[i].node);
 424
 425         for (int i = 0; i < MAX_MSG_CNT; ++i)
 426                 ADDHEAD(&free_msg, &msg_pool[i].msg.link);
 427
 428         for (int i = 0; i < MAX_THREAD_CNT; ++i)
 429                 ADDHEAD(&free_thread, &thread_pool[i].node);
 430 }