From 709f5259f948dd3ee2b7e8b7869b00427a6c97d2 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 24 Feb 2013 17:30:55 +0000 Subject: More Cortex-M0/NUC120 progress git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@5669 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/armv6-m/up_exception.S | 57 ++++++++++++++++++++-------- nuttx/arch/arm/src/nuc1xx/Kconfig | 46 +++++++++++++++++++++++ nuttx/arch/arm/src/nuc1xx/chip/nuc_clk.h | 5 +++ nuttx/arch/arm/src/nuc1xx/nuc_clockconfig.c | 10 ++++- nuttx/arch/arm/src/nuc1xx/nuc_irq.c | 15 ++++++-- nuttx/arch/arm/src/nuc1xx/nuc_lowputc.c | 58 +++++++++++++++++++---------- nuttx/arch/arm/src/nuc1xx/nuc_timerisr.c | 42 +++++++++++++++++---- 7 files changed, 186 insertions(+), 47 deletions(-) (limited to 'nuttx/arch/arm') diff --git a/nuttx/arch/arm/src/armv6-m/up_exception.S b/nuttx/arch/arm/src/armv6-m/up_exception.S index 4bc59126c..33f89b3b1 100644 --- a/nuttx/arch/arm/src/armv6-m/up_exception.S +++ b/nuttx/arch/arm/src/armv6-m/up_exception.S @@ -86,11 +86,11 @@ .type exception_common, function exception_common: - mrs r0, ipsr /* R0=exception number */ - /* Complete the context save */ - /* The EXC_RETURN value tells us whether the context is on the MSP or PSP */ + /* Get the current stack pointer. The EXC_RETURN value tells us whether + * the context is on the MSP or PSP. + */ mov r0, r14 /* Copy high register to low register */ lsl r1, r0, #(31 - EXC_RETURN_PROCESS_BITNO) /* Move to bit 31 */ @@ -101,15 +101,26 @@ exception_common: 1: mrs r1, psp /* R1=The process stack pointer */ + /* R1 is the current stack pointer. HW_XCPT_REGS were pushed onto the stack + * when the interrupt was taken so (R1)+HW_XCPT_SIZE is the value of the + * stack pointer before the interrupt. The total size of the context save + * area is XCPTCONTEXT_SIZE = SW_XCPT_SIZE + HW_XCPT_SIZE so (R1)-SW_XCPT_SIZE + * is the address of the beginning of the context save area. + */ + 2: - mov r2, #XCPTCONTEXT_SIZE /* R2=Size of context array */ - sub r0, r1, r2 /* R0=Beginning of context array on the stack */ - mov r2, r1 /* R2=Copy of the main/process stack pointer */ - add r2, #HW_XCPT_SIZE /* R2=MSP/PSP before the interrupt was taken */ + /* Save SP, BASEPRI, and R4-R7 in the context array */ + + sub r1, #SW_XCPT_SIZE /* R1=Beginning of context array on the stack */ + mov r2, #XCPTCONTEXT_SIZE /* R2=Size of the context array */ + add r2, r1 /* R2=MSP/PSP before the interrupt was taken */ /* (ignoring the xPSR[9] alignment bit) */ mrs r3, basepri /* R3=Current BASEPRI setting */ + mov r0, r1 /* Copy the context array pointer */ + stmia r0!, {r2-r7} /* Save the SP, BASEPRI, and R4-R7 in the context array */ + + /* Save R8-R11 and the EXEC_RETURN value in the context array */ - stmia r0!, {r2-r7} /* Save the SP, BASEPRI, and R4-R7 */ mov r2, r8 /* Copy high registers to low */ mov r3, r9 mov r4, r10 @@ -117,6 +128,10 @@ exception_common: mov r6, r14 stmia r0!, {r2-r6} /* Save the high registers r8-r11 and r14 */ + /* Get the exception number in R0=IRQ, R1=register save area on stack */ + + mrs r0, ipsr /* R0=exception number */ + /* Disable interrupts, select the stack to use for interrupt handling * and call up_doirq to handle the interrupt */ @@ -134,9 +149,9 @@ exception_common: bl up_doirq /* R0=IRQ, R1=register save area on stack */ pop {r1} /* Recover R1=main stack pointer */ #else - msr msp, r1 /* We are using the main stack pointer */ + msr msp, r1 /* We are using the main stack pointer */ bl up_doirq /* R0=IRQ, R1=register save area on stack */ - mrs r1, msp /* Recover R1=main stack pointer */ + mrs r1, msp /* Recover R1=main stack pointer */ #endif /* On return from up_doirq, r0 will hold a pointer to register context @@ -173,26 +188,38 @@ exception_common: * the same stack frame that we created at entry. */ + /* Recover R8-R11 and EXEC_RETURN (5 registers) */ + mov r2, #(4*REG_R8) /* R2=Offset to R8 storage */ sub r0, r1, r2 /* R0=Address of R8 storage */ - ldmia r0!, {r2-r6} /* Recover R8-R11 and R14 */ + ldmia r0!, {r2-r6} /* Recover R8-R11 and R14 (5 registers)*/ mov r8, r2 /* Move to position in high registers */ mov r9, r3 mov r10, r4 mov r11, r5 - mov r14, r6 + mov r14, r6 /* EXEC_RETURN */ + + /* Recover SP (R2), BASEPRI (R3), and R4-R7. The size of the sofware saved + * portion is 11 words. 5 were recovered above; 6 are recovered by the + * ldmia below. So adding (4*5) to r1 accounts for all of the software + * saved registers. + */ + ldmia r1!, {r2-r7} /* Recover R4-R7 + 2 temp values */ + add r1, #(4*5) /* R1=Value of MSP/PSP on exception entry */ - /* The EXC_RETURN value tells us whether the context is on the MSP or PSP */ + /* Restore the stack pointer. The EXC_RETURN value tells us whether the + * context is on the MSP or PSP. + */ mov r0, r14 /* Copy high register to low register */ lsl r1, r0, #(31 - EXC_RETURN_PROCESS_BITNO) /* Move to bit 31 */ bmi 5f /* Test bit 31 */ - mrs r1, msp /* R1=The main stack pointer */ + msr msp, r1 /* R1=The main stack pointer */ b 6f 5: - mrs r1, psp /* R1=The process stack pointer */ + msr psp, r1 /* R1=The process stack pointer */ 6: /* Restore the interrupt state */ diff --git a/nuttx/arch/arm/src/nuc1xx/Kconfig b/nuttx/arch/arm/src/nuc1xx/Kconfig index bc590fbe2..5360f46ca 100644 --- a/nuttx/arch/arm/src/nuc1xx/Kconfig +++ b/nuttx/arch/arm/src/nuc1xx/Kconfig @@ -349,3 +349,49 @@ config NUC_I2S default n endmenu + +config NUC_XTALLO + bool + +config NUC_INTHI + bool + +choice + prompt "SysTick clock source" + default NUC_SYSTICK_XTALHI + +config NUC_SYSTICK_XTALHI + bool "High speed XTAL clock" + +config NUC_SYSTICK_XTALLO + bool "Low speed XTAL clock" + select NUC_XTALLO + +config NUC_SYSTICK_XTALHId2 + bool "High speed XTAL clock/2" + +config NUC_SYSTICK_HCLKd2 + bool "HCLK/2" + +config NUC_SYSTICK_INTHId2 + bool "Internal high speed clock/2" + select NUC_INTHI + +endchoice + +choice + prompt "NUC UART clock source" + default NUC_UARTCLK_INTHI + depends on NUC_UART0 || NUC_UART1 || NUC_UART2 + +config NUC_UARTCLK_XTALHI + bool "External 4-24MHz high speed crystal" + +config NUC_UARTCLK_PLL + bool "PLL output" + +config NUC_UARTCLK_INTHI + bool "Internal 22.1184 high speed clock" + select NUC_INTHI + +endchoice diff --git a/nuttx/arch/arm/src/nuc1xx/chip/nuc_clk.h b/nuttx/arch/arm/src/nuc1xx/chip/nuc_clk.h index 17814a467..426efd782 100644 --- a/nuttx/arch/arm/src/nuc1xx/chip/nuc_clk.h +++ b/nuttx/arch/arm/src/nuc1xx/chip/nuc_clk.h @@ -47,6 +47,11 @@ /******************************************************************************************** * Pre-processor Definitions ********************************************************************************************/ +/* Well-known clock frequencies *************************************************************/ + +#define NUC_INTHI_FREQUENCY 22118400 +#define NUC_INTLO_FREQUENCY 10000 + /* Register offsets *************************************************************************/ #define NUC_CLK_PWRCON_OFFSET 0x0000 /* System power down control register */ diff --git a/nuttx/arch/arm/src/nuc1xx/nuc_clockconfig.c b/nuttx/arch/arm/src/nuc1xx/nuc_clockconfig.c index 500856254..95e4332ce 100644 --- a/nuttx/arch/arm/src/nuc1xx/nuc_clockconfig.c +++ b/nuttx/arch/arm/src/nuc1xx/nuc_clockconfig.c @@ -125,10 +125,18 @@ void nuc_clockconfig(void) nuc_unlock(); - /* Enable External 4~24 mhz high speed crystal */ + /* Enable External 4~24 mhz high speed crystal (And other clocks if needed by + * other drivers). + */ regval = getreg32(NUC_CLK_PWRCON); regval |= CLK_PWRCON_XTL12M_EN; +#ifdef CONFIG_NUC_XTALLO + regval |= CLK_PWRCON_XTL32K_EN; +#endif +#ifdef CONFIG_NUC_INTHI + regval |= CLK_PWRCON_OSC22M_EN; +#endif putreg32(regval, NUC_CLK_PWRCON); /* Delay to assure that crystal input to be stable */ diff --git a/nuttx/arch/arm/src/nuc1xx/nuc_irq.c b/nuttx/arch/arm/src/nuc1xx/nuc_irq.c index 62c17e1a9..ed62e7f43 100644 --- a/nuttx/arch/arm/src/nuc1xx/nuc_irq.c +++ b/nuttx/arch/arm/src/nuc1xx/nuc_irq.c @@ -257,8 +257,12 @@ void up_irqinitialize(void) void up_disable_irq(int irq) { - DEBUGASSERT(irq == NUC_IRQ_SYSTICK || - (irq >= NUC_IRQ_INTERRUPT && irq < NR_IRQS)); + /* This will be called on each interrupt (via up_maskack_irq()) whether + * the interrupt can be disabled or not. So this assertion is necessarily + * lame. + */ + + DEBUGASSERT((unsigned)irq < NR_IRQS); /* Check for an external interrupt */ @@ -291,8 +295,11 @@ void up_disable_irq(int irq) void up_enable_irq(int irq) { - DEBUGASSERT(irq == NUC_IRQ_SYSTICK || - (irq >= NUC_IRQ_INTERRUPT && irq < NR_IRQS)); + /* This will be called on each interrupt exit whether the interrupt can be + * enambled or not. So this assertion is necessarily lame. + */ + + DEBUGASSERT((unsigned)irq < NR_IRQS); /* Check for external interrupt */ diff --git a/nuttx/arch/arm/src/nuc1xx/nuc_lowputc.c b/nuttx/arch/arm/src/nuc1xx/nuc_lowputc.c index 77b744948..ec0c192d2 100644 --- a/nuttx/arch/arm/src/nuc1xx/nuc_lowputc.c +++ b/nuttx/arch/arm/src/nuc1xx/nuc_lowputc.c @@ -82,6 +82,18 @@ # endif #endif +/* Select either the external high speed crystal, the PLL output, or + * the internal high speed clock as the the UART clock source. + */ + +#if defined(CONFIG_NUC_UARTCLK_XTALHI) +# define NUC_UART_CLK BOARD_XTALHI_FREQUENCY +#elif defined(CONFIG_NUC_UARTCLK_PLL) +# define NUC_UART_CLK BOARD_PLL_FOUT +#elif defined(CONFIG_NUC_UARTCLK_INTHI) +# define NUC_UART_CLK NUC_INTHI_FREQUENCY +#endif + /**************************************************************************** * Private Functions ****************************************************************************/ @@ -177,12 +189,22 @@ void nuc_lowsetup(void) putreg32(regval, NUC_GCR_IPRSTC2); #endif - /* Configure the UART clock source - * - * Here we assume that the UART clock source is the external high speed - * crystal -- the power on default value in the CLKSEL0 register. + /* Configure the UART clock source. Set the UART clock source to either + * the external high speed crystal (CLKSEL1 reset value), the PLL output, + * or the internal high speed clock. */ + regval = getreg32(NUC_CLK_CLKSEL1); + regval &= ~CLK_CLKSEL1_UART_S_MASK; +#if defined(CONFIG_NUC_UARTCLK_XTALHI) + regval |= CLK_CLKSEL1_UART_S_XTALHI; +#elif defined(CONFIG_NUC_UARTCLK_PLL) + regval |= CLK_CLKSEL1_UART_S_PLL; +#elif defined(CONFIG_NUC_UARTCLK_INTHI) + regval |= CLK_CLKSEL1_UART_S_INTHI; +#endif + putreg32(regval, NUC_CLK_CLKSEL1); + /* Enable UART clocking for the selected UARTs */ regval = getreg32(NUC_CLK_APBCLK); @@ -206,15 +228,13 @@ void nuc_lowsetup(void) /* Reset the TX FIFO */ - regval = getreg32(NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); - regval |= UART_FCR_TFR; - putreg32(regval, NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); + regval = getreg32(NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); + regval &= ~(UART_FCR_TFR | UART_FCR_RFR); + putreg32(regval | UART_FCR_TFR, NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); /* Reset the RX FIFO */ - regval = getreg32(NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); - regval |= UART_FCR_RFR; - putreg32(regval, NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); + putreg32(regval | UART_FCR_RFR, NUC_CONSOLE_BASE + NUC_UART_FCR_OFFSET); /* Set Rx Trigger Level */ @@ -243,7 +263,6 @@ void nuc_lowsetup(void) regval |= (UART_LCR_PBE | UART_LCR_EPE); #endif - #if NUC_CONSOLE_2STOP != 0 revgval |= UART_LCR_NSB; #endif @@ -292,9 +311,9 @@ void nuc_lowputc(uint32_t ch) * * Mode DIV_X_EN DIV_X_ONE Divider X BRD (Baud rate equation) * ------------------------------------------------------------- - * 0 Disable 0 B A UART_CLK / [16 * (A+2)] - * 1 Enable 0 B A UART_CLK / [(B+1) * (A+2)] , B must >= 8 - * 2 Enable 1 Don't care A UART_CLK / (A+2), A must >=3 + * 0 0 0 B A UART_CLK / [16 * (A+2)] + * 1 1 0 B A UART_CLK / [(B+1) * (A+2)] , B must >= 8 + * 2 1 1 Don't care A UART_CLK / (A+2), A must >=3 * * Here we assume that the default clock source for the UART modules is * the external high speed crystal. @@ -311,9 +330,9 @@ void nuc_setbaud(uintptr_t base, uint32_t baud) regval = getreg32(base + NUC_UART_BAUD_OFFSET); - /* Source Clock mod 16 < 3 => Using Divider X = 16 (MODE#0) */ + /* Mode 0: Source Clock mod 16 < 3 => Using Divider X = 16 */ - clksperbit = BOARD_HIGHSPEED_XTAL_FREQUENCY / baud; + clksperbit = (NUC_UART_CLK + (baud >> 1)) / baud; if ((clksperbit & 15) < 3) { regval &= ~(UART_BAUD_DIV_X_ONE | UART_BAUD_DIV_X_EN); @@ -324,7 +343,7 @@ void nuc_setbaud(uintptr_t base, uint32_t baud) else { - /* Try to Set Divider X = 1 (MODE#2)*/ + /* Mode 2: Try to Set Divider X = 1 */ regval |= (UART_BAUD_DIV_X_ONE | UART_BAUD_DIV_X_EN); brd = clksperbit - 2; @@ -333,13 +352,13 @@ void nuc_setbaud(uintptr_t base, uint32_t baud) if (brd > 0xffff) { - /* Try to Set Divider X up 10 (MODE#1) */ + /* Mode 1: Try to Set Divider X up 10 */ regval &= ~UART_BAUD_DIV_X_ONE; for (divx = 8; divx < 16; divx++) { - brd = (clksperbit % (divx + 1)); + brd = clksperbit % (divx + 1); if (brd < 3) { regval &= ~UART_BAUD_DIVIDER_X_MASK; @@ -355,6 +374,5 @@ void nuc_setbaud(uintptr_t base, uint32_t baud) regval &= ~UART_BAUD_BRD_MASK; regval |= UART_BAUD_BRD(brd); putreg32(regval, base + NUC_UART_BAUD_OFFSET); - } #endif /* HAVE_UART */ diff --git a/nuttx/arch/arm/src/nuc1xx/nuc_timerisr.c b/nuttx/arch/arm/src/nuc1xx/nuc_timerisr.c index c27474601..fdadbf23e 100644 --- a/nuttx/arch/arm/src/nuc1xx/nuc_timerisr.c +++ b/nuttx/arch/arm/src/nuc1xx/nuc_timerisr.c @@ -51,22 +51,33 @@ #include "up_arch.h" #include "chip.h" +#include "chip/nuc_clk.h" /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ +/* Get the frequency of the selected clock source */ + +#if defined(CONFIG_NUC_SYSTICK_XTALHI) +# define SYSTICK_CLOCK BOARD_XTALHI_FREQUENCY /* High speed XTAL clock */ +#elif defined(CONFIG_NUC_SYSTICK_XTALLO) +# define SYSTICK_CLOCK BOARD_XTALLO_FREQUENCY /* Low speed XTAL clock */ +#elif defined(CONFIG_NUC_SYSTICK_XTALHId2) +# define SYSTICK_CLOCK (BOARD_XTALHI_FREQUENCY/2) /* High speed XTAL clock/2 */ +#elif defined(CONFIG_NUC_SYSTICK_HCLKd2) +# define SYSTICK_CLOCK (BOARD_HCLK_FREQUENCY/2) /* HCLK/2 */ +#elif defined(CONFIG_NUC_SYSTICK_INTHId2) +# define SYSTICK_CLOCK (NUC_INTHI_FREQUENCY/2) /* Internal high speed clock/2 */ +#endif /* The desired timer interrupt frequency is provided by the definition * CLK_TCK (see include/time.h). CLK_TCK defines the desired number of * system clock ticks per second. That value is a user configurable setting * that defaults to 100 (100 ticks per second = 10 MS interval). * - * Here we assume that the default clock source for the SysTick is the - * external high speed crystal -- the power-on default value for the - * CLKSEL0 register - * - * Then, for example, if BOARD_HIGHSPEED_XTAL_FREQUENCY is 12MHz and - * CLK_TCK is 100, the the reload value would be: + * Then, for example, if the external high speed crystal is the SysTick + * clock source and BOARD_XTALHI_FREQUENCY is 12MHz and CLK_TCK is 100, then + * the reload value would be: * * SYSTICK_RELOAD = (12,000,000 / 100) - 1 * = 119,999 @@ -75,7 +86,7 @@ * Which fits within the maximum 14-bit reload value. */ -#define SYSTICK_RELOAD ((BOARD_HIGHSPEED_XTAL_FREQUENCY / CLK_TCK) - 1) +#define SYSTICK_RELOAD ((SYSTICK_CLOCK / CLK_TCK) - 1) /* The size of the reload field is 24 bits. Verify that the reload value * will fit in the reload register. @@ -127,6 +138,23 @@ void up_timerinit(void) { uint32_t regval; + /* Configure the SysTick clock source.*/ + + regval = getreg32(NUC_CLK_CLKSEL0); + regval &= ~CLK_CLKSEL0_STCLK_S_MASK; +#if defined(CONFIG_NUC_SYSTICK_XTALHI) + regval |= CLK_CLKSEL0_STCLK_S_XTALHI; /* High speed XTAL clock */ +#elif defined(CONFIG_NUC_SYSTICK_XTALLO) + regval |= CLK_CLKSEL0_STCLK_S_XTALLO; /* Low speed XTAL clock */ +#elif defined(CONFIG_NUC_SYSTICK_XTALHId2) + regval |= CLK_CLKSEL0_STCLK_S_XTALDIV2; /* High speed XTAL clock/2 */ +#elif defined(CONFIG_NUC_SYSTICK_HCLKd2) + regval |= CLK_CLKSEL0_STCLK_S_HCLKDIV2; /* HCLK/2 */ +#elif defined(CONFIG_NUC_SYSTICK_INTHId2) + regval |= CLK_CLKSEL0_STCLK_S_INTDIV2; /* Internal high speed clock/2 */ +#endif + putreg32(regval, NUC_CLK_CLKSEL0); + /* Set the SysTick interrupt to the default priority */ regval = getreg32(ARMV6M_SYSCON_SHPR3); -- cgit v1.2.3