diff options
| -rw-r--r-- | c/src/lib/libbsp/shared/src/irq-server.c | 308 |
1 files changed, 173 insertions, 135 deletions
diff --git a/c/src/lib/libbsp/shared/src/irq-server.c b/c/src/lib/libbsp/shared/src/irq-server.c index 6d0d0d33af..4f127af7ba 100644 --- a/c/src/lib/libbsp/shared/src/irq-server.c +++ b/c/src/lib/libbsp/shared/src/irq-server.c @@ -82,93 +82,6 @@ static void bsp_interrupt_server_trigger(void *arg) } typedef struct { - bsp_interrupt_server_action *action; - bsp_interrupt_server_action **link; - rtems_id task; -} bsp_interrupt_server_helper_data; - -static void bsp_interrupt_server_helper(void *arg) -{ - bsp_interrupt_server_helper_data *hd = arg; - - *hd->link = hd->action; - rtems_event_transient_send(hd->task); -} - -static void bsp_interrupt_server_call_helper( - bsp_interrupt_server_action *action, - bsp_interrupt_server_action **link -) -{ - bsp_interrupt_server_helper_data hd = { - .action = action, - .link = link, - .task = rtems_task_self() - }; - bsp_interrupt_server_action a = { - .handler = bsp_interrupt_server_helper, - .arg = &hd - }; - bsp_interrupt_server_entry e = { - .vector = BSP_INTERRUPT_VECTOR_MAX + 1, - .actions = &a - }; - - bsp_interrupt_server_trigger(&e); - rtems_event_transient_receive(RTEMS_WAIT, RTEMS_NO_TIMEOUT); -} - -static bsp_interrupt_server_entry *bsp_interrupt_server_get_entry(void) -{ - rtems_interrupt_lock_context lock_context; - bsp_interrupt_server_entry *e; - rtems_chain_control *chain; - - rtems_interrupt_lock_acquire(&bsp_interrupt_server_lock, &lock_context); - chain = &bsp_interrupt_server_chain; - - if (!rtems_chain_is_empty(chain)) { - e = (bsp_interrupt_server_entry *) - rtems_chain_get_first_unprotected(chain); - rtems_chain_set_off_chain(&e->node); - } else { - e = NULL; - } - - rtems_interrupt_lock_release(&bsp_interrupt_server_lock, &lock_context); - - return e; -} - -static void bsp_interrupt_server_task(rtems_task_argument arg) -{ - while (true) { - rtems_event_set events; - bsp_interrupt_server_entry *e; - - rtems_event_system_receive( - RTEMS_EVENT_SYSTEM_SERVER, - RTEMS_EVENT_ALL | RTEMS_WAIT, - RTEMS_NO_TIMEOUT, - &events - ); - - while ((e = bsp_interrupt_server_get_entry()) != NULL) { - bsp_interrupt_server_action *action = e->actions; - rtems_vector_number vector = e->vector; - - do { - bsp_interrupt_server_action *current = action; - action = action->next; - (*current->handler)(current->arg); - } while (action != NULL); - - bsp_interrupt_vector_enable(vector); - } - } -} - -typedef struct { bsp_interrupt_server_entry *entry; rtems_option *options; } bsp_interrupt_server_iterate_entry; @@ -208,50 +121,46 @@ static bsp_interrupt_server_entry *bsp_interrupt_server_query_entry( return ie.entry; } -rtems_status_code rtems_interrupt_server_handler_install( - rtems_id server, - rtems_vector_number vector, - const char *info, - rtems_option options, - rtems_interrupt_handler handler, - void *arg -) +typedef struct { + rtems_vector_number vector; + rtems_option options; + rtems_interrupt_handler handler; + void *arg; + rtems_id task; + rtems_status_code sc; +} bsp_interrupt_server_helper_data; + +static void bsp_interrupt_server_install_helper(void *arg) { + bsp_interrupt_server_helper_data *hd = arg; rtems_status_code sc; bsp_interrupt_server_entry *e; bsp_interrupt_server_action *a; rtems_option trigger_options; - sc = bsp_interrupt_server_is_initialized(); - if (sc != RTEMS_SUCCESSFUL) { - return sc; - } - - if (server != RTEMS_ID_NONE) { - return RTEMS_NOT_IMPLEMENTED; - } - a = calloc(1, sizeof(*a)); if (a == NULL) { - return RTEMS_NO_MEMORY; + hd->sc = RTEMS_NO_MEMORY; + rtems_event_transient_send(hd->task); + return; } - a->handler = handler; - a->arg = arg; + a->handler = hd->handler; + a->arg = hd->arg; bsp_interrupt_lock(); - e = bsp_interrupt_server_query_entry(vector, &trigger_options); + e = bsp_interrupt_server_query_entry(hd->vector, &trigger_options); if (e == NULL) { e = calloc(1, sizeof(*e)); if (e != NULL) { - e->vector = vector; + e->vector = hd->vector; e->actions = a; sc = rtems_interrupt_handler_install( - vector, + hd->vector, "IRQS", - options & RTEMS_INTERRUPT_UNIQUE, + hd->options & RTEMS_INTERRUPT_UNIQUE, bsp_interrupt_server_trigger, e ); @@ -262,7 +171,7 @@ rtems_status_code rtems_interrupt_server_handler_install( sc = RTEMS_NO_MEMORY; } } else if ( - RTEMS_INTERRUPT_IS_UNIQUE(options) + RTEMS_INTERRUPT_IS_UNIQUE(hd->options) || RTEMS_INTERRUPT_IS_UNIQUE(trigger_options) ) { sc = RTEMS_RESOURCE_IN_USE; @@ -270,8 +179,10 @@ rtems_status_code rtems_interrupt_server_handler_install( bsp_interrupt_server_action **link = &e->actions; bsp_interrupt_server_action *c; + sc = RTEMS_SUCCESSFUL; + while ((c = *link) != NULL) { - if (c->handler == handler && c->arg == arg) { + if (c->handler == hd->handler && c->arg == hd->arg) { sc = RTEMS_TOO_MANY; break; } @@ -280,7 +191,7 @@ rtems_status_code rtems_interrupt_server_handler_install( } if (sc == RTEMS_SUCCESSFUL) { - bsp_interrupt_server_call_helper(a, link); + *link = a; } } @@ -290,38 +201,26 @@ rtems_status_code rtems_interrupt_server_handler_install( free(a); } - return sc; + hd->sc = sc; + rtems_event_transient_send(hd->task); } -rtems_status_code rtems_interrupt_server_handler_remove( - rtems_id server, - rtems_vector_number vector, - rtems_interrupt_handler handler, - void *arg -) +static void bsp_interrupt_server_remove_helper(void *arg) { + bsp_interrupt_server_helper_data *hd = arg; rtems_status_code sc; bsp_interrupt_server_entry *e; rtems_option trigger_options; - sc = bsp_interrupt_server_is_initialized(); - if (sc != RTEMS_SUCCESSFUL) { - return sc; - } - - if (server != RTEMS_ID_NONE) { - return RTEMS_NOT_IMPLEMENTED; - } - bsp_interrupt_lock(); - e = bsp_interrupt_server_query_entry(vector, &trigger_options); + e = bsp_interrupt_server_query_entry(hd->vector, &trigger_options); if (e != NULL) { bsp_interrupt_server_action **link = &e->actions; bsp_interrupt_server_action *c; while ((c = *link) != NULL) { - if (c->handler == handler && c->arg == arg) { + if (c->handler == hd->handler && c->arg == hd->arg) { break; } @@ -333,18 +232,20 @@ rtems_status_code rtems_interrupt_server_handler_remove( if (remove_last) { rtems_interrupt_handler_remove( - vector, + hd->vector, bsp_interrupt_server_trigger, e ); } - bsp_interrupt_server_call_helper(c->next, link); + *link = c->next; free(c); if (remove_last) { free(e); } + + sc = RTEMS_SUCCESSFUL; } else { sc = RTEMS_UNSATISFIED; } @@ -354,7 +255,144 @@ rtems_status_code rtems_interrupt_server_handler_remove( bsp_interrupt_unlock(); - return sc; + hd->sc = sc; + rtems_event_transient_send(hd->task); +} + +static rtems_status_code bsp_interrupt_server_call_helper( + rtems_vector_number vector, + rtems_option options, + rtems_interrupt_handler handler, + void *arg, + void (*helper)(void *) +) +{ + bsp_interrupt_server_helper_data hd = { + .vector = vector, + .options = options, + .handler = handler, + .arg = arg, + .task = rtems_task_self() + }; + bsp_interrupt_server_action a = { + .handler = helper, + .arg = &hd + }; + bsp_interrupt_server_entry e = { + .vector = BSP_INTERRUPT_VECTOR_MAX + 1, + .actions = &a + }; + + bsp_interrupt_server_trigger(&e); + rtems_event_transient_receive(RTEMS_WAIT, RTEMS_NO_TIMEOUT); + + return hd.sc; +} + +static bsp_interrupt_server_entry *bsp_interrupt_server_get_entry(void) +{ + rtems_interrupt_lock_context lock_context; + bsp_interrupt_server_entry *e; + rtems_chain_control *chain; + + rtems_interrupt_lock_acquire(&bsp_interrupt_server_lock, &lock_context); + chain = &bsp_interrupt_server_chain; + + if (!rtems_chain_is_empty(chain)) { + e = (bsp_interrupt_server_entry *) + rtems_chain_get_first_unprotected(chain); + rtems_chain_set_off_chain(&e->node); + } else { + e = NULL; + } + + rtems_interrupt_lock_release(&bsp_interrupt_server_lock, &lock_context); + + return e; +} + +static void bsp_interrupt_server_task(rtems_task_argument arg) +{ + while (true) { + rtems_event_set events; + bsp_interrupt_server_entry *e; + + rtems_event_system_receive( + RTEMS_EVENT_SYSTEM_SERVER, + RTEMS_EVENT_ALL | RTEMS_WAIT, + RTEMS_NO_TIMEOUT, + &events + ); + + while ((e = bsp_interrupt_server_get_entry()) != NULL) { + bsp_interrupt_server_action *action = e->actions; + rtems_vector_number vector = e->vector; + + do { + bsp_interrupt_server_action *current = action; + action = action->next; + (*current->handler)(current->arg); + } while (action != NULL); + + bsp_interrupt_vector_enable(vector); + } + } +} + +rtems_status_code rtems_interrupt_server_handler_install( + rtems_id server, + rtems_vector_number vector, + const char *info, + rtems_option options, + rtems_interrupt_handler handler, + void *arg +) +{ + rtems_status_code sc; + + sc = bsp_interrupt_server_is_initialized(); + if (sc != RTEMS_SUCCESSFUL) { + return sc; + } + + if (server != RTEMS_ID_NONE) { + return RTEMS_NOT_IMPLEMENTED; + } + + return bsp_interrupt_server_call_helper( + vector, + options, + handler, + arg, + bsp_interrupt_server_install_helper + ); +} + +rtems_status_code rtems_interrupt_server_handler_remove( + rtems_id server, + rtems_vector_number vector, + rtems_interrupt_handler handler, + void *arg +) +{ + rtems_status_code sc; + + sc = bsp_interrupt_server_is_initialized(); + if (sc != RTEMS_SUCCESSFUL) { + return sc; + } + + if (server != RTEMS_ID_NONE) { + return RTEMS_NOT_IMPLEMENTED; + } + + return bsp_interrupt_server_call_helper( + vector, + 0, + handler, + arg, + bsp_interrupt_server_remove_helper + ); } rtems_status_code rtems_interrupt_server_initialize( |