From 92b0749bee9fba8b5105590bf2c2f15786c55cee Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Sat, 24 May 2014 18:51:50 +0200 Subject: [PATCH] hostsim: rework to have a more precise interrupt emulation Signed-off-by: Olivier Matz --- include/aversive.h | 2 - include/aversive/irq_lock.h | 12 +- modules/base/hostsim/hostsim.c | 400 ++++++++++++------ modules/base/hostsim/hostsim.h | 43 +- .../base/scheduler/config/scheduler_config.h | 1 + modules/base/scheduler/test/main.c | 25 +- .../base/scheduler/test/scheduler_config.h | 1 + 7 files changed, 321 insertions(+), 163 deletions(-) diff --git a/include/aversive.h b/include/aversive.h index af91bf1..8de40d0 100644 --- a/include/aversive.h +++ b/include/aversive.h @@ -219,8 +219,6 @@ do { \ #else /* HOST_VERSION */ #define nop() do {} while(0) #define nothing() do {} while(0) -#define cli() do {} while(0) -#define sei() do {} while(0) #define reset() exit(1) #endif /* HOST_VERSION */ diff --git a/include/aversive/irq_lock.h b/include/aversive/irq_lock.h index 055c247..56ef5bd 100644 --- a/include/aversive/irq_lock.h +++ b/include/aversive/irq_lock.h @@ -46,12 +46,16 @@ #include /* we must use 'flags' to avoid a warning */ -#define IRQ_UNLOCK(flags) do { flags=0; /* hostsim_lock(); */ } while(0) -#define IRQ_LOCK(flags) do { flags=0; /* hostsim_unlock(); */ } while(0) +#define cli() do { hostsim_cli(); } while(0) +#define sei() do { hostsim_sei(); } while(0) +#define IRQ_LOCK(flags) do { flags = hostsim_irq_save(); } while(0) +#define IRQ_UNLOCK(flags) do { hostsim_irq_restore(flags); } while(0) #define GLOBAL_IRQ_ARE_MASKED() hostsim_islocked() #else -#define IRQ_UNLOCK(flags) do { flags=0; } while(0) -#define IRQ_LOCK(flags) do { flags=0; } while(0) +#define cli() do {} while(0) +#define sei() do {} while(0) +#define IRQ_LOCK(flags) do { (void)flags; } while(0) +#define IRQ_UNLOCK(flags) do { (void)flags; } while(0) #define GLOBAL_IRQ_ARE_MASKED() (0) #endif /* CONFIG_MODULE_HOSTSIM */ diff --git a/modules/base/hostsim/hostsim.c b/modules/base/hostsim/hostsim.c index 4c6ec3a..17a563d 100644 --- a/modules/base/hostsim/hostsim.c +++ b/modules/base/hostsim/hostsim.c @@ -1,6 +1,5 @@ /* - * Copyright Droids Corporation - * Olivier Matz + * Copyright 2014 Olivier Matz * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -21,6 +20,7 @@ */ #ifdef HOST_VERSION + /* * AVR simulator. This code is used on host (PC for instance) to * generate a signal that will call other aversive modules like @@ -28,6 +28,17 @@ * hardware interrupts in a unix application. */ +/* + * To simulate AVR interruptions, we use signals. The signals are + * always allowed, and the handler is located in hostsim code. In the + * hostsim handler, we check if the IRQ are allowed (in this case we + * call the handler) or not (in this case we ignore the IRQ). Indeed, + * the sei() / cli() functions just set a flag located in hostsim. + * + * Note that a signal handler can be preempted by itself. It means + * that + */ + #include #include @@ -40,7 +51,9 @@ #include #include #include +#include #include +#include #ifdef CONFIG_MODULE_SCHEDULER #include @@ -49,76 +62,175 @@ #include #endif -static pthread_mutex_t mut = PTHREAD_MUTEX_INITIALIZER; -static volatile int cpt = 0; +/* list of scheduled ittimers */ +LIST_HEAD(hostsim_ittimer_list, hostsim_ittimer); +struct hostsim_ittimer_list tim_list = LIST_HEAD_INITIALIZER(&tim_list); + +/* structure describing an interruption timer */ +struct hostsim_ittimer { + LIST_ENTRY(hostsim_ittimer) next; + uint32_t period_ns; + uint64_t prev_tick; + unsigned pending; + void (*handler)(void); +}; -static struct termios oldterm; -/* static */ int old_stdin, old_stdout; +/* tell if global emulated IRQ are locked (1) or allowed (0). The + * default IRQ state is locked as on a real avr */ +#define IRQ_LOCKED 0 +#define IRQ_ALLOWED 1 +static uint8_t irq_flags = IRQ_LOCKED; + +static int old_stdin, old_stdout; static int stdin_pipe[2]; static int stdout_pipe[2]; -enum msg_type { - SCHED, - UART_RCV, - UART_SND, -}; +/* ring buffer to send from pthread to signal */ +#define RG_SIZE 256 /* pow of 2 */ +#define RG_MASK (RG_SIZE-1) +volatile char out_buf[RG_SIZE]; +volatile unsigned out_tail = 0; +volatile unsigned out_head = 0; +pthread_t out_pthread; +volatile char in_buf[RG_SIZE]; +volatile unsigned in_tail = 0; +volatile unsigned in_head = 0; +pthread_t in_pthread; + +/* store the previous state of termios, restored in hostsim_exit() */ +static struct termios oldterm; -struct message { - enum msg_type type; - char c; -}; -static struct message g_msg; +void hostsim_sei(void) +{ + irq_flags = IRQ_ALLOWED; +} -#ifdef SA_SIGINFO -static void sigusr1(__attribute__((unused)) int sig, - __attribute__((unused)) siginfo_t *info, - __attribute__((unused)) void *uc) -#else -void sigusr1(__attribute__((unused)) int sig) -#endif +void hostsim_cli(void) { - struct message m; - m = g_msg; + irq_flags = IRQ_LOCKED; +} -#ifdef CONFIG_MODULE_SCHEDULER - if (m.type == SCHED) { - pthread_mutex_unlock(&mut); - scheduler_interrupt(); +/* lock emulated IRQ and return the previous state */ +uint8_t hostsim_irq_save(void) +{ + uint8_t old = irq_flags; + hostsim_cli(); + return old; +} + +/* returns a monotonic clock in nanoseconds */ +static inline uint64_t get_clk(void) +{ + struct timespec ts; + uint64_t t; + + if (clock_gettime(CLOCK_MONOTONIC_RAW, &ts) < 0) { + printf("clock_getres() failed: %s\n", strerror(errno)); + exit(1); } -#endif + t = ts.tv_sec; + t *= 1000000000; + t += ts.tv_nsec; + return t; +} -#ifdef CONFIG_MODULE_UART - if (m.type == UART_RCV) { - uart_host_rx_event(m.c); - pthread_mutex_unlock(&mut); +static void handle_interrupts(void) +{ + struct hostsim_ittimer *tim; + uint64_t clk; + uint64_t cur, next; + int c; + unsigned head; + + clk = get_clk(); + + /* do the uart events if data is available */ + if (irq_flags == IRQ_ALLOWED) { + while ((int)(out_tail - out_head) > 0) { + irq_flags = IRQ_LOCKED; + head = out_head; + c = out_buf[out_head & RG_MASK]; + if (__sync_val_compare_and_swap(&out_head, + head, head + 1) != head) + continue; + uart_host_tx_event(c); + irq_flags = IRQ_ALLOWED; + } + while ((int)(in_tail - in_head) > 0) { + irq_flags = IRQ_LOCKED; + head = in_head; + c = in_buf[in_head & RG_MASK]; + uart_host_rx_event(c); + if (__sync_val_compare_and_swap(&in_head, + head, head + 1) != head) + continue; + irq_flags = IRQ_ALLOWED; + } } - if (m.type == UART_SND) { - uart_host_tx_event(m.c); - pthread_mutex_unlock(&mut); + + /* browse all timers */ + LIST_FOREACH(tim, &tim_list, next) { + + /* Call the handler if it's time to. We use an atomic operation + * because we can be interrupted by our own signal any moment */ + do { + cur = tim->prev_tick; + next = tim->prev_tick + tim->period_ns; + + if (next > clk) + break; + + if (__sync_val_compare_and_swap(&tim->prev_tick, + cur, next) != cur) + continue; + + /* if irq are disabled, just mark the timer as pending, + * it will be executed from hostsim_irq_restore(). We + * may loose interrupts if they stay locked too long, + * like on the real hw */ + if (irq_flags == IRQ_LOCKED) + tim->pending = 1; + else { + irq_flags = IRQ_LOCKED; + tim->pending = 0; + tim->handler(); + irq_flags = IRQ_ALLOWED; + } + } while (1); + + /* also execute the irq if it was pending */ + if (irq_flags == IRQ_ALLOWED && tim->pending == 1) { + irq_flags = IRQ_LOCKED; + tim->pending = 0; + tim->handler(); + irq_flags = IRQ_ALLOWED; + } } -#endif } -static int lock_count = 0; - -void hostsim_lock(void) +/* restore the state given as parameter */ +void hostsim_irq_restore(uint8_t flags) { - if (lock_count++) + /* on transition "locked" -> "unlocked", call any pending interrupts + * before releasing IRQ lock. For other transitions, just set the + * irq_flags variable */ + + if (irq_flags == IRQ_ALLOWED || flags == IRQ_LOCKED) { + irq_flags = flags; return; - pthread_mutex_lock(&mut); -} + } -void hostsim_unlock(void) -{ - if (lock_count-- == 1) - pthread_mutex_unlock(&mut); + irq_flags = IRQ_ALLOWED; + handle_interrupts(); } -int hostsim_islocked(void) +/* return 1 if emulated IRQ are locked */ +uint8_t hostsim_irq_locked(void) { - return lock_count; + return irq_flags == IRQ_LOCKED; } +/* replacement for wait_ms() */ void host_wait_ms(int ms) { struct timeval tv, tv2, diff; @@ -135,60 +247,101 @@ void host_wait_ms(int ms) } } +/* add a new timer: loaded at init and cannot be unloaded. */ +struct hostsim_ittimer *hostsim_ittimer_add(void (*handler)(void), + unsigned period_ns) +{ + struct hostsim_ittimer *tim; + + tim = malloc(sizeof(*tim)); + if (tim == NULL) { + printf("not enough memory: cannot allocate timer\n"); + exit(1); + } + + tim->period_ns = period_ns; + tim->prev_tick = get_clk(); + tim->handler = handler; + LIST_INSERT_HEAD(&tim_list, tim, next); + return tim; +} -/* sends signal to child */ -void *parent(void *arg) +/* the signal handler, preemptable by itself */ +#ifdef SA_SIGINFO +static void sigtimer(__attribute__((unused)) int sig, + __attribute__((unused)) siginfo_t *info, + __attribute__((unused)) void *uc) +#else + void sigtimer(__attribute__((unused)) int sig) +#endif { - pthread_t thread = (pthread_t)arg; - struct timeval cur_tv, prev_tv, tv_millisec; - int n; + static int recurs = 0; - gettimeofday(&prev_tv, NULL); + if (recurs > 10) + return; + recurs++; + handle_interrupts(); + recurs--; +} - while (1) { - usleep(1000); - gettimeofday(&cur_tv, NULL); - - n = 0; - while (timercmp(&prev_tv, &cur_tv, <)) { - if (n > 5) { - /* give some time between subsequent - * signals */ - usleep(100); - n = 0; - } - pthread_mutex_lock(&mut); - g_msg.type = SCHED; - pthread_kill(thread, SIGUSR1); - - /* signal was acked */ - tv_millisec.tv_sec = 0; - tv_millisec.tv_usec = 1000; - timeradd(&prev_tv, &tv_millisec, &prev_tv); - n ++; - } +/* enable loaded ittimers */ +int hostsim_ittimer_enable(unsigned timer_resolution_us) +{ + struct sigaction sigact; + struct itimerval t; + + /* register a signal handler, which is interruptible */ + memset(&sigact, 0, sizeof(sigact)); + sigemptyset(&sigact.sa_mask); + sigact.sa_flags |= SA_NODEFER; + sigact.sa_sigaction = sigtimer; + sigaction(SIGALRM, &sigact, NULL); + + /* do not interrupt syscalls */ + if (siginterrupt (SIGALRM, 0) != 0) + return -1; + + memset(&t, 0, sizeof(t)); + t.it_value.tv_usec = timer_resolution_us; + t.it_value.tv_sec = 0; + t.it_interval.tv_usec = timer_resolution_us; + t.it_interval.tv_sec = 0; + + if (setitimer(ITIMER_REAL, &t, NULL) < 0) { + printf("setitimer failed\n"); + return -1; } - pthread_exit(NULL); - return NULL; + return 0; } void *hostsim_uart_stdin(void *arg) { - pthread_t thread = (pthread_t)arg; int n; char c; + static char prev_c; + + (void)arg; /* read on old stdin and put it in pipe */ while (1) { + if (in_tail - in_head >= (RG_SIZE - 1)) { + /* no more room, wait (should we drop ?) */ + usleep(1000); + continue; + } + n = read(old_stdin, &c, 1); if (n <= 0) break; - pthread_mutex_lock(&mut); - g_msg.type = UART_RCV; - g_msg.c = c; - pthread_kill(thread, SIGUSR1); + if (prev_c == '\x01' /* ctrl-a */ && c == 'q') + hostsim_uart_exit(); + + prev_c = c; + + in_buf[in_tail & RG_MASK] = c; + in_tail++; write(stdin_pipe[1], &c, 1); } @@ -198,20 +351,27 @@ void *hostsim_uart_stdin(void *arg) void *hostsim_uart_stdout(void *arg) { - pthread_t thread = (pthread_t)arg; int n; char c; + (void)arg; + /* read on our pipe, and forward it to the old stdout */ while (1) { + if (out_tail - out_head >= (RG_SIZE - 1)) { + /* no more room, wait (should we drop ?) */ + usleep(1000); + continue; + } + n = read(stdout_pipe[0], &c, 1); - if (n <= 0) + if (n <= 0) { + printf("read failed: %s\n", strerror(errno)); break; + } - pthread_mutex_lock(&mut); - g_msg.type = UART_SND; - g_msg.c = c; - pthread_kill(thread, SIGUSR1); + out_buf[out_tail & RG_MASK] = c; + out_tail++; write(old_stdout, &c, 1); } @@ -219,15 +379,22 @@ void *hostsim_uart_stdout(void *arg) return NULL; } +/* initialize hostsim framework for uart */ int hostsim_uart_init(void) { struct termios term; + int ret; + + printf("hostsim/uart: type ctrl-a + q to exit\n"); tcgetattr(0, &oldterm); memcpy(&term, &oldterm, sizeof(term)); term.c_lflag &= ~(ICANON | ECHO | ISIG); tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); + setbuf(stdout, NULL); + /* duplicate stdin */ old_stdin = dup(0); if (old_stdin < 0) @@ -253,67 +420,24 @@ int hostsim_uart_init(void) if (dup2(stdout_pipe[1], 1) < 0) return -1; close(stdout_pipe[1]); - setbuf(stdin, NULL); - setbuf(stdout, NULL); - - return 0; -} - -int hostsim_init(void) -{ - struct sigaction sigact; - pthread_t parent_id, child_id, child2_id, child3_id; - int ret; - - parent_id = pthread_self(); - - pthread_mutex_lock(&mut); - ret = pthread_create(&child_id, NULL, parent, (void *)parent_id); - if (ret) { - printf("pthread_create() returned %d\n", ret); - pthread_mutex_unlock(&mut); - return -1; - } -#ifdef CONFIG_MODULE_UART - if (hostsim_uart_init()) - return -1; - - ret = pthread_create(&child2_id, NULL, hostsim_uart_stdin, (void *)parent_id); + ret = pthread_create(&in_pthread, NULL, hostsim_uart_stdin, NULL); if (ret) { printf("pthread_create() returned %d\n", ret); - pthread_mutex_unlock(&mut); return -1; } - ret = pthread_create(&child3_id, NULL, hostsim_uart_stdout, (void *)parent_id); + ret = pthread_create(&out_pthread, NULL, hostsim_uart_stdout, NULL); if (ret) { printf("pthread_create() returned %d\n", ret); - pthread_mutex_unlock(&mut); return -1; } -#endif - - /* register a signal handler, which is interruptible */ - memset(&sigact, 0, sizeof(sigact)); - sigemptyset(&sigact.sa_mask); - sigact.sa_flags |= SA_NODEFER; - sigact.sa_sigaction = sigusr1; - sigaction(SIGUSR1, &sigact, NULL); - - /* */ - if (siginterrupt (SIGUSR1, 0) != 0) - return -1; - - pthread_mutex_unlock(&mut); return 0; } -int hostsim_exit(void) +int hostsim_uart_exit(void) { -#ifdef CONFIG_MODULE_UART - tcsetattr(0, TCSANOW, &oldterm); -#endif + tcsetattr(old_stdin, TCSANOW, &oldterm); exit(0); return 0; } diff --git a/modules/base/hostsim/hostsim.h b/modules/base/hostsim/hostsim.h index 176a52e..64b4eb1 100644 --- a/modules/base/hostsim/hostsim.h +++ b/modules/base/hostsim/hostsim.h @@ -20,13 +20,44 @@ * */ -/* initialize / exit hostsim framework */ -int hostsim_init(void); -int hostsim_exit(void); +#include + +/* initialize hostsim uart framework */ +int hostsim_uart_init(void); + +/* exit hostsim framework (should be called when program exits) */ +int hostsim_uart_exit(void); /* replacement for wait_ms() */ void host_wait_ms(int ms); -void hostsim_lock(void); -void hostsim_unlock(void); -int hostsim_islocked(void); +/* allow irq */ +void hostsim_sei(void); + +/* lock irq */ +void hostsim_cli(void); + +/* lock emulated IRQ and return the previous state */ +uint8_t hostsim_irq_save(void); + +/* restore the state given as parameter */ +void hostsim_irq_restore(uint8_t flags); + +/* return 1 if emulated IRQ are locked */ +uint8_t hostsim_irq_locked(void); + +/* Add a new timer: loaded at init and cannot be unloaded. The resolution is + * specified later in hostsim_ittimer_enable(). If a value lower than the + * resolution is given, the timer handler will be called several times from the + * signal handler. However it's not advised as some callbacks can be lost the + * signal occurs when irq are locked. + * + * This function must be called before hostsim_ittimer_enable(). Once + * hostsim_ittimer_enable() is called, no timer should be added. */ +struct hostsim_ittimer *hostsim_ittimer_add(void (*handler)(void), + unsigned period_ns); + +/* enable loaded ittimers + * 'timer_resolution_us' is the resolution of timer events that can be + * loaded. The advised value is 100 (us). */ +int hostsim_ittimer_enable(unsigned timer_resolution_us); diff --git a/modules/base/scheduler/config/scheduler_config.h b/modules/base/scheduler/config/scheduler_config.h index ba35429..ce95acc 100644 --- a/modules/base/scheduler/config/scheduler_config.h +++ b/modules/base/scheduler/config/scheduler_config.h @@ -56,6 +56,7 @@ define the period here */ #ifdef CONFIG_MODULE_SCHEDULER_MANUAL +/* scheduler period in us */ #define SCHEDULER_UNIT_FLOAT 1000.0 #define SCHEDULER_UNIT 1000UL diff --git a/modules/base/scheduler/test/main.c b/modules/base/scheduler/test/main.c index cc9242c..1e2ae75 100644 --- a/modules/base/scheduler/test/main.c +++ b/modules/base/scheduler/test/main.c @@ -52,29 +52,30 @@ void supp(void * nothing) int main(void) { -#ifndef HOST_VERSION +#ifdef HOST_VERSION + hostsim_uart_init(); + hostsim_ittimer_add(scheduler_interrupt, 1 * 1000 * 1000); /* 1ms period */ + hostsim_ittimer_enable(100); /* 100 us */ +#else uart_init(); fdevopen(uart0_dev_send, uart0_dev_recv); - sei(); -#else - int i; -#endif - printf("init\n"); #ifdef CONFIG_MODULE_TIMER timer_init(); #endif +#endif + scheduler_init(); printf("init2\n"); wait_ms(2000); printf("init3\n"); -#ifdef HOST_VERSION - hostsim_init(); -#endif + sei(); - event_id = scheduler_add_periodical_event_priority(f1, NULL, 500000l/SCHEDULER_UNIT, 200); - scheduler_add_periodical_event_priority(f2, NULL, 500000l/SCHEDULER_UNIT, 100); + event_id = scheduler_add_periodical_event_priority(f1, NULL, + 500000l/SCHEDULER_UNIT, 200); + scheduler_add_periodical_event_priority(f2, NULL, + 500000l/SCHEDULER_UNIT, 100); scheduler_add_periodical_event(f3, NULL, 1000000l/SCHEDULER_UNIT); scheduler_add_single_event(supp, NULL, 5000000l/SCHEDULER_UNIT); @@ -83,5 +84,3 @@ int main(void) return 0; } - - diff --git a/modules/base/scheduler/test/scheduler_config.h b/modules/base/scheduler/test/scheduler_config.h index 8042648..0a39734 100644 --- a/modules/base/scheduler/test/scheduler_config.h +++ b/modules/base/scheduler/test/scheduler_config.h @@ -56,6 +56,7 @@ define the period here */ #ifdef CONFIG_MODULE_SCHEDULER_MANUAL +/* scheduler period in us */ #define SCHEDULER_UNIT_FLOAT 1000.0 #define SCHEDULER_UNIT 1000UL -- 2.39.5