modules/base/hostsim/hostsim.c

   1 /*
   2  *  Copyright 2014 Olivier Matz <zer0@droids-corp.org>
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: main.c,v 1.10 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  */
  21
  22 #ifdef HOST_VERSION
  23
  24 /*
  25  * AVR simulator. This code is used on host (PC for instance) to
  26  * generate a signal that will call other aversive modules like
  27  * scheduler or uart. The goal is to simulate the behaviour of
  28  * hardware interrupts in a unix application.
  29  */
  30
  31 /*
  32  * To simulate AVR interruptions, we use signals. The signals are
  33  * always allowed, and the handler is located in hostsim code.  In the
  34  * hostsim handler, we check if the IRQ are allowed (in this case we
  35  * call the handler) or not (in this case we ignore the IRQ). Indeed,
  36  * the sei() / cli() functions just set a flag located in hostsim.
  37  *
  38  * Note that a signal handler can be preempted by itself. It means
  39  * that
  40  */
  41
  42 #include <aversive.h>
  43
  44 #include <stdio.h>
  45 #include <stdlib.h>
  46 #include <signal.h>
  47 #include <unistd.h>
  48 #include <fcntl.h>
  49 #include <string.h>
  50 #include <pthread.h>
  51 #include <sys/time.h>
  52 #include <sys/types.h>
  53 #include <sys/wait.h>
  54 #include <sys/queue.h>
  55 #include <termios.h>
  56 #include <signal.h>
  57
  58 #ifdef CONFIG_MODULE_SCHEDULER
  59 #include <scheduler.h>
  60 #endif
  61 #ifdef CONFIG_MODULE_UART
  62 #include <uart_host.h>
  63 #endif
  64
  65 /* list of scheduled ittimers */
  66 LIST_HEAD(hostsim_ittimer_list, hostsim_ittimer);
  67 struct hostsim_ittimer_list tim_list = LIST_HEAD_INITIALIZER(&tim_list);
  68
  69 /* structure describing an interruption timer */
  70 struct hostsim_ittimer {
  71         LIST_ENTRY(hostsim_ittimer) next;
  72         uint32_t period_ns;
  73         uint64_t prev_tick;
  74         unsigned pending;
  75         void (*handler)(void);
  76 };
  77
  78 /* tell if global emulated IRQ are locked (1) or allowed (0). The
  79  * default IRQ state is locked as on a real avr */
  80 #define IRQ_LOCKED  0
  81 #define IRQ_ALLOWED 1
  82 static uint8_t irq_flags = IRQ_LOCKED;
  83
  84 static int old_stdin, old_stdout;
  85 static int stdin_pipe[2];
  86 static int stdout_pipe[2];
  87
  88 /* ring buffer to send from pthread to signal */
  89 #define RG_SIZE 256 /* pow of 2 */
  90 #define RG_MASK (RG_SIZE-1)
  91 volatile char out_buf[RG_SIZE];
  92 volatile unsigned out_tail = 0;
  93 volatile unsigned out_head = 0;
  94 pthread_t out_pthread;
  95 volatile char in_buf[RG_SIZE];
  96 volatile unsigned in_tail = 0;
  97 volatile unsigned in_head = 0;
  98 pthread_t in_pthread;
  99
 100 /* store the previous state of termios, restored in hostsim_exit() */
 101 static struct termios oldterm;
 102
 103 void hostsim_sei(void)
 104 {
 105         irq_flags = IRQ_ALLOWED;
 106 }
 107
 108 void hostsim_cli(void)
 109 {
 110         irq_flags = IRQ_LOCKED;
 111 }
 112
 113 /* lock emulated IRQ and return the previous state */
 114 uint8_t hostsim_irq_save(void)
 115 {
 116         uint8_t old = irq_flags;
 117         hostsim_cli();
 118         return old;
 119 }
 120
 121 /* returns a monotonic clock in nanoseconds */
 122 static inline uint64_t get_clk(void)
 123 {
 124         struct timespec ts;
 125         uint64_t t;
 126
 127         if (clock_gettime(CLOCK_MONOTONIC_RAW, &ts) < 0) {
 128                 printf("clock_getres() failed: %s\n", strerror(errno));
 129                 exit(1);
 130         }
 131         t = ts.tv_sec;
 132         t *= 1000000000;
 133         t += ts.tv_nsec;
 134         return t;
 135 }
 136
 137 static void handle_interrupts(void)
 138 {
 139         struct hostsim_ittimer *tim;
 140         uint64_t clk;
 141         uint64_t cur, next;
 142         int c;
 143         unsigned head;
 144
 145         clk = get_clk();
 146
 147         /* do the uart events if data is available */
 148         if (irq_flags == IRQ_ALLOWED) {
 149                 while ((int)(out_tail - out_head) > 0) {
 150                         irq_flags = IRQ_LOCKED;
 151                         head = out_head;
 152                         c = out_buf[out_head & RG_MASK];
 153                         if (__sync_val_compare_and_swap(&out_head,
 154                                         head, head + 1) != head)
 155                                 continue;
 156                         uart_host_tx_event(c);
 157                         irq_flags = IRQ_ALLOWED;
 158                 }
 159                 while ((int)(in_tail - in_head) > 0) {
 160                         irq_flags = IRQ_LOCKED;
 161                         head = in_head;
 162                         c = in_buf[in_head & RG_MASK];
 163                         uart_host_rx_event(c);
 164                         if (__sync_val_compare_and_swap(&in_head,
 165                                         head, head + 1) != head)
 166                                 continue;
 167                         irq_flags = IRQ_ALLOWED;
 168                 }
 169         }
 170
 171         /* browse all timers */
 172         LIST_FOREACH(tim, &tim_list, next) {
 173
 174                 /* Call the handler if it's time to. We use an atomic operation
 175                  * because we can be interrupted by our own signal any moment */
 176                 do {
 177                         cur = tim->prev_tick;
 178                         next = tim->prev_tick + tim->period_ns;
 179
 180                         if (next > clk)
 181                                 break;
 182
 183                         if (__sync_val_compare_and_swap(&tim->prev_tick,
 184                                         cur, next) != cur)
 185                                 continue;
 186
 187                         /* if irq are disabled, just mark the timer as pending,
 188                          * it will be executed from hostsim_irq_restore(). We
 189                          * may loose interrupts if they stay locked too long,
 190                          * like on the real hw */
 191                         if (irq_flags == IRQ_LOCKED)
 192                                 tim->pending = 1;
 193                         else {
 194                                 irq_flags = IRQ_LOCKED;
 195                                 tim->pending = 0;
 196                                 tim->handler();
 197                                 irq_flags = IRQ_ALLOWED;
 198                         }
 199                 } while (1);
 200
 201                 /* also execute the irq if it was pending */
 202                 if (irq_flags == IRQ_ALLOWED && tim->pending == 1) {
 203                         irq_flags = IRQ_LOCKED;
 204                         tim->pending = 0;
 205                         tim->handler();
 206                         irq_flags = IRQ_ALLOWED;
 207                 }
 208         }
 209 }
 210
 211 /* restore the state given as parameter  */
 212 void hostsim_irq_restore(uint8_t flags)
 213 {
 214         /* on transition "locked" -> "unlocked", call any pending interrupts
 215          * before releasing IRQ lock. For other transitions, just set the
 216          * irq_flags variable */
 217
 218         if (irq_flags == IRQ_ALLOWED || flags == IRQ_LOCKED) {
 219                 irq_flags = flags;
 220                 return;
 221         }
 222
 223         irq_flags = IRQ_ALLOWED;
 224         handle_interrupts();
 225 }
 226
 227 /* return 1 if emulated IRQ are locked */
 228 uint8_t hostsim_irq_locked(void)
 229 {
 230         return irq_flags == IRQ_LOCKED;
 231 }
 232
 233 /* replacement for wait_ms() */
 234 void host_wait_ms(int ms)
 235 {
 236         struct timeval tv, tv2, diff;
 237
 238         gettimeofday(&tv, NULL);
 239         diff.tv_sec = (1000 * ms) / 1000000;
 240         diff.tv_usec = (1000 * ms) % 1000000;
 241         timeradd(&tv, &diff, &tv);
 242         gettimeofday(&tv2, NULL);
 243
 244         while (timercmp(&tv2, &tv, <)) {
 245                 usleep(1000);
 246                 gettimeofday(&tv2, NULL);
 247         }
 248 }
 249
 250 /* add a new timer: loaded at init and cannot be unloaded. */
 251 struct hostsim_ittimer *hostsim_ittimer_add(void (*handler)(void),
 252         unsigned period_ns)
 253 {
 254         struct hostsim_ittimer *tim;
 255
 256         tim = malloc(sizeof(*tim));
 257         if (tim == NULL) {
 258                 printf("not enough memory: cannot allocate timer\n");
 259                 exit(1);
 260         }
 261
 262         tim->period_ns = period_ns;
 263         tim->prev_tick = get_clk();
 264         tim->handler = handler;
 265         LIST_INSERT_HEAD(&tim_list, tim, next);
 266         return tim;
 267 }
 268
 269 /* the signal handler, preemptable by itself */
 270 #ifdef SA_SIGINFO
 271 static void sigtimer(__attribute__((unused)) int sig,
 272         __attribute__((unused)) siginfo_t *info,
 273         __attribute__((unused)) void *uc)
 274 #else
 275         void sigtimer(__attribute__((unused)) int sig)
 276 #endif
 277 {
 278         static int recurs = 0;
 279
 280         if (recurs > 10)
 281                 return;
 282         recurs++;
 283         handle_interrupts();
 284         recurs--;
 285 }
 286
 287 /* enable loaded ittimers */
 288 int hostsim_ittimer_enable(unsigned timer_resolution_us)
 289 {
 290         struct sigaction sigact;
 291         struct itimerval t;
 292
 293         /* register a signal handler, which is interruptible */
 294         memset(&sigact, 0, sizeof(sigact));
 295         sigemptyset(&sigact.sa_mask);
 296         sigact.sa_flags |= SA_NODEFER;
 297         sigact.sa_sigaction = sigtimer;
 298         sigaction(SIGALRM, &sigact, NULL);
 299
 300         /* do not interrupt syscalls */
 301         if (siginterrupt (SIGALRM, 0) != 0)
 302                 return -1;
 303
 304         memset(&t, 0, sizeof(t));
 305         t.it_value.tv_usec = timer_resolution_us;
 306         t.it_value.tv_sec = 0;
 307         t.it_interval.tv_usec = timer_resolution_us;
 308         t.it_interval.tv_sec = 0;
 309
 310         if (setitimer(ITIMER_REAL, &t, NULL) < 0) {
 311                 printf("setitimer failed\n");
 312                 return -1;
 313         }
 314
 315         return 0;
 316 }
 317
 318 void *hostsim_uart_stdin(void *arg)
 319 {
 320         int n;
 321         char c;
 322         static char prev_c;
 323
 324         (void)arg;
 325
 326         /* read on old stdin and put it in pipe */
 327         while (1) {
 328                 if (in_tail - in_head >= (RG_SIZE - 1)) {
 329                         /* no more room, wait (should we drop ?) */
 330                         usleep(1000);
 331                         continue;
 332                 }
 333
 334                 n = read(old_stdin, &c, 1);
 335                 if (n <= 0)
 336                         break;
 337
 338                 if (prev_c == '\x01' /* ctrl-a */ && c == 'q')
 339                         hostsim_uart_exit();
 340
 341                 prev_c = c;
 342
 343                 in_buf[in_tail & RG_MASK] = c;
 344                 in_tail++;
 345
 346                 write(stdin_pipe[1], &c, 1);
 347         }
 348         pthread_exit(NULL);
 349         return NULL;
 350 }
 351
 352 void *hostsim_uart_stdout(void *arg)
 353 {
 354         int n;
 355         char c;
 356
 357         (void)arg;
 358
 359         /* read on our pipe, and forward it to the old stdout */
 360         while (1) {
 361                 if (out_tail - out_head >= (RG_SIZE - 1)) {
 362                         /* no more room, wait (should we drop ?) */
 363                         usleep(1000);
 364                         continue;
 365                 }
 366
 367                 n = read(stdout_pipe[0], &c, 1);
 368                 if (n <= 0) {
 369                         printf("read failed: %s\n", strerror(errno));
 370                         break;
 371                 }
 372
 373                 out_buf[out_tail & RG_MASK] = c;
 374                 out_tail++;
 375
 376                 write(old_stdout, &c, 1);
 377         }
 378         pthread_exit(NULL);
 379         return NULL;
 380 }
 381
 382 /* initialize hostsim framework for uart */
 383 int hostsim_uart_init(void)
 384 {
 385         struct termios term;
 386         int ret;
 387
 388         printf("hostsim/uart: type ctrl-a + q to exit\n");
 389
 390         tcgetattr(0, &oldterm);
 391         memcpy(&term, &oldterm, sizeof(term));
 392         term.c_lflag &= ~(ICANON | ECHO | ISIG);
 393         tcsetattr(0, TCSANOW, &term);
 394
 395         setbuf(stdin, NULL);
 396         setbuf(stdout, NULL);
 397
 398         /* duplicate stdin */
 399         old_stdin = dup(0);
 400         if (old_stdin < 0)
 401                 return 1;
 402
 403         /* duplicate stdout */
 404         old_stdout = dup(1);
 405         if (old_stdout < 0)
 406                 return -1;
 407
 408         /* create 2 pipes */
 409         if (pipe(stdin_pipe) < 0)
 410                 return -1;
 411         if (pipe(stdout_pipe) < 0)
 412                 return -1;
 413
 414         /* replace file desc 0 (stdin) by our pipe */
 415         if (dup2(stdin_pipe[0], 0) < 0)
 416                 return -1;
 417         close(stdin_pipe[0]);
 418
 419         /* replace file desc 1 (stdout) by our pipe */
 420         if (dup2(stdout_pipe[1], 1) < 0)
 421                 return -1;
 422         close(stdout_pipe[1]);
 423
 424         ret = pthread_create(&in_pthread, NULL, hostsim_uart_stdin, NULL);
 425         if (ret) {
 426                 printf("pthread_create() returned %d\n", ret);
 427                 return -1;
 428         }
 429         ret = pthread_create(&out_pthread, NULL, hostsim_uart_stdout, NULL);
 430         if (ret) {
 431                 printf("pthread_create() returned %d\n", ret);
 432                 return -1;
 433         }
 434
 435         return 0;
 436 }
 437
 438 int hostsim_uart_exit(void)
 439 {
 440         tcsetattr(old_stdin, TCSANOW, &oldterm);
 441         exit(0);
 442         return 0;
 443 }
 444 #endif /* HOST_VERSION */