From c8e4cbd63bba53290014a54d854286a14798580e Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 8 Nov 2008 15:12:56 +0000 Subject: Initial vector handling logic git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1166 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/sh/include/irq.h | 25 ----- nuttx/arch/sh/include/sh1/irq.h | 40 +++++++ nuttx/arch/sh/src/sh1/Make.defs | 2 +- nuttx/arch/sh/src/sh1/sh1_703x.h | 12 +-- nuttx/arch/sh/src/sh1/sh1_head.S | 3 +- nuttx/arch/sh/src/sh1/sh1_tramp.h | 95 +++++++++++++++++ nuttx/arch/sh/src/sh1/sh1_vector.S | 171 ++++++++++++++++++++++++++++++ nuttx/configs/us7032evb1/include/board.h | 16 +-- nuttx/configs/us7032evb1/ostest/ld.script | 1 + nuttx/configs/us7032evb1/src/up_leds.c | 57 ++++++++-- 10 files changed, 372 insertions(+), 50 deletions(-) create mode 100644 nuttx/arch/sh/src/sh1/sh1_tramp.h create mode 100644 nuttx/arch/sh/src/sh1/sh1_vector.S diff --git a/nuttx/arch/sh/include/irq.h b/nuttx/arch/sh/include/irq.h index 65821e653..d43237f96 100644 --- a/nuttx/arch/sh/include/irq.h +++ b/nuttx/arch/sh/include/irq.h @@ -51,31 +51,6 @@ * Definitions ****************************************************************************/ -/* IRQ Stack Frame Format: */ - -#define REG_R0 (0) -#define REG_R1 (1) -#define REG_R2 (2) -#define REG_R3 (3) -#define REG_R4 (4) -#define REG_R5 (5) -#define REG_R6 (6) -#define REG_R7 (7) -#define REG_R8 (8) -#define REG_R9 (9) -#define REG_R10 (10) -#define REG_R11 (11) -#define REG_R12 (12) -#define REG_R13 (13) -#define REG_R14 (14) -#define REG_R15 (15) -#define REG_SP REG_R15 -#define REG_SR (16) -#define REG_PC (17) - -#define XCPTCONTEXT_REGS (18) -#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) - /**************************************************************************** * Public Types ****************************************************************************/ diff --git a/nuttx/arch/sh/include/sh1/irq.h b/nuttx/arch/sh/include/sh1/irq.h index fd738c8f0..05319891d 100644 --- a/nuttx/arch/sh/include/sh1/irq.h +++ b/nuttx/arch/sh/include/sh1/irq.h @@ -327,6 +327,46 @@ /* 116-255 reserved */ #endif +/* IRQ Stack Frame Format. The SH-1 has a push down stack. The PC + * and SR are pushed by hardware at the time an IRQ is taken. + */ + +/* Saved to the stacked by up_vector */ + +#define REG_R0 (0) +#define REG_R1 (1) +#define REG_R2 (2) +#define REG_R3 (3) +#define REG_R5 (5) +#define REG_R6 (6) +#define REG_R7 (7) +#define REG_R8 (8) +#define REG_R9 (9) +#define REG_R10 (10) +#define REG_R11 (11) +#define REG_R12 (12) +#define REG_R13 (13) +#define REG_R14 (14) + +#define REG_PR (15) +#define REG_GBR (16) +#define REG_MACH (17) +#define REG_MACL (18) + +/* Saved to the stack by the trampoline logic */ + +#define REG_R4 (19) +#define REG_R15 (20) +#define REG_SP REG_R15 + +/* Pushed by hardware when the exception is taken */ + +#define REG_PC (21) +#define REG_SR (22) + +#define XCPTCONTEXT_REGS (23) +#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) + /************************************************************************************ * Public Types ************************************************************************************/ diff --git a/nuttx/arch/sh/src/sh1/Make.defs b/nuttx/arch/sh/src/sh1/Make.defs index ae2605c5a..7ba6f0d7a 100644 --- a/nuttx/arch/sh/src/sh1/Make.defs +++ b/nuttx/arch/sh/src/sh1/Make.defs @@ -46,7 +46,7 @@ ifneq ($(CONFIG_DISABLE_SIGNALS),y) CMN_CSRCS += up_schedulesigaction.c up_sigdeliver.c endif -CHIP_ASRCS = +CHIP_ASRCS = sh1_vector.S CHIP_CSRCS = sh1_lowputc.c sh1_irq.c sh1_timerisr.c sh1_serial.c ifeq ($(CONFIG_USBDEV),y) diff --git a/nuttx/arch/sh/src/sh1/sh1_703x.h b/nuttx/arch/sh/src/sh1/sh1_703x.h index 37b41e05f..22de4753d 100644 --- a/nuttx/arch/sh/src/sh1/sh1_703x.h +++ b/nuttx/arch/sh/src/sh1/sh1_703x.h @@ -243,12 +243,12 @@ /* Pin Function Controller (PFC) */ -#define SH1_PFC_PAIOR (0x05ffffc4) /* 16-bits wide */ -#define SH1_PFC_PBIOR (0x05ffffc6) /* 16-bits wide */ -#define SH1_PFC_PACR1 (0x05ffffc8) /* 16-bits wide */ -#define SH1_PFC_PACR2 (0x05ffffca) /* 16-bits wide */ -#define SH1_PFC_PBCR1 (0x05ffffcc) /* 16-bits wide */ -#define SH1_PFC_PBCR2 (0x05ffffce) /* 16-bits wide */ +#define SH1_PFC_PAIOR (0x05ffffc4) /* Port B I/O register (16-bits wide) */ +#define SH1_PFC_PBIOR (0x05ffffc6) /* Port B I/O register (16-bits wide) */ +#define SH1_PFC_PACR1 (0x05ffffc8) /* Port A control register 1 (16-bits wide) */ +#define SH1_PFC_PACR2 (0x05ffffca) /* Port A control register 2 (16-bits wide) */ +#define SH1_PFC_PBCR1 (0x05ffffcc) /* Port B control register 1 (16-bits wide) */ +#define SH1_PFC_PBCR2 (0x05ffffce) /* Port B control register 2 )16-bits wide) */ /* Port C */ diff --git a/nuttx/arch/sh/src/sh1/sh1_head.S b/nuttx/arch/sh/src/sh1/sh1_head.S index 82e8c7175..f446b37a4 100644 --- a/nuttx/arch/sh/src/sh1/sh1_head.S +++ b/nuttx/arch/sh/src/sh1/sh1_head.S @@ -143,7 +143,7 @@ __vector_table: * Text *****************************************************************************/ - .section .text + .section .reset /***************************************************************************** * Name: __start @@ -319,6 +319,7 @@ __start0: .long _svect .Lvectend: .long (SH1_IRQ7_VECOFFSET+3) + .size __start, .-__start /***************************************************************************** * DATA diff --git a/nuttx/arch/sh/src/sh1/sh1_tramp.h b/nuttx/arch/sh/src/sh1/sh1_tramp.h new file mode 100644 index 000000000..4be4bcf2b --- /dev/null +++ b/nuttx/arch/sh/src/sh1/sh1_tramp.h @@ -0,0 +1,95 @@ +/************************************************************************************ + * arch/sh/src/sh1/sh1_tramp.h + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_SH_SRC_SH1_TRAMPOLINE_H +#define __ARCH_SH_SRC_SH1_TRAMPOLINE_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * External references + ************************************************************************************/ + + .globl _up_vector + +/************************************************************************************ + * Macro: trampoline + * + * Description: + * Enter on exception with: + * + * SP -> PC + * SR + * + * Branch to up_vector with: + * + * R4 : IRQ vector number + * SP -> Saved R4 + * Saved SP (R15) + * PC + * SR + * + ************************************************************************************/ + + .macro trampoline, \irq + .section .reset + stc.l r4, @-r15 /* Save R4 on the stack */ + bra _up_vector /* Jump to the common vector handling logic */ + mov #\irq, r4 /* With the IRQ number in R4 */ + .endr + +#endif /* __ARCH_SH_SRC_SH1_TRAMPOLINE_H */ + + + + + + + + + + + + + diff --git a/nuttx/arch/sh/src/sh1/sh1_vector.S b/nuttx/arch/sh/src/sh1/sh1_vector.S new file mode 100644 index 000000000..1df2838d0 --- /dev/null +++ b/nuttx/arch/sh/src/sh1/sh1_vector.S @@ -0,0 +1,171 @@ +/***************************************************************************** + * arch/sh/src/sh1/sh1_vector.S + * + * 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 /* NuttX configuration settings */ +#include /* Board-specific settings */ +#include /* IRQ definitons */ + +#include "chip.h" /* Chip-specific settings */ +#include "up_internal.h" + +/***************************************************************************** + * Definitions + *****************************************************************************/ + +/***************************************************************************** + * External references + *****************************************************************************/ + +/* Called functions */ + + .globl _up_doirq /* C interrupt processing logic */ + +/***************************************************************************** + * Macros + *****************************************************************************/ + +/***************************************************************************** + * Text + *****************************************************************************/ + + .section .reset + +/***************************************************************************** + * Name: _up_vector + * + * Description: + * Execption entry point. Upon entry: + * + * R4 : Holds IRQ number + * SP -> Saved R4 + * Saved SP (R15) + * PC + * SR + * + *****************************************************************************/ + + .section irq + .global _up_vector + .type _up_vector, #function + +_up_vector: + /* Save the registers on the stack. (R4 is already saved) */ + + sts.l macl, @-r15 + sts.l mach, @-r15 + stc.l gbr, @-r15 + sts.l pr, @-r15 + + mov.l r14, @-r15 + mov.l r13, @-r15 + mov.l r12, @-r15 + mov.l r11, @-r15 + mov.l r10, @-r15 + mov.l r9, @-r15 + mov.l r8, @-r15 + + stc sr, r8 /* Mask all interrupts */ + mov.l .Lintmask, r9 + or r9, r8 + ldc r8, sr + + mov.l r7, @-r15 + mov.l r6, @-r15 + mov.l r5, @-r15 + mov.l r3, @-r15 + mov.l r2, @-r15 + mov.l r1, @-r15 + mov.l r0, @-r15 + + /* Setup parameters: R4=IRQ number, R5=base of saved state */ + + mov r15, r5 + add #-XCPTCONTEXT_SIZE, r5 + + /* Switch to the interrupt stack */ + + /* Dispatch the interrupt */ + + mov.l .Ldoirq, r0 + jsr @r0 + mov r15, r14 + + /* On return, R0 holds the address of the base of the XCPTCONTEXT + * structure to use for the return (may not be the same as the + * one that we passed in via r5. + */ + + add #XCPTCONTEXT_SIZE, r0 + mov r0, r15 + + /* Restore registers */ + + mov.l @r15+, r0 + mov.l @r15+, r1 + mov.l @r15+, r2 + mov.l @r15+, r3 + mov.l @r15+, r5 + mov.l @r15+, r6 + mov.l @r15+, r7 + mov.l @r15+, r8 + mov.l @r15+, r9 + mov.l @r15+, r10 + mov.l @r15+, r11 + mov.l @r15+, r12 + mov.l @r15+, r13 + mov.l @r15+, r14 + + lds.l @r15+, pr + ldc.l @r15+, gbr + lds.l @r15+, mach + lds.l @r15+, macl + + mov.l @r15+, r4 + add #4, r15 + rte + nop + + .align 2 +.Lintmask: + .long 0x000000f0 +.Ldoirq: + .long _up_doirq + .size _up_vector, .-_up_vector + .end + diff --git a/nuttx/configs/us7032evb1/include/board.h b/nuttx/configs/us7032evb1/include/board.h index c481b14b6..e73022b62 100644 --- a/nuttx/configs/us7032evb1/include/board.h +++ b/nuttx/configs/us7032evb1/include/board.h @@ -55,18 +55,18 @@ /* LED definitions **********************************************************/ -/* The SH1_LPEVB has no user controllable LEDs. These are provided only - * in the event that CONFIG_ARCH_LEDs is enabled. +/* The SH1_LPEVB only a single LED controlled by either port A, pin 15, or + * port B, pin 15 (selectable via JP8). */ #define LED_STARTED 0 #define LED_HEAPALLOCATE 1 -#define LED_IRQSENABLED 2 -#define LED_STACKCREATED 3 -#define LED_INIRQ 4 -#define LED_SIGNAL 5 -#define LED_ASSERTION 6 -#define LED_PANIC 7 +#define LED_IRQSENABLED 1 +#define LED_STACKCREATED 1 +#define LED_INIRQ 0 +#define LED_SIGNAL 0 +#define LED_ASSERTION 0 +#define LED_PANIC 1 /* Button definitions *******************************************************/ diff --git a/nuttx/configs/us7032evb1/ostest/ld.script b/nuttx/configs/us7032evb1/ostest/ld.script index 846fda4c6..60436cff1 100644 --- a/nuttx/configs/us7032evb1/ostest/ld.script +++ b/nuttx/configs/us7032evb1/ostest/ld.script @@ -54,6 +54,7 @@ SECTIONS . = 0x0a002400; .text : { _stext = ABSOLUTE(.); + *(.reset) /* Reset/IRQ code */ *(.text) /* Code */ *(.fixup) *(.gnu.warning) diff --git a/nuttx/configs/us7032evb1/src/up_leds.c b/nuttx/configs/us7032evb1/src/up_leds.c index 2e711471a..321299309 100644 --- a/nuttx/configs/us7032evb1/src/up_leds.c +++ b/nuttx/configs/us7032evb1/src/up_leds.c @@ -48,6 +48,15 @@ * Definitions ****************************************************************************/ +/* The SH1_LPEVB only a single LED controlled by either port A, pin 15, or + * port B, pin 15 (selectable via JP8). In this file, we assume the portB + * setup. + */ + +#define SH1_PBDR_LED 0x8000 +#define SH1_PBIOR_LED 0x8000 +#define SH1_PBCR2_LED 0xc000 + /**************************************************************************** * Private Data ****************************************************************************/ @@ -67,9 +76,25 @@ #ifdef CONFIG_ARCH_LEDS void up_ledinit(void) { - /* The SH1_LPEVB has no user controllable LEDs. This is provided only - * in the event that CONFIG_ARCH_LEDs is enabled. - */ + uint16 reg16; + + /* Setup port B, pin 15 as an output */ + + reg16 = getreg(SH1_PFC_PBIOR); + reg16 |= SH1_PBIOR_LED; + putreg(reg16, SH1_PFC_PBIOR); + + /* Setup port B, pin 15 as a normal I/O register */ + + reg16 = getreg(SH1_PFC_PBCR1); + reg16 &= ~SH1_PBCR2_LED; + putreg(reg16, SH1_PFC_PBCR1); + + /* Turn the LED off */ + + reg16 = getreg(SH1_PORTB_DR); + reg16 &= ~SH1_PBDR_LED; + putreg(reg16, SH1_PORTB_DR); } /**************************************************************************** @@ -78,9 +103,16 @@ void up_ledinit(void) void up_ledon(int led) { - /* The SH1_LPEVB has no user controllable LEDs. This is provided only - * in the event that CONFIG_ARCH_LEDs is enabled. - */ + uint16 reg16; + + if (led) + { + /* Turn the LED on */ + + reg16 = getreg(SH1_PORTB_DR); + reg16 |= SH1_PBDR_LED; + putreg(reg16, SH1_PORTB_DR); + } } /**************************************************************************** @@ -89,8 +121,15 @@ void up_ledon(int led) void up_ledoff(int led) { - /* The SH1_LPEVB has no user controllable LEDs. This is provided only - * in the event that CONFIG_ARCH_LEDs is enabled. - */ + uint16 reg16; + + if (led) + { + /* Turn the LED off */ + + reg16 = getreg(SH1_PORTB_DR); + reg16 &= ~SH1_PBDR_LED; + putreg(reg16, SH1_PORTB_DR); + } } #endif /* CONFIG_ARCH_LEDS */ -- cgit v1.2.3