From 5fbab9af64e5f09f8b806e5b66e6731efe06252c Mon Sep 17 00:00:00 2001 From: patacongo Date: Thu, 31 Jan 2008 22:19:53 +0000 Subject: *** empty log message *** git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@597 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/TODO | 5 +- nuttx/arch/z16/src/common/up_internal.h | 2 +- nuttx/arch/z16/src/common/up_schedulesigaction.c | 27 ++- nuttx/arch/z16/src/common/up_sigdeliver.c | 21 +-- nuttx/arch/z16/src/z16f/z16f_head.S | 2 +- nuttx/arch/z16/src/z16f/z16f_restoreusercontext.S | 215 +++++++++++----------- nuttx/examples/ostest/timedmqueue.c | 4 +- nuttx/include/nuttx/compiler.h | 2 +- 8 files changed, 133 insertions(+), 145 deletions(-) diff --git a/nuttx/TODO b/nuttx/TODO index 8029936d1..728efe2e5 100644 --- a/nuttx/TODO +++ b/nuttx/TODO @@ -383,6 +383,7 @@ o z16 (arch/z16) Priority: Medium. A polled, write-only serial driver is used in the interim for system testing. - Description: Signals are not functional + Description: The system delays do not appear to be correct with the + examples/ostest/timedmqueue.c test. Status: Open - Priority: Medium + Priority: Medium-High diff --git a/nuttx/arch/z16/src/common/up_internal.h b/nuttx/arch/z16/src/common/up_internal.h index 144fee754..1f2f51829 100644 --- a/nuttx/arch/z16/src/common/up_internal.h +++ b/nuttx/arch/z16/src/common/up_internal.h @@ -73,7 +73,7 @@ #define IN_INTERRUPT (current_regs != NULL) #define SAVE_IRQCONTEXT(tcb) up_copystate((tcb)->xcp.regs, current_regs) -#define SET_IRQCONTEXT(tcb) up_copystate(current_regs, (tcb)->xcp.regs) +#define SET_IRQCONTEXT(tcb) do { current_regs = (tcb)->xcp.regs; } while (0) #define SAVE_USERCONTEXT(tcb) up_saveusercontext((tcb)->xcp.regs) #define RESTORE_USERCONTEXT(tcb) up_restoreusercontext((tcb)->xcp.regs) #define SIGNAL_RETURN(regs) up_restoreusercontext(regs) diff --git a/nuttx/arch/z16/src/common/up_schedulesigaction.c b/nuttx/arch/z16/src/common/up_schedulesigaction.c index f085b2e95..4a4b0bcc8 100644 --- a/nuttx/arch/z16/src/common/up_schedulesigaction.c +++ b/nuttx/arch/z16/src/common/up_schedulesigaction.c @@ -133,19 +133,19 @@ void up_schedule_sigaction(FAR _TCB *tcb, sig_deliver_t sigdeliver) sigdeliver(tcb); } - /* CASE 2: We are in an interrupt handler AND the - * interrupted task is the same as the one that - * must receive the signal, then we will have to modify - * the return state as well as the state in the TCB. + /* CASE 2: We are in an interrupt handler AND the interrupted + * task is the same as the one that must receive the signal, then + * we will have to modify the return state as well as the state + * in the TCB. */ else { FAR uint32 *current_pc = (FAR uint32*)¤t_regs[REG_PC]; - /* Save the return address and interrupt state. - * These will be restored by the signal trampoline after - * the signals have been delivered. + /* Save the return address and interrupt state. These will be + * restored by the signal trampoline after the signals have + * been delivered. */ tcb->xcp.sigdeliver = sigdeliver; @@ -159,23 +159,22 @@ void up_schedule_sigaction(FAR _TCB *tcb, sig_deliver_t sigdeliver) *current_pc = (uint32)up_sigdeliver; current_regs[REG_FLAGS] = 0; - /* And make sure that the saved context in the TCB - * is the same as the interrupt return context. + /* And make sure that the saved context in the TCB is the + * same as the interrupt return context. */ up_copystate(tcb->xcp.regs, current_regs); } } - /* Otherwise, we are (1) signaling a task is not running - * from an interrupt handler or (2) we are not in an - * interrupt handler and the running task is signalling - * some non-running task. + /* Otherwise, we are (1) signaling a task is not running from an + * interrupt handler or (2) we are not in an interrupt handler + * and the running task is signalling some non-running task. */ else { - uint32 *saved_pc = (uint32*)&tcb->xcp.regs[REG_PC]; + FAR uint32 *saved_pc = (FAR uint32*)&tcb->xcp.regs[REG_PC]; /* Save the return lr and cpsr and one scratch register * These will be restored by the signal trampoline after diff --git a/nuttx/arch/z16/src/common/up_sigdeliver.c b/nuttx/arch/z16/src/common/up_sigdeliver.c index 09bcd14ac..afc554e85 100644 --- a/nuttx/arch/z16/src/common/up_sigdeliver.c +++ b/nuttx/arch/z16/src/common/up_sigdeliver.c @@ -72,10 +72,9 @@ * Name: up_sigdeliver * * Description: - * This is the a signal handling trampoline. When a - * signal action was posted. The task context was mucked - * with and forced to branch to this location with interrupts - * disabled. + * This is the a signal handling trampoline. When a signal action was + * posted. The task context was mucked with and forced to branch to this + * location with interrupts disabled. * ****************************************************************************/ @@ -106,10 +105,9 @@ void up_sigdeliver(void) regs32[REG_PC/2] = rtcb->xcp.saved_pc; regs[REG_FLAGS] = rtcb->xcp.saved_i; - /* Get a local copy of the sigdeliver function pointer. - * we do this so that we can nullify the sigdeliver - * function point in the TCB and accept more signal - * deliveries while processing the current pending + /* Get a local copy of the sigdeliver function pointer. We do this so + * that we can nullify the sigdeliver function point in the TCB and + * accept more signal deliveries while processing the current pending * signals. */ @@ -127,10 +125,9 @@ void up_sigdeliver(void) sigdeliver(rtcb); - /* Output any debug messaged BEFORE restoring errno - * (because they may alter errno), then restore the - * original errno that is needed by the user logic - * (it is probably EINTR). + /* Output any debug messaged BEFORE restoring errno (because they may + * alter errno), then restore the original errno that is needed by + * the user logic (it is probably EINTR). */ dbg("Resuming\n"); diff --git a/nuttx/arch/z16/src/z16f/z16f_head.S b/nuttx/arch/z16/src/z16f/z16f_head.S index 88b9ca5bb..569b0a3fd 100755 --- a/nuttx/arch/z16/src/z16f/z16f_head.S +++ b/nuttx/arch/z16/src/z16f/z16f_head.S @@ -456,7 +456,7 @@ _common_isr: * to use to return from the interrupt in r0. This may or may not be the * same value as sp. */ - + cp r0, sp /* Check if we are performing a context switch */ jp nz, _common_switch /* Jump if yes, else use faster return */ popmhi /* Restore r8-r14 */ diff --git a/nuttx/arch/z16/src/z16f/z16f_restoreusercontext.S b/nuttx/arch/z16/src/z16f/z16f_restoreusercontext.S index b65354611..0d6f5fec1 100755 --- a/nuttx/arch/z16/src/z16f/z16f_restoreusercontext.S +++ b/nuttx/arch/z16/src/z16f/z16f_restoreusercontext.S @@ -1,112 +1,103 @@ -/************************************************************************* - * arch/z16/src/z16f/z16f_restoreusercontext.asm - * - * Copyright (C) 2008 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSeqUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *************************************************************************/ - -/************************************************************************* - * Included Files - *************************************************************************/ - -#include -#include -#include "chip/chip.h" - -/************************************************************************* - * External References / External Definitions - *************************************************************************/ - - xdef _up_restoreusercontext - -/************************************************************************* - * Data Allocation - *************************************************************************/ - - define CODESEG, SPACE=EROM - segment CODESEG - -/************************************************************************* - * Code - *************************************************************************/ - -/************************************************************************* - * Name: up_restoreusercontext - * - * Description: - * Restore the current user context. - * - * Parameters: - * r1: pointer to the register save array in the XCPT structure - * - *************************************************************************/ - -_up_restoreusercontext: - /* Disable interrupts while we are accessing the TCB's register - * save structure - */ - - di - - /* Set up the stack for return */ - - ld sp, 2*REG_SP(r1) /* sp=Value of SP on return */ - ld r2, 2*REG_PC(r1) /* r2=return address */ - push r2 /* Push the return address onto the stack */ - ld r7, 2*REG_R7(r1) /* r7=saved value of r7 */ - push r7 /* Save this so that we can use r7 */ - ld r7, sp /* r7=saved SP value */ - - /* Retore r8-r14 */ - - ld sp, r1 /* sp=Points to register save structure */ - popmhi /* Restore r8-r14 */ - add sp, #4 /* Skip over r15=sp */ - - /* Test if interrupts should be enabled upon return */ - - ld.w r2, 2*REG_FLAGS(r1) /* r2=padded flags value */ - tm r2, #Z16F_CNTRL_FLAGS_IRQE /* Test IRQE bit (Z-bit == 0 if disabled) */ - popmlo /* Restore r0-r6 (retains Z-bit) */ - ld sp, r7 /* Recover the correct SP value (retains Z-bit) */ - pop r7 /* Get the saved value of r7 (retains Z-bit)*/ - jp z, _up_returndisabled /* Jump if interrupts should remain disabled */ - - /* Return with interrupts enabled. Note that at this point in the logic, - * we are finished accessing the TCB. Therefore, any interrupt level context - * switches that may modify the TCB should be OK - */ - - ei /* Enable interrupts */ - -_up_returndisabled: - ret - end +/************************************************************************* + * arch/z16/src/z16f/z16f_restoreusercontext.asm + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSeqUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + *************************************************************************/ + +/************************************************************************* + * Included Files + *************************************************************************/ + +#include +#include +#include "chip/chip.h" + +/************************************************************************* + * External References / External Definitions + *************************************************************************/ + + xdef _up_restoreusercontext + +/************************************************************************* + * Data Allocation + *************************************************************************/ + + define CODESEG, SPACE=EROM + segment CODESEG + +/************************************************************************* + * Code + *************************************************************************/ + +/************************************************************************* + * Name: up_restoreusercontext + * + * Description: + * Restore the current user context. + * + * Parameters: + * r1: pointer to the register save array in the XCPT structure + * + *************************************************************************/ + +_up_restoreusercontext: + /* Disable interrupts while we are accessing the TCB's register + * save structure. The correct interrupt state will be retored by + * the iret instruction below + */ + + di + + /* Set up the stack for return from 'interrupt' */ + + ld sp, 2*REG_SP(r1) /* sp=Value of SP on return */ + ld.w r2, 2*REG_FLAGS(r1) /* r1=padded flags value */ + push.w r2 /* Push padded flags value onto the stack */ + ld r2, 2*REG_PC(r1) /* r2=return address */ + push r2 /* Push the return address onto the stack */ + ld r7, 2*REG_R7(r1) /* r7=saved value of r7 */ + push r7 /* Save this so that we can use r7 */ + ld r7, sp /* r7=saved SP value */ + + /* Restore registers and return from interrupt */ + + ld sp, r1 /* sp=Pointer to register save structure */ + popmhi /* Restore r8-r14 */ + add sp, #4 /* Skip over restore of r15=sp */ + popmlo /* Restore r0-r6 */ + ld sp, r7 /* Switch to the correct stack for return */ + pop r7 /* Recover the saved value of r7 from the stack */ + iret2 /* Return from interrupt */ + + end + + diff --git a/nuttx/examples/ostest/timedmqueue.c b/nuttx/examples/ostest/timedmqueue.c index 3d68039d0..a25c05971 100644 --- a/nuttx/examples/ostest/timedmqueue.c +++ b/nuttx/examples/ostest/timedmqueue.c @@ -1,5 +1,5 @@ /************************************************************************** - * mqueue.c + * examples/ostest/mqueue.c * * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. * Author: Gregory Nutt @@ -380,7 +380,7 @@ void timedmqueue_test(void) /* Wait for the receiving thread to complete */ - printf("timedmqueue_test: Waiting for sender to complete\n"); + printf("timedmqueue_test: Waiting for receiver to complete\n"); pthread_join(receiver, &result); if (result != (void*)0) { diff --git a/nuttx/include/nuttx/compiler.h b/nuttx/include/nuttx/compiler.h index 10f0c1802..4115559a1 100644 --- a/nuttx/include/nuttx/compiler.h +++ b/nuttx/include/nuttx/compiler.h @@ -263,7 +263,7 @@ # define FAR _Far # define NEAR _Near # define DSEG _Far -# define CODE _Far +# define CODE _Erom /* Select the large, 32-bit addressing model */ -- cgit v1.2.3