From 6cdfb2ac92d8422136f926024b2c2f969fce6b9e Mon Sep 17 00:00:00 2001 From: shaojinchun Date: Thu, 9 May 2019 19:28:52 +0800 Subject: [PATCH] fix signal code --- include/rtdef.h | 3 + libcpu/arm/cortex-a/context_gcc.S | 81 +++++--------- libcpu/arm/cortex-a/start_gcc.S | 39 ++++++- src/scheduler.c | 84 ++++++++++---- src/signal.c | 176 +++++++++++++++++++++++------- src/thread.c | 2 + 6 files changed, 267 insertions(+), 118 deletions(-) diff --git a/include/rtdef.h b/include/rtdef.h index 743b91d959..253333253a 100644 --- a/include/rtdef.h +++ b/include/rtdef.h @@ -495,6 +495,7 @@ typedef siginfo_t rt_siginfo_t; #define RT_THREAD_STAT_SIGNAL 0x10 #define RT_THREAD_STAT_SIGNAL_READY (RT_THREAD_STAT_SIGNAL | RT_THREAD_READY) #define RT_THREAD_STAT_SIGNAL_WAIT 0x20 +#define RT_THREAD_STAT_SIGNAL_PENDING 0x40 #define RT_THREAD_STAT_SIGNAL_MASK 0xf0 /** @@ -596,7 +597,9 @@ struct rt_thread rt_sigset_t sig_pending; /**< the pending signals */ rt_sigset_t sig_mask; /**< the mask bits of signal */ +#ifndef RT_USING_SMP void *sig_ret; /**< the return stack pointer from signal */ +#endif rt_sighandler_t *sig_vectors; /**< vectors of signal handler */ void *si_list; /**< the signal infor list */ #endif diff --git a/libcpu/arm/cortex-a/context_gcc.S b/libcpu/arm/cortex-a/context_gcc.S index af5c516e4d..dff7a1de68 100644 --- a/libcpu/arm/cortex-a/context_gcc.S +++ b/libcpu/arm/cortex-a/context_gcc.S @@ -46,16 +46,7 @@ rt_hw_context_switch_to: mov r0, r1 bl rt_cpus_lock_status_restore #endif /*RT_USING_SMP*/ - -#ifdef RT_USING_LWP - ldmfd sp, {r13, r14}^ @ pop usr_sp usr_lr - add sp, #8 -#endif - - ldmfd sp!, {r4} @ pop new task spsr - msr spsr_cxsf, r4 - - ldmfd sp!, {r0-r12, lr, pc}^ @ pop new task r0-r12, lr & pc + b rt_hw_context_switch_exit .section .bss.share.isr _guest_switch_lvl: @@ -93,15 +84,7 @@ rt_hw_context_switch: mov r0, r2 bl rt_cpus_lock_status_restore #endif /*RT_USING_SMP*/ - -#ifdef RT_USING_LWP - ldmfd sp, {r13, r14}^ @ pop usr_sp usr_lr - add sp, #8 -#endif - - ldmfd sp!, {r4} @ pop new task cpsr to spsr - msr spsr_cxsf, r4 - ldmfd sp!, {r0-r12, lr, pc}^ @ pop new task r0-r12, lr & pc, copy spsr to cpsr + b rt_hw_context_switch_exit /* * void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to); @@ -123,51 +106,19 @@ rt_hw_context_switch: .globl rt_hw_context_switch_interrupt rt_hw_context_switch_interrupt: #ifdef RT_USING_SMP - /* r0 :irq_mod context + /* r0 :svc_mod context * r1 :addr of from_thread's sp * r2 :addr of to_thread's sp * r3 :to_thread's tcb */ - @ r0 point to {r0-r3} in stack - push {r1 - r3} - mov r1, r0 - add r0, r0, #4*4 - ldmfd r0!, {r4-r12,lr}@ reload saved registers - mrs r3, spsr @ get cpsr of interrupt thread - sub r2, lr, #4 @ save old task's pc to r2 - msr cpsr_c, #I_Bit|F_Bit|Mode_SVC - - stmfd sp!, {r2} @ push old task's pc - stmfd sp!, {r4-r12,lr}@ push old task's lr,r12-r4 - ldmfd r1, {r4-r7} @ restore r0-r3 of the interrupt thread - stmfd sp!, {r4-r7} @ push old task's r0-r3 - stmfd sp!, {r3} @ push old task's cpsr - -#ifdef RT_USING_LWP - stmfd sp, {r13,r14}^ @push usr_sp usr_lr - sub sp, #8 -#endif - - msr cpsr_c, #I_Bit|F_Bit|Mode_IRQ - pop {r1 - r3} - mov sp, r0 - msr cpsr_c, #I_Bit|F_Bit|Mode_SVC - str sp, [r1] + str r0, [r1] ldr sp, [r2] mov r0, r3 bl rt_cpus_lock_status_restore -#ifdef RT_USING_LWP - ldmfd sp, {r13,r14}^ @pop usr_sp usr_lr - add sp, #8 -#endif - - ldmfd sp!, {r4} @ pop new task's cpsr to spsr - msr spsr_cxsf, r4 - - ldmfd sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr + b rt_hw_context_switch_exit #else /*RT_USING_SMP*/ ldr r2, =rt_thread_switch_interrupt_flag @@ -183,3 +134,25 @@ _reswitch: str r1, [r2] bx lr #endif /*RT_USING_SMP*/ + +.global rt_hw_context_switch_exit +rt_hw_context_switch_exit: + +#ifdef RT_USING_SMP +#ifdef RT_USING_SIGNALS + mov r0, sp + cps #Mode_IRQ + bl rt_signal_check + cps #Mode_SVC + mov sp, r0 +#endif +#endif + +#ifdef RT_USING_LWP + ldmfd sp, {r13, r14}^ /* usr_sp, usr_lr */ + add sp, #8 +#endif + ldmfd sp!, {r1} + msr spsr_cxsf, r1 /* original mode */ + ldmfd sp!, {r0-r12,lr,pc}^ /* irq return */ + diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S index 51026b7ed5..014846f3aa 100644 --- a/libcpu/arm/cortex-a/start_gcc.S +++ b/libcpu/arm/cortex-a/start_gcc.S @@ -11,7 +11,6 @@ */ #include "rtconfig.h" - .equ Mode_USR, 0x10 .equ Mode_FIQ, 0x11 .equ Mode_IRQ, 0x12 @@ -158,20 +157,48 @@ vector_fiq: vector_irq: #ifdef RT_USING_SMP clrex + + stmfd sp!, {r0, r1} + cps #Mode_SVC + mov r0, sp /* svc_sp */ + mov r1, lr /* svc_lr */ + + cps #Mode_IRQ + sub lr, #4 + stmfd r0!, {r1, lr} /* svc_lr, svc_pc */ + stmfd r0!, {r2 - r12} + ldmfd sp!, {r1, r2} /* original r0, r1 */ + stmfd r0!, {r1 - r2} + mrs r1, spsr /* original mode */ + stmfd r0!, {r1} + +#ifdef RT_USING_LWP + stmfd r0, {r13, r14}^ /* usr_sp, usr_lr */ + sub r0, #8 #endif - stmfd sp!, {r0-r12,lr} + /* now irq stack is clean */ + /* r0 is task svc_sp */ + /* backup r0 -> r8 */ + mov r8, r0 bl rt_interrupt_enter bl rt_hw_trap_irq bl rt_interrupt_leave -#ifdef RT_USING_SMP - mov r0, sp + cps #Mode_SVC + mov sp, r8 + mov r0, r8 bl rt_scheduler_do_irq_switch - ldmfd sp!, {r0-r12,lr} - subs pc, lr, #4 + b rt_hw_context_switch_exit + #else + stmfd sp!, {r0-r12,lr} + + bl rt_interrupt_enter + bl rt_hw_trap_irq + bl rt_interrupt_leave + @ if rt_thread_switch_interrupt_flag set, jump to @ rt_hw_context_switch_interrupt_do and don't return ldr r0, =rt_thread_switch_interrupt_flag diff --git a/src/scheduler.c b/src/scheduler.c index be29d9a7e0..3da723687c 100644 --- a/src/scheduler.c +++ b/src/scheduler.c @@ -322,8 +322,23 @@ void rt_schedule(void) if (pcpu->irq_nest) { pcpu->irq_switch_flag = 1; + rt_hw_interrupt_enable(level); + goto __exit; } - else if (current_thread->scheduler_lock_nest == 1) /* whether lock scheduler */ + +#ifdef RT_USING_SIGNALS + if ((current_thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_SUSPEND) + { + /* if current_thread signal is in pending */ + + if ((current_thread->stat & RT_THREAD_STAT_SIGNAL_MASK) & RT_THREAD_STAT_SIGNAL_PENDING) + { + rt_thread_resume(current_thread); + } + } +#endif + + if (current_thread->scheduler_lock_nest == 1) /* whether lock scheduler */ { rt_ubase_t highest_ready_priority; @@ -366,27 +381,32 @@ void rt_schedule(void) _rt_scheduler_stack_check(to_thread); #endif - { - extern void rt_thread_handle_sig(rt_bool_t clean_state); - - rt_hw_context_switch((rt_ubase_t)¤t_thread->sp, - (rt_ubase_t)&to_thread->sp, to_thread); - - /* enable interrupt */ - rt_hw_interrupt_enable(level); - -#ifdef RT_USING_SIGNALS - /* check signal status */ - rt_thread_handle_sig(RT_TRUE); -#endif - goto __exit; - } + rt_hw_context_switch((rt_ubase_t)¤t_thread->sp, + (rt_ubase_t)&to_thread->sp, to_thread); } } } +#ifdef RT_USING_SIGNALS + if (current_thread->stat & RT_THREAD_STAT_SIGNAL_PENDING) + { + extern void rt_thread_handle_sig(rt_bool_t clean_state); + + current_thread->stat &= ~RT_THREAD_STAT_SIGNAL_PENDING; + + rt_hw_interrupt_enable(level); + + /* check signal status */ + rt_thread_handle_sig(RT_TRUE); + } + else + { + rt_hw_interrupt_enable(level); + } +#else /* enable interrupt */ rt_hw_interrupt_enable(level); +#endif __exit: return ; @@ -465,13 +485,25 @@ void rt_schedule(void) rt_hw_context_switch((rt_ubase_t)&from_thread->sp, (rt_ubase_t)&to_thread->sp); +#ifdef RT_USING_SIGNALS + if (rt_current_thread->stat & RT_THREAD_STAT_SIGNAL_PENDING) + { + extern void rt_thread_handle_sig(rt_bool_t clean_state); + + rt_current_thread->stat &= ~RT_THREAD_STAT_SIGNAL_PENDING; + rt_hw_interrupt_enable(level); + + /* check signal status */ + rt_thread_handle_sig(RT_TRUE); + } + else + { + rt_hw_interrupt_enable(level); + } +#else /* enable interrupt */ rt_hw_interrupt_enable(level); - -#ifdef RT_USING_SIGNALS - /* check signal status */ - rt_thread_handle_sig(RT_TRUE); #endif goto __exit; } @@ -519,6 +551,18 @@ void rt_scheduler_do_irq_switch(void *context) pcpu = rt_cpu_index(cpu_id); current_thread = pcpu->current_thread; +#ifdef RT_USING_SIGNALS + if ((current_thread->stat & RT_THREAD_STAT_MASK) == RT_THREAD_SUSPEND) + { + /* if current_thread signal is in pending */ + + if ((current_thread->stat & RT_THREAD_STAT_SIGNAL_MASK) & RT_THREAD_STAT_SIGNAL_PENDING) + { + rt_thread_resume(current_thread); + } + } +#endif + if (pcpu->irq_switch_flag == 0) { rt_hw_interrupt_enable(level); diff --git a/src/signal.c b/src/signal.c index 06c2055d26..0cab6215af 100644 --- a/src/signal.c +++ b/src/signal.c @@ -52,17 +52,27 @@ static void _signal_entry(void *parameter) /* handle signal */ rt_thread_handle_sig(RT_FALSE); - /* never come back... */ - rt_hw_interrupt_disable(); +#ifdef RT_USING_SMP + { + struct rt_cpu* pcpu = rt_cpu_self(); + + if (--pcpu->current_thread->cpus_lock_nest == 0) + { + pcpu->current_thread->scheduler_lock_nest--; + } + + } +#else /* return to thread */ tid->sp = tid->sig_ret; tid->sig_ret = RT_NULL; +#endif LOG_D("switch back to: 0x%08x\n", tid->sp); tid->stat &= ~RT_THREAD_STAT_SIGNAL; #ifdef RT_USING_SMP - rt_hw_context_switch_to((rt_ubase_t)&(tid->sp), tid); + rt_hw_context_switch_to((rt_base_t)¶meter, tid); #else rt_hw_context_switch_to((rt_ubase_t)&(tid->sp)); #endif /*RT_USING_SMP*/ @@ -82,16 +92,21 @@ static void _signal_deliver(rt_thread_t tid) { rt_ubase_t level; + level = rt_hw_interrupt_disable(); + /* thread is not interested in pended signals */ - if (!(tid->sig_pending & tid->sig_mask)) return; + if (!(tid->sig_pending & tid->sig_mask)) + { + rt_hw_interrupt_enable(level); + return; + } - level = rt_hw_interrupt_disable(); if ((tid->stat & RT_THREAD_STAT_MASK) == RT_THREAD_SUSPEND) { /* resume thread to handle signal */ rt_thread_resume(tid); /* add signal state */ - tid->stat |= RT_THREAD_STAT_SIGNAL; + tid->stat |= (RT_THREAD_STAT_SIGNAL | RT_THREAD_STAT_SIGNAL_PENDING); rt_hw_interrupt_enable(level); @@ -108,17 +123,36 @@ static void _signal_deliver(rt_thread_t tid) rt_hw_interrupt_enable(level); /* do signal action in self thread context */ - rt_thread_handle_sig(RT_TRUE); + if (rt_interrupt_get_nest() == 0) + { + rt_thread_handle_sig(RT_TRUE); + } } else if (!((tid->stat & RT_THREAD_STAT_SIGNAL_MASK) & RT_THREAD_STAT_SIGNAL)) { /* add signal state */ - tid->stat |= RT_THREAD_STAT_SIGNAL; + tid->stat |= (RT_THREAD_STAT_SIGNAL | RT_THREAD_STAT_SIGNAL_PENDING); +#ifdef RT_USING_SMP + { + int cpu_id; + + cpu_id = tid->oncpu; + if ((cpu_id != RT_CPU_DETACHED) && (cpu_id != rt_hw_cpu_id())) + { + rt_uint32_t cpu_mask; + + cpu_mask = RT_CPU_MASK ^ (1 << cpu_id); + rt_hw_ipi_send(RT_SCHEDULE_IPI, cpu_mask); + } + } +#else /* point to the signal handle entry */ + tid->stat &= ~RT_THREAD_STAT_SIGNAL_PENDING; tid->sig_ret = tid->sp; tid->sp = rt_hw_stack_init((void *)_signal_entry, RT_NULL, (void *)((char *)tid->sig_ret - 32), RT_NULL); +#endif rt_hw_interrupt_enable(level); LOG_D("signal stack pointer @ 0x%08x", tid->sp); @@ -133,14 +167,53 @@ static void _signal_deliver(rt_thread_t tid) } } +#ifdef RT_USING_SMP +void *rt_signal_check(void* context) +{ + rt_base_t level; + int cpu_id; + struct rt_cpu* pcpu; + struct rt_thread *current_thread; + + level = rt_hw_interrupt_disable(); + cpu_id = rt_hw_cpu_id(); + pcpu = rt_cpu_index(cpu_id); + current_thread = pcpu->current_thread; + + if (pcpu->irq_nest) + { + rt_hw_interrupt_enable(level); + return context; + } + + if (current_thread->cpus_lock_nest == 1) + { + if (current_thread->stat & RT_THREAD_STAT_SIGNAL_PENDING) + { + void *sig_context; + + current_thread->stat &= ~RT_THREAD_STAT_SIGNAL_PENDING; + + rt_hw_interrupt_enable(level); + sig_context = rt_hw_stack_init((void *)_signal_entry, context, + (void *)(context - 32), RT_NULL); + return sig_context; + } + } + rt_hw_interrupt_enable(level); + return context; +} +#endif + rt_sighandler_t rt_signal_install(int signo, rt_sighandler_t handler) { + rt_base_t level; rt_sighandler_t old = RT_NULL; rt_thread_t tid = rt_thread_self(); if (!sig_valid(signo)) return SIG_ERR; - rt_enter_critical(); + level = rt_hw_interrupt_disable(); if (tid->sig_vectors == RT_NULL) { rt_thread_alloc_sig(tid); @@ -154,7 +227,7 @@ rt_sighandler_t rt_signal_install(int signo, rt_sighandler_t handler) else if (handler == SIG_DFL) tid->sig_vectors[signo] = _signal_default_handler; else tid->sig_vectors[signo] = handler; } - rt_exit_critical(); + rt_hw_interrupt_enable(level); return old; } @@ -272,7 +345,20 @@ __done: LOG_D("sigwait: %d sig raised!", signo); if (si_prev) si_prev->list.next = si_node->list.next; - else tid->si_list = si_node->list.next; + else + { + struct siginfo_node *node_next; + + if (si_node->list.next) + { + node_next = (void *)rt_slist_entry(si_node->list.next, struct siginfo_node, list); + tid->si_list = node_next; + } + else + { + tid->si_list = RT_NULL; + } + } /* clear pending */ tid->sig_pending &= ~sig_mask(signo); @@ -281,7 +367,14 @@ __done: } si_prev = si_node; - si_node = (void *)rt_slist_entry(si_node->list.next, struct siginfo_node, list); + if (si_node->list.next) + { + si_node = (void *)rt_slist_entry(si_node->list.next, struct siginfo_node, list); + } + else + { + si_node = RT_NULL; + } } __done_int: @@ -320,13 +413,13 @@ void rt_thread_handle_sig(rt_bool_t clean_state) signo = si_node->si.si_signo; handler = tid->sig_vectors[signo]; + tid->sig_pending &= ~sig_mask(signo); rt_hw_interrupt_enable(level); LOG_D("handle signal: %d, handler 0x%08x", signo, handler); if (handler) handler(signo); level = rt_hw_interrupt_disable(); - tid->sig_pending &= ~sig_mask(signo); error = -RT_EINTR; rt_mp_free(si_node); /* release this siginfo node */ @@ -335,10 +428,16 @@ void rt_thread_handle_sig(rt_bool_t clean_state) } /* whether clean signal status */ - if (clean_state == RT_TRUE) tid->stat &= ~RT_THREAD_STAT_SIGNAL; + if (clean_state == RT_TRUE) + { + tid->stat &= ~RT_THREAD_STAT_SIGNAL; + } + else + { + return; + } } } - rt_hw_interrupt_enable(level); } @@ -364,30 +463,30 @@ void rt_thread_alloc_sig(rt_thread_t tid) void rt_thread_free_sig(rt_thread_t tid) { rt_base_t level; - struct siginfo_node *si_list; + struct siginfo_node *si_node; rt_sighandler_t *sig_vectors; level = rt_hw_interrupt_disable(); - si_list = (struct siginfo_node *)tid->si_list; + si_node = (struct siginfo_node *)tid->si_list; tid->si_list = RT_NULL; sig_vectors = tid->sig_vectors; tid->sig_vectors = RT_NULL; rt_hw_interrupt_enable(level); - if (si_list) + if (si_node) { struct rt_slist_node *node; - struct siginfo_node *si_node; + struct rt_slist_node *node_to_free; LOG_D("free signal info list"); - node = &(si_list->list); + node = &(si_node->list); do { - si_node = rt_slist_entry(node, struct siginfo_node, list); - rt_mp_free(si_node); - + node_to_free = node; node = node->next; + si_node = rt_slist_entry(node_to_free, struct siginfo_node, list); + rt_mp_free(si_node); } while (node); } @@ -418,30 +517,23 @@ int rt_thread_kill(rt_thread_t tid, int sig) struct rt_slist_node *node; struct siginfo_node *entry; - node = (struct rt_slist_node *)tid->si_list; - rt_hw_interrupt_enable(level); + si_node = (struct siginfo_node *)tid->si_list; + if (si_node) + node = (struct rt_slist_node *)&si_node->list; + else + node = RT_NULL; /* update sig info */ - rt_enter_critical(); for (; (node) != RT_NULL; node = node->next) { entry = rt_slist_entry(node, struct siginfo_node, list); if (entry->si.si_signo == sig) { memcpy(&(entry->si), &si, sizeof(siginfo_t)); - rt_exit_critical(); + rt_hw_interrupt_enable(level); return 0; } } - rt_exit_critical(); - - /* disable interrupt to protect tcb */ - level = rt_hw_interrupt_disable(); - } - else - { - /* a new signal */ - tid->sig_pending |= sig_mask(sig); } rt_hw_interrupt_enable(level); @@ -452,14 +544,22 @@ int rt_thread_kill(rt_thread_t tid, int sig) memcpy(&(si_node->si), &si, sizeof(siginfo_t)); level = rt_hw_interrupt_disable(); - if (!tid->si_list) tid->si_list = si_node; - else + + if (tid->si_list) { struct siginfo_node *si_list; si_list = (struct siginfo_node *)tid->si_list; rt_slist_append(&(si_list->list), &(si_node->list)); } + else + { + tid->si_list = si_node; + } + + /* a new signal */ + tid->sig_pending |= sig_mask(sig); + rt_hw_interrupt_enable(level); } else diff --git a/src/thread.c b/src/thread.c index 0f2e1dc3d0..276bba9938 100644 --- a/src/thread.c +++ b/src/thread.c @@ -191,7 +191,9 @@ static rt_err_t _rt_thread_init(struct rt_thread *thread, thread->sig_mask = 0x00; thread->sig_pending = 0x00; +#ifndef RT_USING_SMP thread->sig_ret = RT_NULL; +#endif thread->sig_vectors = RT_NULL; thread->si_list = RT_NULL; #endif -- GitLab