diff --git a/sched/irq/irq_attach.c b/sched/irq/irq_attach.c index 3f1e534bf49..849f9be9fa4 100644 --- a/sched/irq/irq_attach.c +++ b/sched/irq/irq_attach.c @@ -113,19 +113,22 @@ int ndx_to_irq(int ndx) int irq_attach(int irq, xcpt_t isr, FAR void *arg) { + int ret = OK; #if NR_IRQS > 0 - int ret = -EINVAL; + int ndx = -EINVAL; + irqstate_t flags; if (irq >= 0 && irq < NR_IRQS) { - int ndx = IRQ_TO_NDX(irq); - irqstate_t flags; - - if (ndx < 0) - { - return ndx; - } + ndx = IRQ_TO_NDX(irq); + } + if (ndx < 0) + { + ret = ndx; + } + else + { /* If the new ISR is NULL, then the ISR is being detached. * In this case, disable the ISR and direct any interrupts * to the unexpected interrupt handler. @@ -166,26 +169,24 @@ int irq_attach(int irq, xcpt_t isr, FAR void *arg) { ret = irqchain_attach(ndx, isr, arg); spin_unlock_irqrestore(&g_irqlock, flags); - return ret; } + else #endif + { + /* Save the new ISR and its argument in the table. */ - /* Save the new ISR and its argument in the table. */ + g_irqvector[ndx].handler = isr; + g_irqvector[ndx].arg = arg; + #ifdef CONFIG_SCHED_IRQMONITOR + g_irqvector[ndx].start = clock_systime_ticks(); + g_irqvector[ndx].time = 0; + g_irqvector[ndx].count = 0; + #endif - g_irqvector[ndx].handler = isr; - g_irqvector[ndx].arg = arg; -#ifdef CONFIG_SCHED_IRQMONITOR - g_irqvector[ndx].start = clock_systime_ticks(); - g_irqvector[ndx].time = 0; - g_irqvector[ndx].count = 0; -#endif - - spin_unlock_irqrestore(&g_irqlock, flags); - ret = OK; + spin_unlock_irqrestore(&g_irqlock, flags); + } } +#endif /* NR_IRQS */ return ret; -#else - return OK; -#endif } diff --git a/sched/irq/irq_attach_thread.c b/sched/irq/irq_attach_thread.c index ef57c7f1510..9e2fc5df333 100644 --- a/sched/irq/irq_attach_thread.c +++ b/sched/irq/irq_attach_thread.c @@ -145,6 +145,7 @@ static int isr_thread_main(int argc, FAR char *argv[]) int irq_attach_thread(int irq, xcpt_t isr, xcpt_t isrthread, FAR void *arg, int priority, int stack_size) { + int ret = OK; #if NR_IRQS > 0 static pid_t irq_thread_pid[NR_IRQS]; @@ -154,56 +155,52 @@ int irq_attach_thread(int irq, xcpt_t isr, xcpt_t isrthread, FAR void *arg, char arg3[32]; /* isrthread */ char arg4[32]; /* arg */ pid_t pid; - int ndx; + int ndx = -EINVAL; - if (irq < 0 || irq >= NR_IRQS) + if (irq >= 0 && irq < NR_IRQS) { - return -EINVAL; + ndx = IRQ_TO_NDX(irq); } - ndx = IRQ_TO_NDX(irq); if (ndx < 0) { - return ndx; + ret = ndx; } - - /* If the isrthread is NULL, then the ISR is being detached. */ - - if (isrthread == NULL) + else if(isrthread == NULL) { + /* If the isrthread is NULL, then the ISR is being detached. */ + irq_detach(irq); DEBUGASSERT(irq_thread_pid[ndx] != 0); kthread_delete(irq_thread_pid[ndx]); irq_thread_pid[ndx] = 0; - - return OK; } - - if (irq_thread_pid[ndx] != 0) + else if(irq_thread_pid[ndx] != 0) { - return -EINVAL; + ret = -EINVAL; } - - snprintf(arg1, sizeof(arg1), "%d", irq); - snprintf(arg2, sizeof(arg2), "%p", isr); - snprintf(arg3, sizeof(arg3), "%p", isrthread); - snprintf(arg4, sizeof(arg4), "%p", arg); - argv[0] = arg1; - argv[1] = arg2; - argv[2] = arg3; - argv[3] = arg4; - argv[4] = NULL; - - pid = kthread_create("isr_thread", priority, stack_size, - isr_thread_main, argv); - if (pid < 0) + else { - return pid; + snprintf(arg1, sizeof(arg1), "%d", irq); + snprintf(arg2, sizeof(arg2), "%p", isr); + snprintf(arg3, sizeof(arg3), "%p", isrthread); + snprintf(arg4, sizeof(arg4), "%p", arg); + argv[0] = arg1; + argv[1] = arg2; + argv[2] = arg3; + argv[3] = arg4; + argv[4] = NULL; + + pid = kthread_create("isr_thread", priority, stack_size, + isr_thread_main, argv); + if (pid < 0) + { + ret = pid; + } + + irq_thread_pid[ndx] = pid; } - - irq_thread_pid[ndx] = pid; - #endif /* NR_IRQS */ - return OK; + return ret; } diff --git a/sched/irq/irq_attach_wqueue.c b/sched/irq/irq_attach_wqueue.c index 10fdaf7c7bd..9ca431e7011 100644 --- a/sched/irq/irq_attach_wqueue.c +++ b/sched/irq/irq_attach_wqueue.c @@ -75,7 +75,7 @@ inline_function FAR struct kwork_wqueue_s *irq_get_wqueue(int priority) static mutex_t irq_wqueue_lock = NXMUTEX_INITIALIZER; static FAR struct kwork_wqueue_s *irq_wqueue[CONFIG_IRQ_NWORKS]; - FAR struct kwork_wqueue_s *queue; + FAR struct kwork_wqueue_s *queue = NULL; int wqueue_priority; int i; @@ -89,17 +89,22 @@ inline_function FAR struct kwork_wqueue_s *irq_get_wqueue(int priority) if (wqueue_priority == priority) { nxmutex_unlock(&irq_wqueue_lock); - return irq_wqueue[i]; + queue = irq_wqueue[i]; + break; } } DEBUGASSERT(i < CONFIG_IRQ_NWORKS); - queue = work_queue_create("isrwork", priority, irq_work_stack[i], - CONFIG_IRQ_WORK_STACKSIZE, 1); + if (queue == NULL) + { + queue = work_queue_create("isrwork", priority, irq_work_stack[i], + CONFIG_IRQ_WORK_STACKSIZE, 1); + + irq_wqueue[i] = queue; + nxmutex_unlock(&irq_wqueue_lock); + } - irq_wqueue[i] = queue; - nxmutex_unlock(&irq_wqueue_lock); return queue; } @@ -170,47 +175,48 @@ int irq_attach_wqueue(int irq, xcpt_t isr, xcpt_t isrwork, #endif FAR struct irq_work_info_s *info; - + int ret = OK; #if NR_IRQS > 0 - int ndx; + int ndx = -EINVAL; - if (irq < 0 || irq >= NR_IRQS) + if (irq >= 0 && irq < NR_IRQS) { - return -EINVAL; + ndx = IRQ_TO_NDX(irq); } - ndx = IRQ_TO_NDX(irq); if (ndx < 0) { - return ndx; + ret = ndx; } - - /* If the isrwork is NULL, then the ISR is being detached. */ - - info = &irq_work_vector[ndx]; - - if (isrwork == NULL) + else { - irq_detach(irq); - info->isrwork = NULL; - info->handler = NULL; - info->arg = NULL; - info->wqueue = NULL; - return OK; - } + /* If the isrwork is NULL, then the ISR is being detached. */ - info->isrwork = isrwork; - info->handler = isr; - info->arg = arg; - info->irq = irq; - if (info->wqueue == NULL) - { - info->wqueue = irq_get_wqueue(priority); - } + info = &irq_work_vector[ndx]; - irq_attach(irq, irq_default_handler, info); + if (isrwork == NULL) + { + irq_detach(irq); + info->isrwork = NULL; + info->handler = NULL; + info->arg = NULL; + info->wqueue = NULL; + } + else + { + info->isrwork = isrwork; + info->handler = isr; + info->arg = arg; + info->irq = irq; + if (info->wqueue == NULL) + { + info->wqueue = irq_get_wqueue(priority); + } + + irq_attach(irq, irq_default_handler, info); + } + } #endif /* NR_IRQS */ - return OK; + return ret; } -