From 208a3002e7832108a71978aa8a1afe712f5e7f17 Mon Sep 17 00:00:00 2001 From: Giovanni Di Sirio Date: Fri, 22 Sep 2017 08:06:33 +0000 Subject: More GHS stuff. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@10666 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- demos/SPC5/RT-SPC56EC-EVB/Makefile_ghs | 10 +- os/common/ports/e200/chcore.h | 51 ++- os/common/ports/e200/compilers/GHS/chcoreasm.s | 113 ++++++ os/common/ports/e200/compilers/GHS/chtypes.h | 115 ++++++ os/common/ports/e200/compilers/GHS/ivor.s | 265 ++++++++++++++ os/common/ports/e200/compilers/GHS/mk/port.mk | 8 + os/common/startup/e200/compilers/GHS/crt0.s | 8 +- .../e200/compilers/GHS/mk/startup_spc560bcxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc560bxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc560dxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc560pxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc563mxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc564axx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc56ecxx.mk | 6 +- .../e200/compilers/GHS/mk/startup_spc56elxx.mk | 6 +- os/common/startup/e200/compilers/GHS/vectors.s | 6 +- .../startup/e200/devices/SPC56ECxx/boot_ghs.s | 405 +++++++++++++++++++++ 17 files changed, 989 insertions(+), 40 deletions(-) create mode 100644 os/common/ports/e200/compilers/GHS/chcoreasm.s create mode 100644 os/common/ports/e200/compilers/GHS/chtypes.h create mode 100644 os/common/ports/e200/compilers/GHS/ivor.s create mode 100644 os/common/ports/e200/compilers/GHS/mk/port.mk create mode 100644 os/common/startup/e200/devices/SPC56ECxx/boot_ghs.s diff --git a/demos/SPC5/RT-SPC56EC-EVB/Makefile_ghs b/demos/SPC5/RT-SPC56EC-EVB/Makefile_ghs index 36dcd7a94..5fa7c22d6 100644 --- a/demos/SPC5/RT-SPC56EC-EVB/Makefile_ghs +++ b/demos/SPC5/RT-SPC56EC-EVB/Makefile_ghs @@ -83,7 +83,7 @@ PROJECT = ch # Imported source files and paths CHIBIOS = ../../.. # Startup files. -include $(CHIBIOS)/os/common/startup/e200/compilers/GCC/mk/startup_spc56ecxx.mk +include $(CHIBIOS)/os/common/startup/e200/compilers/GHS/mk/startup_spc56ecxx.mk # HAL-OSAL files (optional). include $(CHIBIOS)/os/hal/hal.mk include $(CHIBIOS)/os/hal/boards/ST_EVB_SPC56EC/board.mk @@ -91,7 +91,7 @@ include $(CHIBIOS)/os/hal/ports/SPC5/SPC56ECxx/platform.mk include $(CHIBIOS)/os/hal/osal/rt/osal.mk # RTOS files (optional). include $(CHIBIOS)/os/rt/rt.mk -include $(CHIBIOS)/os/common/ports/e200/compilers/GCC/mk/port.mk +include $(CHIBIOS)/os/common/ports/e200/compilers/GHS/mk/port.mk # Other files (optional). include $(CHIBIOS)/test/rt/test.mk include $(CHIBIOS)/os/hal/lib/streams/streams.mk @@ -117,8 +117,8 @@ CSRC = $(STARTUPSRC) \ CPPSRC = # List ASM source files here -ASMSRC = -ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) +ASMSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) +ASMXSRC = INCDIR = $(CHIBIOS)/os/license \ $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \ @@ -133,7 +133,7 @@ INCDIR = $(CHIBIOS)/os/license \ # Compiler settings # -MCU = ppc564xcz0 +MCU = ppc564xc TRGT = CC = $(TRGT)ccppc diff --git a/os/common/ports/e200/chcore.h b/os/common/ports/e200/chcore.h index a58ce6ce8..f3f50364d 100644 --- a/os/common/ports/e200/chcore.h +++ b/os/common/ports/e200/chcore.h @@ -28,6 +28,10 @@ #ifndef CHCORE_H #define CHCORE_H +#if defined(__ghs__) && !defined(_FROM_ASM_) +#include +#endif + #include "intc.h" /*===========================================================================*/ @@ -90,6 +94,9 @@ #elif defined(__MWERKS__) #define PORT_COMPILER_NAME "CW" +#elif defined(__ghs__) +#define PORT_COMPILER_NAME "GHS " __VERSION__ + #else #error "unsupported compiler" #endif @@ -444,8 +451,13 @@ struct port_context { * @param[in] spr special register number * @param[in] val value to be written, must be an automatic variable */ +#if !defined(__ghs__) || defined(__DOXYGEN__) #define port_write_spr(spr, val) \ asm volatile ("mtspr %[p0], %[p1]" : : [p0] "n" (spr), [p1] "r" (val)) +#else +#define port_write_spr(spr, val) \ + __MTSPR(spr, val); +#endif /** * @brief Reads a special register. @@ -453,8 +465,13 @@ struct port_context { * @param[in] spr special register number * @param[in] val returned value, must be an automatic variable */ +#if !defined(__ghs__) || defined(__DOXYGEN__) #define port_read_spr(spr, val) \ asm volatile ("mfspr %[p0], %[p1]" : [p0] "=r" (val) : [p1] "n" (spr)) +#else +#define port_read_spr(spr, val) \ + val = __MFSPR(spr) +#endif /*===========================================================================*/ /* External declarations. */ @@ -483,6 +500,9 @@ extern "C" { asm module.*/ #if !defined(_FROM_ASM_) +extern void _IVOR4(void); +extern void _IVOR10(void); + /** * @brief Kernel port layer initialization. * @details IVOR4 and IVOR10 initialization. @@ -500,12 +520,10 @@ static inline void port_init(void) { { /* The CPU supports IVOR registers, the kernel requires IVOR4 and IVOR10 and the initialization is performed here.*/ - extern void _IVOR4(void); - port_write_spr(404, _IVOR4); + port_write_spr(404, (uint32_t)_IVOR4); #if PPC_SUPPORTS_DECREMENTER - extern void _IVOR10(void); - port_write_spr(410, _IVOR10); + port_write_spr(410, (uint32_t)_IVOR10); #endif } #endif @@ -527,7 +545,12 @@ static inline void port_init(void) { static inline syssts_t port_get_irq_status(void) { uint32_t sts; +#if defined(__ghs__) + sts = __GETSR(); +#else asm volatile ("mfmsr %[p0]" : [p0] "=r" (sts) :); +#endif + return sts; } @@ -567,7 +590,11 @@ static inline bool port_is_isr_context(void) { */ static inline void port_lock(void) { +#if defined(__ghs__) + __DI(); +#else asm volatile ("wrteei 0" : : : "memory"); +#endif } /** @@ -576,7 +603,11 @@ static inline void port_lock(void) { */ static inline void port_unlock(void) { +#if defined(__ghs__) + __EI(); +#else asm volatile("wrteei 1" : : : "memory"); +#endif } /** @@ -601,7 +632,11 @@ static inline void port_unlock_from_isr(void) { */ static inline void port_disable(void) { +#if defined(__ghs__) + __DI(); +#else asm volatile ("wrteei 0" : : : "memory"); +#endif } /** @@ -611,7 +646,11 @@ static inline void port_disable(void) { */ static inline void port_suspend(void) { +#if defined(__ghs__) + __DI(); +#else asm volatile ("wrteei 0" : : : "memory"); +#endif } /** @@ -620,7 +659,11 @@ static inline void port_suspend(void) { */ static inline void port_enable(void) { +#if defined(__ghs__) + __EI(); +#else asm volatile ("wrteei 1" : : : "memory"); +#endif } /** diff --git a/os/common/ports/e200/compilers/GHS/chcoreasm.s b/os/common/ports/e200/compilers/GHS/chcoreasm.s new file mode 100644 index 000000000..36324be2f --- /dev/null +++ b/os/common/ports/e200/compilers/GHS/chcoreasm.s @@ -0,0 +1,113 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file e200/compilers/GHS/chcoreasm.S + * @brief Power Architecture port low level code. + * + * @addtogroup PPC_GHS_CORE + * @{ + */ + +/*===========================================================================*/ +/* Module constants. */ +/*===========================================================================*/ + +#if !defined(FALSE) || defined(__DOXYGEN__) +#define FALSE 0 +#endif + +#if !defined(TRUE) || defined(__DOXYGEN__) +#define TRUE 1 +#endif + +/*===========================================================================*/ +/* Code section. */ +/*===========================================================================*/ + +#define _FROM_ASM_ +#include "chlicense.h" +#include "chconf.h" +#include "chcore.h" + +#if !defined(__DOXYGEN__) + +/* + * RTOS-specific context offset. + */ +#if defined(_CHIBIOS_RT_CONF_) +#define CONTEXT_OFFSET 12 +#elif defined(_CHIBIOS_NIL_CONF_) +#define CONTEXT_OFFSET 0 +#else +#error "invalid chconf.h" +#endif + + .vle + + .section .text_vle, "axv" + + .align 2 + .globl _port_switch + .type _port_switch, @function +_port_switch: + e_subi sp, sp, 80 + mflr r0 + e_stw r0, 84(sp) + mfcr r0 + se_stw r0, 0(sp) + e_stmw r14, 4(sp) + + se_stw sp, CONTEXT_OFFSET(r4) + se_lwz sp, CONTEXT_OFFSET(r3) + + e_lmw r14, 4(sp) + se_lwz r0, 0(sp) + mtcr r0 + e_lwz r0, 84(sp) + mtlr r0 + e_addi sp, sp, 80 + se_blr + + .align 2 + .globl _port_thread_start + .type _port_thread_start, @function +_port_thread_start: +#if CH_DBG_SYSTEM_STATE_CHECK + e_bl _dbg_check_unlock +#endif +#if CH_DBG_STATISTICS + e_bl _stats_stop_measure_crit_thd +#endif + wrteei 1 + mr r3, r31 + mtctr r30 + se_bctrl +#if defined(_CHIBIOS_RT_CONF_) + e_li r0, 0 + e_bl chThdExit +#endif +#if defined(_CHIBIOS_NIL_CONF_) + se_li r0, 0 + e_bl chSysHalt +#endif + +#endif /* !defined(__DOXYGEN__) */ + +/** @} */ diff --git a/os/common/ports/e200/compilers/GHS/chtypes.h b/os/common/ports/e200/compilers/GHS/chtypes.h new file mode 100644 index 000000000..20a601e93 --- /dev/null +++ b/os/common/ports/e200/compilers/GHS/chtypes.h @@ -0,0 +1,115 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file e200/compilers/GCC/chtypes.h + * @brief Power e200 port system types. + * + * @addtogroup PPC_GCC_CORE + * @{ + */ + +#ifndef CHTYPES_H +#define CHTYPES_H + +#include +#include +#include + +/** + * @name Common constants + */ +/** + * @brief Generic 'false' boolean constant. + */ +#if !defined(FALSE) || defined(__DOXYGEN__) +#define FALSE 0 +#endif + +/** + * @brief Generic 'true' boolean constant. + */ +#if !defined(TRUE) || defined(__DOXYGEN__) +#define TRUE 1 +#endif +/** @} */ + +/** + * @name Kernel types + * @{ + */ +typedef uint32_t rtcnt_t; /**< Realtime counter. */ +typedef uint64_t rttime_t; /**< Realtime accumulator. */ +typedef uint32_t syssts_t; /**< System status word. */ +typedef uint8_t tmode_t; /**< Thread flags. */ +typedef uint8_t tstate_t; /**< Thread state. */ +typedef uint8_t trefs_t; /**< Thread references counter. */ +typedef uint8_t tslices_t; /**< Thread time slices counter.*/ +typedef uint32_t tprio_t; /**< Thread priority. */ +typedef int32_t msg_t; /**< Inter-thread message. */ +typedef int32_t eventid_t; /**< Numeric event identifier. */ +typedef uint32_t eventmask_t; /**< Mask of event identifiers. */ +typedef uint32_t eventflags_t; /**< Mask of event flags. */ +typedef int32_t cnt_t; /**< Generic signed counter. */ +typedef uint32_t ucnt_t; /**< Generic unsigned counter. */ +/** @} */ + +/** + * @brief ROM constant modifier. + * @note It is set to use the "const" keyword in this port. + */ +#define ROMCONST const + +/** + * @brief Makes functions not inlineable. + * @note If the compiler does not support such attribute then some + * time-dependent services could be degraded. + */ +#define NOINLINE __attribute__((noinline)) + +/** + * @brief Optimized thread function declaration macro. + */ +#define PORT_THD_FUNCTION(tname, arg) void tname(void *arg) + +/** + * @brief Packed variable specifier. + */ +#define PACKED_VAR __attribute__((packed)) + +/** + * @brief Memory alignment enforcement for variables. + */ +#define ALIGNED_VAR(n) __attribute__((aligned(n))) + +/** + * @brief Size of a pointer. + * @note To be used where the sizeof operator cannot be used, preprocessor + * expressions for example. + */ +#define SIZEOF_PTR 4 + +/** + * @brief True if alignment is low-high in current architecture. + */ +#define REVERSE_ORDER 0 + +#endif /* CHTYPES_H */ + +/** @} */ diff --git a/os/common/ports/e200/compilers/GHS/ivor.s b/os/common/ports/e200/compilers/GHS/ivor.s new file mode 100644 index 000000000..01aaceed6 --- /dev/null +++ b/os/common/ports/e200/compilers/GHS/ivor.s @@ -0,0 +1,265 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio. + + This file is part of ChibiOS. + + ChibiOS is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + ChibiOS is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/** + * @file e200/compilers/GHS/ivor.S + * @brief Kernel ISRs. + * + * @addtogroup PPC_GHS_CORE + * @{ + */ + +#if !defined(FALSE) || defined(__DOXYGEN__) +#define FALSE 0 +#endif + +#if !defined(TRUE) || defined(__DOXYGEN__) +#define TRUE 1 +#endif + +/* + * Imports the PPC configuration headers. + */ +#define _FROM_ASM_ +#include "chlicense.h" +#include "chconf.h" +#include "chcore.h" + +#if defined(__HIGHTEC__) +#define se_beq beq +#endif + +#if !defined(__DOXYGEN__) + + .vle + + .section .handlers, "axv" + +#if PPC_SUPPORTS_DECREMENTER + /* + * _IVOR10 handler (Book-E decrementer). + */ + .align 4 + .globl _IVOR10 + .type _IVOR10, @function +_IVOR10: + /* Saving the external context (port_extctx structure).*/ + e_stwu sp, -80(sp) +#if PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI + e_stmvsrrw 8(sp) /* Saves PC, MSR. */ + e_stmvsprw 16(sp) /* Saves CR, LR, CTR, XER. */ + e_stmvgprw 32(sp) /* Saves GPR0, GPR3...GPR12. */ +#else /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + se_stw r0, 32(sp) /* Saves GPR0. */ + mfSRR0 r0 + se_stw r0, 8(sp) /* Saves PC. */ + mfSRR1 r0 + se_stw r0, 12(sp) /* Saves MSR. */ + mfCR r0 + se_stw r0, 16(sp) /* Saves CR. */ + mfLR r0 + se_stw r0, 20(sp) /* Saves LR. */ + mfCTR r0 + se_stw r0, 24(sp) /* Saves CTR. */ + mfXER r0 + se_stw r0, 28(sp) /* Saves XER. */ + se_stw r3, 36(sp) /* Saves GPR3...GPR12. */ + se_stw r4, 40(sp) + se_stw r5, 44(sp) + se_stw r6, 48(sp) + se_stw r7, 52(sp) + e_stw r8, 56(sp) + e_stw r9, 60(sp) + e_stw r10, 64(sp) + e_stw r11, 68(sp) + e_stw r12, 72(sp) +#endif /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + + /* Increasing the SPGR0 register.*/ + mfspr r0, 272 + se_addi r0, 1 + mtspr 272, r0 + + /* Reset DIE bit in TSR register.*/ + e_lis r3, 0x0800 /* DIS bit mask. */ + mtspr 336, r3 /* TSR register. */ + + /* Restoring pre-IRQ MSR register value.*/ + mfSRR1 r0 +#if !PPC_USE_IRQ_PREEMPTION + /* No preemption, keeping EE disabled.*/ + se_bclri r0, 16 /* EE = bit 16. */ +#endif + mtMSR r0 + +#if CH_DBG_SYSTEM_STATE_CHECK + e_bl _dbg_check_enter_isr + e_bl _dbg_check_lock_from_isr +#endif + /* System tick handler invocation.*/ + e_bl chSysTimerHandlerI +#if CH_DBG_SYSTEM_STATE_CHECK + e_bl _dbg_check_unlock_from_isr + e_bl _dbg_check_leave_isr +#endif + +#if PPC_USE_IRQ_PREEMPTION + /* Prevents preemption again.*/ + wrteei 0 +#endif + + /* Jumps to the common IVOR epilogue code.*/ + e_b _ivor_exit +#endif /* PPC_SUPPORTS_DECREMENTER */ + + /* + * _IVOR4 handler (Book-E external interrupt). + */ + .align 4 + .globl _IVOR4 + .type _IVOR4, @function +_IVOR4: + /* Saving the external context (port_extctx structure).*/ + e_stwu sp, -80(sp) +#if PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI + e_stmvsrrw 8(sp) /* Saves PC, MSR. */ + e_stmvsprw 16(sp) /* Saves CR, LR, CTR, XER. */ + e_stmvgprw 32(sp) /* Saves GPR0, GPR3...GPR12. */ +#else /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + se_stw r0, 32(sp) /* Saves GPR0. */ + mfSRR0 r0 + se_stw r0, 8(sp) /* Saves PC. */ + mfSRR1 r0 + se_stw r0, 12(sp) /* Saves MSR. */ + mfCR r0 + se_stw r0, 16(sp) /* Saves CR. */ + mfLR r0 + se_stw r0, 20(sp) /* Saves LR. */ + mfCTR r0 + se_stw r0, 24(sp) /* Saves CTR. */ + mfXER r0 + se_stw r0, 28(sp) /* Saves XER. */ + se_stw r3, 36(sp) /* Saves GPR3...GPR12. */ + se_stw r4, 40(sp) + se_stw r5, 44(sp) + se_stw r6, 48(sp) + se_stw r7, 52(sp) + e_stw r8, 56(sp) + e_stw r9, 60(sp) + e_stw r10, 64(sp) + e_stw r11, 68(sp) + e_stw r12, 72(sp) +#endif /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + + /* Increasing the SPGR0 register.*/ + mfspr r0, 272 + se_addi r0, 1 + mtspr 272, r0 + + /* Software vector address from the INTC register.*/ + e_lis r3, INTC_IACKR_ADDR@h + e_or2i r3, INTC_IACKR_ADDR@l /* IACKR register address. */ + e_lwz r3, 0(r3) /* IACKR register value. */ + e_lwz r3, 0(r3) + mtCTR r3 /* Software handler address. */ + + /* Restoring pre-IRQ MSR register value.*/ + mfSRR1 r0 +#if !PPC_USE_IRQ_PREEMPTION + /* No preemption, keeping EE disabled.*/ + se_bclri r0, 16 /* EE = bit 16. */ +#endif + mtMSR r0 + + /* Exectes the software handler.*/ + se_bctrl + +#if PPC_USE_IRQ_PREEMPTION + /* Prevents preemption again.*/ + wrteei 0 +#endif + + /* Informs the INTC that the interrupt has been served.*/ + mbar 0 + e_lis r3, INTC_EOIR_ADDR@h + e_or2i r3, INTC_EOIR_ADDR@l + se_stw r3, 0(r3) /* Writing any value should do. */ + + /* Common IVOR epilogue code, context restore.*/ + .globl _ivor_exit +_ivor_exit: + /* Decreasing the SPGR0 register.*/ + mfspr r0, 272 + se_subi r0, 1 + mtspr 272, r0 + +#if CH_DBG_STATISTICS + e_bl _stats_start_measure_crit_thd +#endif +#if CH_DBG_SYSTEM_STATE_CHECK + e_bl _dbg_check_lock +#endif + e_bl chSchIsPreemptionRequired + se_cmpi r3, 0 + se_beq .noresch + e_bl chSchDoReschedule +.noresch: +#if CH_DBG_SYSTEM_STATE_CHECK + e_bl _dbg_check_unlock +#endif +#if CH_DBG_STATISTICS + e_bl _stats_stop_measure_crit_thd +#endif + + /* Restoring the external context.*/ +#if PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI + e_lmvgprw 32(sp) /* Restores GPR0, GPR3...GPR12. */ + e_lmvsprw 16(sp) /* Restores CR, LR, CTR, XER. */ + e_lmvsrrw 8(sp) /* Restores PC, MSR. */ +#else /*!(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + se_lwz r3, 36(sp) /* Restores GPR3...GPR12. */ + se_lwz r4, 40(sp) + se_lwz r5, 44(sp) + se_lwz r6, 48(sp) + se_lwz r7, 52(sp) + e_lwz r8, 56(sp) + e_lwz r9, 60(sp) + e_lwz r10, 64(sp) + e_lwz r11, 68(sp) + e_lwz r12, 72(sp) + se_lwz r0, 8(sp) + mtSRR0 r0 /* Restores PC. */ + se_lwz r0, 12(sp) + mtSRR1 r0 /* Restores MSR. */ + se_lwz r0, 16(sp) + mtCR r0 /* Restores CR. */ + se_lwz r0, 20(sp) + mtLR r0 /* Restores LR. */ + se_lwz r0, 24(sp) + mtCTR r0 /* Restores CTR. */ + se_lwz r0, 28(sp) + mtXER r0 /* Restores XER. */ + se_lwz r0, 32(sp) /* Restores GPR0. */ +#endif /* !(PPC_USE_VLE && PPC_SUPPORTS_VLE_MULTI) */ + e_addi sp, sp, 80 /* Back to the previous frame. */ + se_rfi + +#endif /* !defined(__DOXYGEN__) */ + +/** @} */ diff --git a/os/common/ports/e200/compilers/GHS/mk/port.mk b/os/common/ports/e200/compilers/GHS/mk/port.mk new file mode 100644 index 000000000..8bab3110b --- /dev/null +++ b/os/common/ports/e200/compilers/GHS/mk/port.mk @@ -0,0 +1,8 @@ +# List of the ChibiOS/RT e200 generic port files. +PORTSRC = $(CHIBIOS)/os/common/ports/e200/chcore.c + +PORTASM = $(CHIBIOS)/os/common/ports/e200/compilers/GHS/ivor.s \ + $(CHIBIOS)/os/common/ports/e200/compilers/GHS/chcoreasm.s + +PORTINC = $(CHIBIOS)/os/common/ports/e200 \ + $(CHIBIOS)/os/common/ports/e200/compilers/GHS diff --git a/os/common/startup/e200/compilers/GHS/crt0.s b/os/common/startup/e200/compilers/GHS/crt0.s index 7c4b47bd4..cd549b98f 100644 --- a/os/common/startup/e200/compilers/GHS/crt0.s +++ b/os/common/startup/e200/compilers/GHS/crt0.s @@ -34,10 +34,6 @@ #define TRUE 1 #endif -#if defined(__HIGHTEC__) -#define se_bge bge -#endif - /*===========================================================================*/ /* Module pre-compile time settings. */ /*===========================================================================*/ @@ -90,7 +86,9 @@ #if !defined(__DOXYGEN__) - .section .crt0, "ax" + .vle + + .section .crt0, "axv" .align 2 .globl _boot_address .type _boot_address, @function diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc560bcxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc560bcxx.mk index c69c7b88f..e271c121e 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc560bcxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc560bcxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z0 SPC560BCxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560BCxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560BCxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC560BCxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc560bxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc560bxx.mk index aba451bee..fc0d81d50 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc560bxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc560bxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z0 SPC560Bxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Bxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Bxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC560Bxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc560dxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc560dxx.mk index 3824070a8..8b53117b2 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc560dxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc560dxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z0 SPC560Dxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Dxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Dxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC560Dxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc560pxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc560pxx.mk index 4766491f7..93942cd3a 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc560pxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc560pxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z0 SPC560Pxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Pxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC560Pxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC560Pxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc563mxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc563mxx.mk index 83d12374a..7bf28e2d3 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc563mxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc563mxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z3 SPC563Mxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC563Mxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC563Mxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC563Mxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc564axx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc564axx.mk index db968e0e9..6702f51a1 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc564axx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc564axx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z4 SPC564Axx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC564Axx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC564Axx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC564Axx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc56ecxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc56ecxx.mk index 93cf96bfc..7a5b74b0e 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc56ecxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc56ecxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z4 SPC56ECxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC56ECxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC56ECxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC56ECxx diff --git a/os/common/startup/e200/compilers/GHS/mk/startup_spc56elxx.mk b/os/common/startup/e200/compilers/GHS/mk/startup_spc56elxx.mk index 80603c948..775eb6398 100644 --- a/os/common/startup/e200/compilers/GHS/mk/startup_spc56elxx.mk +++ b/os/common/startup/e200/compilers/GHS/mk/startup_spc56elxx.mk @@ -1,9 +1,9 @@ # List of the ChibiOS e200z4 SPC56ELxx startup files. STARTUPSRC = -STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC56ELxx/boot.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.S \ - $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.S +STARTUPASM = $(CHIBIOS)/os/common/startup/e200/devices/SPC56ELxx/boot_ghs.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/vectors.s \ + $(CHIBIOS)/os/common/startup/e200/compilers/GHS/crt0.s STARTUPINC = ${CHIBIOS}/os/common/startup/e200/compilers/GHS \ ${CHIBIOS}/os/common/startup/e200/devices/SPC56ELxx diff --git a/os/common/startup/e200/compilers/GHS/vectors.s b/os/common/startup/e200/compilers/GHS/vectors.s index 251d1e935..ba39a0230 100644 --- a/os/common/startup/e200/compilers/GHS/vectors.s +++ b/os/common/startup/e200/compilers/GHS/vectors.s @@ -31,11 +31,13 @@ #if !defined(__DOXYGEN__) + .vle + /* Software vectors table. The vectors are accessed from the IVOR4 handler only. In order to declare an interrupt handler just create a function withe the same name of a vector, the symbol will override the weak symbol declared here.*/ - .section .vectors, "ax" + .section .vectors, "axv" .align 4 .globl _vectors _vectors: @@ -806,7 +808,7 @@ _vectors: .long vector1020, vector1021, vector1022, vector1023 #endif - .text + .section .vletext, "axv" .align 2 .weak vector0, vector1, vector2, vector3 diff --git a/os/common/startup/e200/devices/SPC56ECxx/boot_ghs.s b/os/common/startup/e200/devices/SPC56ECxx/boot_ghs.s new file mode 100644 index 000000000..f4a6fa55e --- /dev/null +++ b/os/common/startup/e200/devices/SPC56ECxx/boot_ghs.s @@ -0,0 +1,405 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/** + * @file SPC56ECxx/boot_ghs.s + * @brief SPC56ECxx boot-related code. + * + * @addtogroup PPC_BOOT + * @{ + */ + +#include "boot.h" + +#if !defined(__DOXYGEN__) + + .vle + + /* BAM record.*/ + .section .boot, "axv" + +#if BOOT_USE_VLE + .long 0x015A0000 +#else + .long 0x005A0000 +#endif + .long _reset_address + + .align 2 + .globl _reset_address + .type _reset_address, @function +_reset_address: +#if BOOT_PERFORM_CORE_INIT + se_bl _coreinit +#endif + se_bl _ivinit + +#if BOOT_RELOCATE_IN_RAM + /* + * Image relocation in RAM. + */ + e_lis r4, __ram_reloc_start__@h + e_or2i r4, __ram_reloc_start__@l + e_lis r5, __ram_reloc_dest__@h + e_or2i r5, __ram_reloc_dest__@l + e_lis r6, __ram_reloc_end__@h + e_or2i r6, r6, __ram_reloc_end__@l +.relloop: + se_cmpl r4, r6 + se_bge .relend + se_lwz r7, 0(r4) + se_addi r4, 4 + se_stw r7, 0(r5) + se_addi r5, 4 + se_b .relloop +.relend: + e_lis r3, _boot_address@h + e_or2i r3, _boot_address@l + mtctr r3 + se_bctrl +#else + e_b _boot_address +#endif + +#if BOOT_PERFORM_CORE_INIT + .align 2 +_ramcode: + tlbwe + se_isync + se_blr + + .align 2 +_coreinit: + /* + * Invalidating all TLBs except TLB0. + */ + e_lis r3, 0 + mtspr 625, r3 /* MAS1 */ + mtspr 626, r3 /* MAS2 */ + mtspr 627, r3 /* MAS3 */ + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(1))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(2))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(3))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(4))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(5))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(6))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(7))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(8))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(9))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(10))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(11))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(12))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(13))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(14))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + e_lis r3, (MAS0_TBLMAS_TBL | MAS0_ESEL(15))@h + mtspr 624, r3 /* MAS0 */ + tlbwe + + /* + * TLB1 allocated to internal RAM. + */ + e_lis r3, TLB1_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB1_MAS1@h + e_or2i r3, TLB1_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB1_MAS2@h + e_or2i r3, TLB1_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB1_MAS3@h + e_or2i r3, TLB1_MAS3@l + mtspr 627, r3 /* MAS3 */ + tlbwe + + /* + * TLB2 allocated to internal Peripherals Bridge A. + */ + e_lis r3, TLB2_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB2_MAS1@h + e_or2i r3, TLB2_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB2_MAS2@h + e_or2i r3, TLB2_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB2_MAS3@h + e_or2i r3, TLB2_MAS3@l + mtspr 627, r3 /* MAS3 */ + tlbwe + + /* + * TLB3 allocated to internal Peripherals Bridge B. + */ + e_lis r3, TLB3_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB3_MAS1@h + e_or2i r3, TLB3_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB3_MAS2@h + e_or2i r3, TLB3_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB3_MAS3@h + e_or2i r3, TLB3_MAS3@l + mtspr 627, r3 /* MAS3 */ + tlbwe + + /* + * TLB4 allocated to on-platform peripherals. + */ + e_lis r3, TLB4_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB4_MAS1@h + e_or2i r3, TLB4_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB4_MAS2@h + e_or2i r3, TLB4_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB4_MAS3@h + e_or2i r3, TLB4_MAS3@l + mtspr 627, r3 /* MAS3 */ + tlbwe + + /* + * TLB5 allocated to on-platform peripherals. + */ + e_lis r3, TLB5_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB5_MAS1@h + e_or2i r3, TLB5_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB5_MAS2@h + e_or2i r3, TLB5_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB5_MAS3@h + e_or2i r3, TLB5_MAS3@l + mtspr 627, r3 /* MAS3 */ + tlbwe + + /* + * RAM clearing, this device requires a write to all RAM location in + * order to initialize the ECC detection hardware, this is going to + * slow down the startup but there is no way around. + */ + xor r0, r0, r0 + xor r1, r1, r1 + xor r2, r2, r2 + xor r3, r3, r3 + xor r4, r4, r4 + xor r5, r5, r5 + xor r6, r6, r6 + xor r7, r7, r7 + xor r8, r8, r8 + xor r9, r9, r9 + xor r10, r10, r10 + xor r11, r11, r11 + xor r12, r12, r12 + xor r13, r13, r13 + xor r14, r14, r14 + xor r15, r15, r15 + xor r16, r16, r16 + xor r17, r17, r17 + xor r18, r18, r18 + xor r19, r19, r19 + xor r20, r20, r20 + xor r21, r21, r21 + xor r22, r22, r22 + xor r23, r23, r23 + xor r24, r24, r24 + xor r25, r25, r25 + xor r26, r26, r26 + xor r27, r27, r27 + xor r28, r28, r28 + xor r29, r29, r29 + xor r30, r30, r30 + xor r31, r31, r31 + e_lis r4, __ram_start__@h + e_or2i r4, __ram_start__@l + e_lis r5, __ram_end__@h + e_or2i r5, __ram_end__@l +.cleareccloop: + se_cmpl r4, r5 + se_bge .cleareccend + e_stmw r16, 0(r4) + e_addi r4, r4, 64 + se_b .cleareccloop +.cleareccend: + + /* + * Special function registers clearing, required in order to avoid + * possible problems with lockstep mode. + */ + mtcrf 0xFF, r31 + mtspr 9, r31 /* CTR */ + mtspr 22, r31 /* DEC */ + mtspr 26, r31 /* SRR0-1 */ + mtspr 27, r31 + mtspr 54, r31 /* DECAR */ + mtspr 58, r31 /* CSRR0-1 */ + mtspr 59, r31 + mtspr 61, r31 /* DEAR */ + mtspr 256, r31 /* USPRG0 */ + mtspr 272, r31 /* SPRG1-7 */ + mtspr 273, r31 + mtspr 274, r31 + mtspr 275, r31 + mtspr 276, r31 + mtspr 277, r31 + mtspr 278, r31 + mtspr 279, r31 + mtspr 285, r31 /* TBU */ + mtspr 284, r31 /* TBL */ +#if 0 + mtspr 318, r31 /* DVC1-2 */ + mtspr 319, r31 +#endif + mtspr 562, r31 /* DBCNT */ + mtspr 570, r31 /* MCSRR0 */ + mtspr 571, r31 /* MCSRR1 */ + mtspr 604, r31 /* SPRG8-9 */ + mtspr 605, r31 + + /* + * *Finally* the TLB0 is re-allocated to flash, note, the final phase + * is executed from RAM. + */ + e_lis r3, TLB0_MAS0@h + mtspr 624, r3 /* MAS0 */ + e_lis r3, TLB0_MAS1@h + e_or2i r3, TLB0_MAS1@l + mtspr 625, r3 /* MAS1 */ + e_lis r3, TLB0_MAS2@h + e_or2i r3, TLB0_MAS2@l + mtspr 626, r3 /* MAS2 */ + e_lis r3, TLB0_MAS3@h + e_or2i r3, TLB0_MAS3@l + mtspr 627, r3 /* MAS3 */ + mflr r4 + e_lis r6, _ramcode@h + e_or2i r6, _ramcode@l + e_lis r7, 0x40010000@h + mtctr r7 + se_lwz r3, 0(r6) + se_stw r3, 0(r7) + se_lwz r3, 4(r6) + se_stw r3, 4(r7) + se_lwz r3, 8(r6) + se_stw r3, 8(r7) + se_bctrl + mtlr r4 + + /* + * Branch prediction enabled. + */ + e_li r3, BOOT_BUCSR_DEFAULT + mtspr 1013, r3 /* BUCSR */ + + /* + * Cache invalidated and then enabled. + */ + e_li r3, LICSR1_ICINV + mtspr 1011, r3 /* LICSR1 */ +.inv: mfspr r3, 1011 /* LICSR1 */ + e_and2i. r3, LICSR1_ICINV + se_bne .inv + e_lis r3, BOOT_LICSR1_DEFAULT@h + e_or2i r3, BOOT_LICSR1_DEFAULT@l + mtspr 1011, r3 /* LICSR1 */ + + se_blr +#endif /* BOOT_PERFORM_CORE_INIT */ + + /* + * Exception vectors initialization. + */ + .align 2 +_ivinit: + /* MSR initialization.*/ + e_lis r3, BOOT_MSR_DEFAULT@h + e_or2i r3, BOOT_MSR_DEFAULT@l + mtMSR r3 + + /* IVPR initialization.*/ + e_lis r3, __ivpr_base__@h + e_or2i r3, __ivpr_base__@l + mtIVPR r3 + + /* IVORs initialization.*/ + e_lis r3, _unhandled_exception@h + e_or2i r3, _unhandled_exception@l + + mtspr 400, r3 /* IVOR0-15 */ + mtspr 401, r3 + mtspr 402, r3 + mtspr 403, r3 + mtspr 404, r3 + mtspr 405, r3 + mtspr 406, r3 + mtspr 407, r3 + mtspr 408, r3 + mtspr 409, r3 + mtspr 410, r3 + mtspr 411, r3 + mtspr 412, r3 + mtspr 413, r3 + mtspr 414, r3 + mtspr 415, r3 + mtspr 528, r3 /* IVOR32-34 */ + mtspr 529, r3 + mtspr 530, r3 + + se_blr + + .section .handlers, "axv" + + /* + * Unhandled exceptions handler. + */ + .weak _unhandled_exception + .type _unhandled_exception, @function +_unhandled_exception: + se_b _unhandled_exception + +#endif /* !defined(__DOXYGEN__) */ + +/** @} */ -- cgit v1.2.3