--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * \brief System IRQ handler for Atmel AT91 ARM7 processors.
+ *
+ * In Atmel AT91 ARM7TDMI processors, there are various
+ * peripheral interrupt sources.
+ * In general, every source has its own interrupt vector, so it
+ * is possible to assign a specific handler for each interrupt
+ * independently.
+ * However, there are a few sources called "system sources" that
+ * share a common IRQ line and vector, called "system IRQ".
+ * So a unique system IRQ dispatcher is implemented here.
+ * This module also contains an interface to manage every source
+ * independently. It is possible to assign to every system IRQ
+ * a specific IRQ handler.
+ *
+ * \see sysirq_setHandler
+ * \see sysirq_setEnable
+ */
+
+#include "sysirq.h"
+#include "at91sam7s.h"
+#include <cfg/cpu.h>
+#include <cfg/module.h>
+#include <cfg/macros.h>
+
+/**
+ * Enable/disable the Periodic Interrupt Timer
+ * interrupt.
+ */
+INLINE void pit_setEnable(bool enable)
+{
+ if (enable)
+ PIT_MR |= BV(PITIEN);
+ else
+ PIT_MR &= ~BV(PITIEN);
+}
+
+/**
+ * Table containing all system irqs.
+ */
+static SysIrq sysirq_tab[] =
+{
+ /* PIT, Periodic Interval Timer (System timer)*/
+ {
+ .enabled = false,
+ .setEnable = pit_setEnable,
+ .handler = NULL,
+ },
+ /* TODO: add other system sources here */
+};
+
+STATIC_ASSERT(countof(sysirq_tab) == SYSIRQ_CNT);
+
+/*!
+ * \brief Interrupt entry.
+ */
+#define IRQ_ENTRY() \
+ asm volatile("sub lr, lr,#4" "\n\t" /* Adjust LR */ \
+ "stmfd sp!,{r0-r12,lr}" "\n\t" /* Save registers on IRQ stack. */ \
+ "mrs r1, spsr" "\n\t" /* Save SPSR */ \
+ "stmfd sp!,{r1}" "\n\t") /* */
+
+/*!
+ * \brief Interrupt exit.
+ */
+#define IRQ_EXIT() \
+ asm volatile("ldmfd sp!, {r1}" "\n\t" /* Restore SPSR */ \
+ "msr spsr_c, r1" "\n\t" /* */ \
+ "ldr r0, =0xFFFFF000" "\n\t" /* End of interrupt. */ \
+ "str r0, [r0, #0x130]" "\n\t" /* */ \
+ "ldmfd sp!, {r0-r12, pc}^" "\n\t") /* Restore registers and return. */
+
+
+/**
+ * System IRQ dispatcher.
+ * This is the entry point for all system IRQs in AT91.
+ * This function checks for interrupt enable state of
+ * various sources (system timer, etc..) and calls
+ * the corresponding handler.
+ */
+static void sysirq_dispatcher(void) __attribute__ ((naked));
+static void sysirq_dispatcher(void)
+{
+ IRQ_ENTRY();
+ for (unsigned i = 0; i < countof(sysirq_tab); i++)
+ {
+ if (sysirq_tab[i].enabled
+ && sysirq_tab[i].handler)
+ sysirq_tab[i].handler();
+ }
+
+ IRQ_EXIT();
+}
+
+#define SYSIRQ_PRIORITY 0 ///< default priority for system irqs.
+
+
+MOD_DEFINE(sysirq);
+
+/**
+ * Init system IRQ handling.
+ * \note all system interrupts are disabled.
+ */
+void sysirq_init(void)
+{
+ cpuflags_t flags;
+ IRQ_SAVE_DISABLE(flags);
+
+ /* Disable all system interrupts */
+ for (unsigned i = 0; i < countof(sysirq_tab); i++)
+ sysirq_tab[i].setEnable(false);
+
+ /* Set the vector. */
+ AIC_SVR(SYSC_ID) = sysirq_dispatcher;
+ /* Initialize to edge triggered with defined priority. */
+ AIC_SMR(SYSC_ID) = AIC_SRCTYPE_INT_EDGE_TRIGGERED | SYSIRQ_PRIORITY;
+ /* Clear pending interrupt */
+ AIC_ICCR = BV(SYSC_ID);
+ /* Enable the system IRQ */
+ AIC_IECR = BV(SYSC_ID);
+
+ IRQ_RESTORE(flags);
+ MOD_INIT(sysirq);
+}
+
+
+/**
+ * Helper function used to set handler for system IRQ \a irq.
+ */
+void sysirq_setHandler(sysirq_t irq, sysirq_handler_t handler)
+{
+ ASSERT(irq >= 0);
+ ASSERT(irq < SYSIRQ_CNT);
+ sysirq_tab[irq].handler = handler;
+}
+
+/**
+ * Helper function used to enable/disable system IRQ \a irq.
+ */
+void sysirq_setEnable(sysirq_t irq, bool enable)
+{
+ ASSERT(irq >= 0);
+ ASSERT(irq < SYSIRQ_CNT);
+
+ sysirq_tab[irq].setEnable(enable);
+ sysirq_tab[irq].enabled = enable;
+}
+
+/**
+ * Helper function used to get system IRQ \a irq state.
+ */
+bool sysirq_enabled(sysirq_t irq)
+{
+ ASSERT(irq >= 0);
+ ASSERT(irq < SYSIRQ_CNT);
+
+ return sysirq_tab[irq].enabled;
+}
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * \brief System irq handler for Atmel AT91 ARM7 processors (interface).
+ */
+
+#ifndef DRV_AT91_SYSIRQ_H
+#define DRV_AT91_SYSIRQ_H
+
+#include <cfg/compiler.h>
+
+typedef void (* sysirq_handler_t)(void); ///< Type for system irq handler.
+typedef void (* sysirq_setEnable_t)(bool); ///< Type for system irq enable/disable function.
+
+/**
+ * Structure used to define a system interrupt source.
+ */
+typedef struct SysIrq
+{
+ bool enabled; ///< Getter for irq enable/disable state.
+ sysirq_setEnable_t setEnable; ///< Setter for irq enable/disable state.
+ sysirq_handler_t handler; ///< IRQ handler.
+} SysIrq;
+
+/**
+ * System IRQ ID list.
+ */
+typedef enum sysirq_t
+{
+ SYSIRQ_PIT, ///< Periodic Interval Timer
+ /* TODO: add all system irqs */
+ SYSIRQ_CNT
+} sysirq_t;
+
+void sysirq_init(void);
+void sysirq_setHandler(sysirq_t irq, sysirq_handler_t handler);
+void sysirq_setEnable(sysirq_t irq, bool enable);
+bool sysirq_enabled(sysirq_t irq);
+
+#endif /* ARCH_ARM_SYSIRQ_H */
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * \brief Low-level timer module for ARM (inplementation).
+ */
+
+#include <cpu/detect.h>
+
+#if CPU_ARM_AT91
+ #include "timer_at91.c"
+/*#elif Add other ARM families here */
+#else
+ #error Unknown CPU
+#endif
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * \brief Low-level timer module for Atmel AT91 (inplementation).
+ */
+
+#include "timer_at91.h"
+#include "at91sam7s.h"
+#include "sysirq.h"
+
+#include <cfg/macros.h> // BV()
+#include <cfg/module.h>
+#include <cfg/cpu.h>
+
+
+/** HW dependent timer initialization */
+#if (CONFIG_TIMER == TIMER_ON_PIT)
+ INLINE void timer_hw_irq(void)
+ {
+ /* Reset counters, this is needed to reset timer and interrupt flags */
+ uint32_t dummy = PIVR;
+ (void) dummy;
+ }
+
+ INLINE bool timer_hw_triggered(void)
+ {
+ return PIT_SR & BV(PITS);
+ }
+
+ INLINE void timer_hw_init(void)
+ {
+ cpuflags_t flags;
+
+ MOD_CHECK(sysirq);
+
+ IRQ_SAVE_DISABLE(flags);
+
+ PIT_MR = TIMER_HW_CNT;
+ /* Register system interrupt handler. */
+ sysirq_setHandler(SYSIRQ_PIT, timer_handler);
+
+ /* Enable interval timer and interval timer interrupts */
+ PIT_MR |= BV(PITEN);
+ sysirq_setEnable(SYSIRQ_PIT, true);
+
+ /* Reset counters, this is needed to start timer and interrupt flags */
+ uint32_t dummy = PIVR;
+ (void) dummy;
+
+ IRQ_RESTORE(flags);
+ }
+
+ INLINE hptime_t timer_hw_hpread(void)
+ {
+ /* In the upper part of PIT_PIIR there is unused data */
+ return PIIR & CPIV_MASK;
+ }
+
+#else
+ #error Unimplemented value for CONFIG_TIMER
+#endif /* CONFIG_TIMER */
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * \brief Low-level timer module for Atmel AT91 (interface).
+ */
+
+#ifndef DRV_AT91_TIMER_H
+#define DRV_AT91_TIMER_H
+
+#include <appconfig.h> /* CONFIG_TIMER */
+#include <cfg/compiler.h> /* uint8_t */
+#include <hw_cpu.h> /* CLOCK_FREQ */
+
+/**
+ * \name Values for CONFIG_TIMER.
+ *
+ * Select which hardware timer interrupt to use for system clock and softtimers.
+ *
+ * \{
+ */
+#define TIMER_ON_PIT 1 ///< System timer on Periodic interval timer
+
+#define TIMER_DEFAULT TIMER_ON_PIT ///< Default system timer
+/* \} */
+
+/*
+ * Hardware dependent timer initialization.
+ */
+#if (CONFIG_TIMER == TIMER_ON_PIT)
+
+ void timer_handler(void);
+
+ #define DEFINE_TIMER_ISR void timer_handler(void)
+ #define TIMER_TICKS_PER_SEC 1000
+ #define TIMER_HW_CNT (CLOCK_FREQ / (16 * TIMER_TICKS_PER_SEC) - 1)
+
+ /** Frequency of the hardware high-precision timer. */
+ #define TIMER_HW_HPTICKS_PER_SEC (CLOCK_FREQ / 16)
+
+ /// Type of time expressed in ticks of the hardware high-precision timer
+ typedef uint32_t hptime_t;
+#else
+
+ #error Unimplemented value for CONFIG_TIMER
+#endif /* CONFIG_TIMER */
+
+
+#endif /* DRV_TIMER_AT91_H */
--- /dev/null
+/****************************************************************************\r
+* Copyright (c) 2006 by Michael Fischer. All rights reserved.\r
+*\r
+* Redistribution and use in source and binary forms, with or without \r
+* modification, are permitted provided that the following conditions \r
+* are met:\r
+* \r
+* 1. Redistributions of source code must retain the above copyright \r
+* notice, this list of conditions and the following disclaimer.\r
+* 2. Redistributions in binary form must reproduce the above copyright\r
+* notice, this list of conditions and the following disclaimer in the \r
+* documentation and/or other materials provided with the distribution.\r
+* 3. Neither the name of the author nor the names of its contributors may \r
+* be used to endorse or promote products derived from this software \r
+* without specific prior written permission.\r
+*\r
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \r
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT \r
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS \r
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL \r
+* THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, \r
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS \r
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED \r
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, \r
+* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF \r
+* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF \r
+* SUCH DAMAGE.\r
+*\r
+****************************************************************************\r
+*\r
+* History:\r
+*\r
+* 18.12.06 mifi First Version\r
+* The hardware initialization is based on the startup file\r
+* crtat91sam7x256_rom.S from NutOS 4.2.1. \r
+* Therefore partial copyright by egnite Software GmbH.\r
+****************************************************************************/\r
+\r
+/*\r
+ * Some defines for the program status registers\r
+ */\r
+ ARM_MODE_USER = 0x10 /* Normal User Mode */ \r
+ ARM_MODE_FIQ = 0x11 /* FIQ Fast Interrupts Mode */\r
+ ARM_MODE_IRQ = 0x12 /* IRQ Standard Interrupts Mode */\r
+ ARM_MODE_SVC = 0x13 /* Supervisor Interrupts Mode */\r
+ ARM_MODE_ABORT = 0x17 /* Abort Processing memory Faults Mode */\r
+ ARM_MODE_UNDEF = 0x1B /* Undefined Instructions Mode */\r
+ ARM_MODE_SYS = 0x1F /* System Running in Priviledged Operating Mode */\r
+ ARM_MODE_MASK = 0x1F\r
+ \r
+ I_BIT = 0x80 /* disable IRQ when I bit is set */\r
+ F_BIT = 0x40 /* disable IRQ when I bit is set */\r
+ \r
+/*\r
+ * Register Base Address\r
+ */\r
+ AIC_BASE = 0xFFFFF000\r
+ AIC_EOICR_OFF = 0x130\r
+ AIC_IDCR_OFF = 0x124\r
+\r
+ RSTC_MR = 0xFFFFFD08\r
+ RSTC_KEY = 0xA5000000\r
+ RSTC_URSTEN = 0x00000001\r
+\r
+ WDT_BASE = 0xFFFFFD40\r
+ WDT_MR_OFF = 0x00000004\r
+ WDT_WDDIS = 0x00008000\r
+\r
+ MC_BASE = 0xFFFFFF00\r
+ MC_FMR_OFF = 0x00000060\r
+ MC_FWS_1FWS = 0x00480100\r
+ \r
+ .section .vectors,"ax"\r
+ .code 32\r
+ \r
+/****************************************************************************/\r
+/* Vector table and reset entry */\r
+/****************************************************************************/\r
+_vectors:\r
+ ldr pc, ResetAddr /* Reset */\r
+ ldr pc, UndefAddr /* Undefined instruction */\r
+ ldr pc, SWIAddr /* Software interrupt */\r
+ ldr pc, PAbortAddr /* Prefetch abort */\r
+ ldr pc, DAbortAddr /* Data abort */\r
+ ldr pc, ReservedAddr /* Reserved */\r
+ ldr pc, [pc, #-0xF20] /* IRQ interrupt */\r
+ ldr pc, FIQAddr /* FIQ interrupt */\r
+\r
+.extern maion\r
+\r
+ResetAddr: .word ResetHandler\r
+UndefAddr: .word UndefHandler\r
+SWIAddr: .word SWIHandler\r
+PAbortAddr: .word PAbortHandler\r
+DAbortAddr: .word DAbortHandler\r
+ReservedAddr: .word 0\r
+IRQAddr: .word IRQHandler\r
+FIQAddr: .word FIQHandler\r
+\r
+ .ltorg\r
+\r
+ .section .init, "ax"\r
+ .code 32\r
+ \r
+ .global ResetHandler\r
+ .global ExitFunction\r
+ .extern main\r
+/****************************************************************************/\r
+/* Reset handler */\r
+/****************************************************************************/\r
+ResetHandler:\r
+ /*\r
+ * The watchdog is enabled after processor reset. Disable it.\r
+ */\r
+ ldr r1, =WDT_BASE\r
+ ldr r0, =WDT_WDDIS\r
+ str r0, [r1, #WDT_MR_OFF]\r
+\r
+ \r
+ /*\r
+ * Enable user reset: assertion length programmed to 1ms\r
+ */\r
+ ldr r0, =(RSTC_KEY | RSTC_URSTEN | (4 << 8))\r
+ ldr r1, =RSTC_MR\r
+ str r0, [r1, #0]\r
+\r
+ \r
+ /*\r
+ * Use 2 cycles for flash access.\r
+ */\r
+ ldr r1, =MC_BASE\r
+ ldr r0, =MC_FWS_1FWS\r
+ str r0, [r1, #MC_FMR_OFF]\r
+\r
+\r
+ /*\r
+ * Disable all interrupts. Useful for debugging w/o target reset.\r
+ */\r
+ ldr r1, =AIC_BASE\r
+ mvn r0, #0\r
+ str r0, [r1, #AIC_EOICR_OFF]\r
+ str r0, [r1, #AIC_IDCR_OFF]\r
+\r
+ \r
+ /*\r
+ * Setup a stack for each mode\r
+ */ \r
+ msr CPSR_c, #ARM_MODE_UNDEF | I_BIT | F_BIT /* Undefined Instruction Mode */ \r
+ ldr sp, =__stack_und_end\r
+ \r
+ msr CPSR_c, #ARM_MODE_ABORT | I_BIT | F_BIT /* Abort Mode */\r
+ ldr sp, =__stack_abt_end\r
+ \r
+ msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT /* FIQ Mode */ \r
+ ldr sp, =__stack_fiq_end\r
+ \r
+ msr CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT /* IRQ Mode */ \r
+ ldr sp, =__stack_irq_end\r
+ \r
+ msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT /* Supervisor Mode */\r
+ ldr sp, =__stack_svc_end\r
+\r
+\r
+ /*\r
+ * Clear .bss section\r
+ */\r
+ ldr r1, =__bss_start\r
+ ldr r2, =__bss_end\r
+ ldr r3, =0\r
+bss_clear_loop:\r
+ cmp r1, r2\r
+ strne r3, [r1], #+4\r
+ bne bss_clear_loop\r
+ \r
+ \r
+ /*\r
+ * Jump to main\r
+ */\r
+\r
+ mov r0, #0 /* No arguments */\r
+ mov r1, #0 /* No arguments */\r
+ ldr r2, =main\r
+ mov lr, pc\r
+ bx r2 /* And jump... */\r
+ \r
+ExitFunction:\r
+ nop\r
+ nop\r
+ nop\r
+ b ExitFunction \r
+ \r
+\r
+/****************************************************************************/\r
+/* Default interrupt handler */\r
+/****************************************************************************/\r
+\r
+UndefHandler:\r
+ b UndefHandler\r
+ \r
+SWIHandler:\r
+ b SWIHandler\r
+\r
+PAbortHandler:\r
+ b PAbortHandler\r
+\r
+DAbortHandler:\r
+ b DAbortHandler\r
+ \r
+IRQHandler:\r
+ b IRQHandler\r
+ \r
+FIQHandler:\r
+ b FIQHandler\r
+ \r
+ .weak ExitFunction\r
+ .weak UndefHandler, PAbortHandler, DAbortHandler\r
+ .weak IRQHandler, FIQHandler\r
+\r
+ .ltorg\r
+/*** EOF ***/ \r
+ \r
+\r
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * AT91 advanced interrupt controller.
+ * This file is based on NUT/OS implementation. See license below.
+ */
+
+/*
+ * Copyright (C) 2005-2006 by egnite Software GmbH. All rights reserved.
+ *
+ * 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 of the copyright holders nor the names of
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY EGNITE SOFTWARE GMBH 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 EGNITE
+ * SOFTWARE GMBH 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.
+ *
+ * For additional information see http://www.ethernut.de/
+ */
+
+#ifndef AT91_AIC_H
+#define AT91_AIC_H
+
+#include <cfg/compiler.h>
+
+
+
+/**
+ * Source mode register array.
+ */
+#define AIC_SMR(i) (*((volatile uint32_t *)(AIC_BASE + (i) * 4)))
+
+/**
+ * Priority mask.
+ * Priority levels can be between 0 (lowest) and 7 (highest).
+ */
+#define AIC_PRIOR_MASK 0x00000007
+
+/**
+ * Interrupt source type mask.
+ * Internal interrupts can level sensitive or edge triggered.
+ *
+ * External interrupts can triggered on positive or negative levels or
+ * on rising or falling edges.
+ */
+#define AIC_SRCTYPE_MASK 0x00000060
+
+#define AIC_SRCTYPE_INT_LEVEL_SENSITIVE 0x00000000 ///< Internal level sensitive.
+#define AIC_SRCTYPE_INT_EDGE_TRIGGERED 0x00000020 ///< Internal edge triggered.
+#define AIC_SRCTYPE_EXT_LOW_LEVEL 0x00000000 ///< External low level.
+#define AIC_SRCTYPE_EXT_NEGATIVE_EDGE 0x00000020 ///< External falling edge.
+#define AIC_SRCTYPE_EXT_HIGH_LEVEL 0x00000040 ///< External high level.
+#define AIC_SRCTYPE_EXT_POSITIVE_EDGE 0x00000060 ///< External rising edge.
+/*\}*/
+
+
+/**
+ * Type for interrupt handlers.
+ */
+typedef void (*irq_handler_t)(void);
+
+/** Interrupt Source Vector Registers */
+/*\{*/
+/** Source vector register array.
+ *
+ * Stores the addresses of the corresponding interrupt handlers.
+ */
+#define AIC_SVR(i) (*((volatile irq_handler_t *)(AIC_BASE + 0x80 + (i) * 4)))
+/*\}*/
+
+/** Interrupt Vector Register */
+/*\{*/
+#define AIC_IVR_OFF 0x00000100 ///< IRQ vector register offset.
+#define AIC_IVR (*((volatile uint32_t *)(AIC_BASE + AIC_IVR_OFF))) ///< IRQ vector register address.
+/*\}*/
+
+/** Fast Interrupt Vector Register */
+/*\{*/
+#define AIC_FVR_OFF 0x00000104 ///< FIQ vector register offset.
+#define AIC_FVR (*((volatile uint32_t *)(AIC_BASE + AIC_FVR_OFF))) ///< FIQ vector register address.
+/*\}*/
+
+/** Interrupt Status Register */
+/*\{*/
+#define AIC_ISR_OFF 0x00000108 ///< Interrupt status register offset.
+#define AIC_ISR (*((volatile uint32_t *)(AIC_BASE + AIC_ISR_OFF))) ///< Interrupt status register address.
+#define AIC_IRQID_MASK 0x0000001F ///< Current interrupt identifier mask.
+/*\}*/
+
+/** Interrupt Pending Register */
+/*\{*/
+#define AIC_IPR_OFF 0x0000010C ///< Interrupt pending register offset.
+#define AIC_IPR (*((volatile uint32_t *)(AIC_BASE + AIC_IPR_OFF))) ///< Interrupt pending register address.
+/*\}*/
+
+/** Interrupt Mask Register */
+/*\{*/
+#define AIC_IMR_OFF 0x00000110 ///< Interrupt mask register offset.
+#define AIC_IMR (*((volatile uint32_t *)(AIC_BASE + AIC_IMR_OFF))) ///< Interrupt mask register address.
+/*\}*/
+
+/** Interrupt Core Status Register */
+/*\{*/
+#define AIC_CISR_OFF 0x00000114 ///< Core interrupt status register offset.
+#define AIC_CISR (*((volatile uint32_t *)(AIC_BASE + AIC_CISR_OFF))) ///< Core interrupt status register address.
+#define AIC_NFIQ 1 ///< Core FIQ Status
+#define AIC_NIRQ 2 ///< Core IRQ Status
+/*\}*/
+
+/** Interrupt Enable Command Register */
+/*\{*/
+#define AIC_IECR_OFF 0x00000120 ///< Interrupt enable command register offset.
+#define AIC_IECR (*((volatile uint32_t *)(AIC_BASE + AIC_IECR_OFF))) ///< Interrupt enable command register address.
+/*\}*/
+
+/** Interrupt Disable Command Register */
+/*\{*/
+#define AIC_IDCR_OFF 0x00000124 ///< Interrupt disable command register offset.
+#define AIC_IDCR (*((volatile uint32_t *)(AIC_BASE + AIC_IDCR_OFF))) ///< Interrupt disable command register address.
+/*\}*/
+
+/** Interrupt Clear Command Register */
+/*\{*/
+#define AIC_ICCR_OFF 0x00000128 ///< Interrupt clear command register offset.
+#define AIC_ICCR (*((volatile uint32_t *)(AIC_BASE + AIC_ICCR_OFF))) ///< Interrupt clear command register address.
+/*\}*/
+
+/** Interrupt Set Command Register */
+/*\{*/
+#define AIC_ISCR_OFF 0x0000012C ///< Interrupt set command register offset.
+#define AIC_ISCR (*((volatile uint32_t *)(AIC_BASE + AIC_ISCR_OFF))) ///< Interrupt set command register address.
+/*\}*/
+
+/** End Of Interrupt Command Register */
+/*\{*/
+#define AIC_EOICR_OFF 0x00000130 ///< End of interrupt command register offset.
+#define AIC_EOICR (*((volatile uint32_t *)(AIC_BASE + AIC_EOICR_OFF))) ///< End of interrupt command register address.
+/*\}*/
+
+/** Spurious Interrupt Vector Register */
+/*\{*/
+#define AIC_SPU_OFF 0x00000134 ///< Spurious vector register offset.
+#define AIC_SPU (*((volatile uint32_t *)(AIC_BASE + AIC_SPU_OFF)== ///< Spurious vector register address.
+/*\}*/
+
+/** Debug Control Register */
+/*\{*/
+#define AIC_DCR_OFF 0x0000138 ///< Debug control register offset.
+#define AIC_DCR (*((volatile uint32_t *)(AIC_BASE + AIC_DCR_OFF))) ///< Debug control register address.
+/*\}*/
+
+/** Fast Forcing Enable Register */
+/*\{*/
+#define AIC_FFER_OFF 0x00000140 ///< Fast forcing enable register offset.
+#define AIC_FFER (*((volatile uint32_t *)(AIC_BASE + AIC_FFER_OFF))) ///< Fast forcing enable register address.
+/*\}*/
+
+/** Fast Forcing Disable Register */
+/*\{*/
+#define AIC_FFDR_OFF 0x00000144 ///< Fast forcing disable register address.
+#define AIC_FFDR (*((volatile uint32_t *)(AIC_BASE + AIC_FFDR_OFF))) ///< Fast forcing disable register address.
+/*\}*/
+
+/** Fast Forcing Status Register */
+/*\{*/
+#define AIC_FFSR_OFF 0x00000148 ///< Fast forcing status register address.
+#define AIC_FFSR (*((volatile uint32_t *)(AIC_BASE + AIC_FFSR_OFF))) ///< Fast forcing status register address.
+/*\}*/
+
+#endif /* AT91_AIC_H */
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * AT91 periodic interval timer.
+ * This file is based on NUT/OS implementation. See license below.
+ */
+
+/*
+ * Copyright (C) 2007 by egnite Software GmbH. All rights reserved.
+ *
+ * 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 of the copyright holders nor the names of
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY EGNITE SOFTWARE GMBH 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 EGNITE
+ * SOFTWARE GMBH 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.
+ *
+ * For additional information see http://www.ethernut.de/
+ */
+
+#ifndef AT91_PIT_H
+#define AT91_PIT_H
+
+#include <cfg/compiler.h>
+/**
+ *Periodic Inverval Timer Mode Register
+ *\{
+ */
+#define PIT_MR_OFF 0x00000000 ///< Mode register offset.
+#define PIT_MR (*((volatile uint32_t *)(PIT_BASE + PIT_MR_OFF))) ///< Mode register address.
+
+#define PIV_MASK 0x000FFFFF ///< Periodic interval value mask.
+#define PIV_SHIFT 0 ///< Periodic interval value shift.
+#define PITEN 24 ///< Periodic interval timer enable.
+#define PITIEN 25 ///< Periodic interval timer interrupt enable.
+/*\}*/
+
+/**
+ * Periodic Inverval Timer Status Register
+ *\{
+ */
+#define PIT_SR_OFF 0x00000004 ///< Status register offset.
+#define PIT_SR (*((volatile uint32_t *)(PIT_BASE + PIT_SR_OFF))) ///< Status register address.
+
+#define PITS 0 ///< Timer has reached PIV.
+/*\}*/
+
+/**
+ * Periodic Inverval Timer Value and Image Registers
+ *\{
+ */
+#define PIVR_OFF 0x00000008 ///< Value register offset.
+#define PIVR (*((volatile uint32_t *)(PIT_BASE + PIVR_OFF))) ///< Value register address.
+
+#define PIIR_OFF 0x0000000C ///< Image register offset.
+#define PIIR (*((volatile uint32_t *)(PIT_BASE + PIIR_OFF))) ///< Image register address.
+#define CPIV_MASK 0x000FFFFF ///< Current periodic interval value mask.
+#define CPIV_SHIFT 0 ///< Current periodic interval value SHIFT.
+#define PICNT_MASK 0xFFF00000 ///< Periodic interval counter mask.
+#define PICNT_SHIFT 20 ///< Periodic interval counter LSB.
+/*\}*/
+
+#endif /* AT91_PIT_H */
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \version $Id$
+ *
+ * \author Francesco Sacchi <batt@develer.com>
+ *
+ * AT91SAM7S register definitions.
+ * This file is based on NUT/OS implementation. See license below.
+ */
+
+/*
+ * Copyright (C) 2006-2007 by egnite Software GmbH. All rights reserved.
+ *
+ * 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 of the copyright holders nor the names of
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY EGNITE SOFTWARE GMBH 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 EGNITE
+ * SOFTWARE GMBH 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.
+ *
+ * For additional information see http://www.ethernut.de/
+ */
+
+#ifndef AT91SAM7S_H
+#define AT91SAM7S_H
+
+#define FLASH_BASE 0x100000UL
+#define RAM_BASE 0x200000UL
+
+#define TC_BASE 0xFFFA0000 ///< Timer/counter base address.
+#define UDP_BASE 0xFFFB0000 ///< USB device port base address.
+#define TWI_BASE 0xFFFB8000 ///< Two-wire interface base address.
+#define USART0_BASE 0xFFFC0000 ///< USART 0 base address.
+#define USART1_BASE 0xFFFC4000 ///< USART 1 base address.
+#define PWMC_BASE 0xFFFCC000 ///< PWM controller base address.
+#define SSC_BASE 0xFFFD4000 ///< Serial synchronous controller base address.
+#define ADC_BASE 0xFFFD8000 ///< ADC base address.
+#define SPI_BASE 0xFFFE0000 ///< SPI0 base address.
+
+#define AIC_BASE 0xFFFFF000 ///< AIC base address.
+#define DBGU_BASE 0xFFFFF200 ///< DBGU base address.
+#define PIOA_BASE 0xFFFFF400 ///< PIO A base address.
+#define PMC_BASE 0xFFFFFC00 ///< PMC base address.
+#define RSTC_BASE 0xFFFFFD00 ///< Resect controller register base address.
+#define RTT_BASE 0xFFFFFD20 ///< Realtime timer base address.
+#define PIT_BASE 0xFFFFFD30 ///< Periodic interval timer base address.
+#define WDT_BASE 0xFFFFFD40 ///< Watch Dog register base address.
+#define VREG_BASE 0xFFFFFD60 ///< Voltage regulator mode controller base address.
+#define MC_BASE 0xFFFFFF00 ///< Memory controller base.
+
+#include "at91_aic.h"
+#include "at91_pit.h"
+//TODO: add other peripherals
+
+/** Peripheral Identifiers and Interrupts */
+/*\{*/
+#define FIQ_ID 0 ///< Fast interrupt ID.
+#define SYSC_ID 1 ///< System controller interrupt.
+#define PIOA_ID 2 ///< Parallel I/O controller ID.
+/* ID 3 is reserved */
+#define ADC_ID 4 ///< Analog to digital converter ID.
+#define SPI_ID 5 ///< Serial peripheral interface ID.
+#define US0_ID 6 ///< USART 0 ID.
+#define US1_ID 7 ///< USART 1 ID.
+#define SSC_ID 8 ///< Synchronous serial controller ID.
+#define TWI_ID 9 ///< Two-wire interface ID.
+#define PWMC_ID 10 ///< PWM controller ID.
+#define UDP_ID 11 ///< USB device port ID.
+#define TC0_ID 12 ///< Timer 0 ID.
+#define TC1_ID 13 ///< Timer 1 ID.
+#define TC2_ID 14 ///< Timer 2 ID.
+
+#define IRQ0_ID 30 ///< External interrupt 0 ID.
+#define IRQ1_ID 31 ///< External interrupt 1 ID.
+/*\}*/
+
+#warning Revise me after this line!
+
+#define PERIPH_RPR_OFF 0x00000100 ///< Receive pointer register offset.
+#define PERIPH_RCR_OFF 0x00000104 ///< Receive counter register offset.
+#define PERIPH_TPR_OFF 0x00000108 ///< Transmit pointer register offset.
+#define PERIPH_TCR_OFF 0x0000010C ///< Transmit counter register offset.
+#define PERIPH_RNPR_OFF 0x00000110 ///< Receive next pointer register offset.
+#define PERIPH_RNCR_OFF 0x00000114 ///< Receive next counter register offset.
+#define PERIPH_TNPR_OFF 0x00000118 ///< Transmit next pointer register offset.
+#define PERIPH_TNCR_OFF 0x0000011C ///< Transmit next counter register offset.
+#define PERIPH_PTCR_OFF 0x00000120 ///< PDC transfer control register offset.
+#define PERIPH_PTSR_OFF 0x00000124 ///< PDC transfer status register offset.
+
+#define PDC_RXTEN 0x00000001 ///< Receiver transfer enable.
+#define PDC_RXTDIS 0x00000002 ///< Receiver transfer disable.
+#define PDC_TXTEN 0x00000100 ///< Transmitter transfer enable.
+#define PDC_TXTDIS 0x00000200 ///< Transmitter transfer disable.
+
+#define DBGU_HAS_PDC
+#define SPI_HAS_PDC
+#define SSC_HAS_PDC
+#define USART_HAS_PDC
+
+#define PIO_HAS_MULTIDRIVER
+#define PIO_HAS_PULLUP
+#define PIO_HAS_PERIPHERALSELECT
+#define PIO_HAS_OUTPUTWRITEENABLE
+
+
+/** Historical SPI0 Peripheral Multiplexing Names */
+/*\{*/
+#define SPI0_NPCS0_PA12A 12 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_NPCS1_PA13A 13 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_NPCS1_PA07B 7 ///< Port bit number on PIO-A Perpheral B.
+#define SPI0_NPCS1_PB13B 13 ///< Port bit number on PIO-B Perpheral B.
+#define SPI0_NPCS2_PA14A 14 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_NPCS2_PA08B 8 ///< Port bit number on PIO-A Perpheral B.
+#define SPI0_NPCS2_PB14B 14 ///< Port bit number on PIO-B Perpheral B.
+#define SPI0_NPCS3_PA15A 15 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_NPCS3_PA09B 9 ///< Port bit number on PIO-A Perpheral B.
+#define SPI0_NPCS3_PB17B 17 ///< Port bit number on PIO-B Perpheral B.
+#define SPI0_MISO_PA16A 16 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_MOSI_PA17A 17 ///< Port bit number on PIO-A Perpheral A.
+#define SPI0_SPCK_PA18A 18 ///< Port bit number on PIO-A Perpheral A.
+/*\}*/
+
+/** USART Peripheral Multiplexing */
+/*\{*/
+#define PA0_RXD0_A 0
+#define PA1_TXD0_A 1
+#define PA2_SCK0_A 2
+#define PA3_RTS0_A 3
+#define PA4_CTS0_A 4
+
+#define PA5_RXD1_A 5
+#define PA6_TXD1_A 6
+#define PA7_SCK1_A 7
+#define PA8_RTS1_A 8
+#define PA9_CTS1_A 9
+#define PB23_DCD1_B 23
+#define PB24_DSR1_B 24
+#define PB25_DTR1_B 25
+#define PB26_RI1_B 26
+/*\}*/
+
+/** SPI Peripheral Multiplexing */
+/*\{*/
+#define PA16_SPI0_MISO_A 16
+#define PA17_SPI0_MOSI_A 17
+#define PA18_SPI0_SPCK_A 18
+#define PA12_SPI0_NPCS0_A 12
+#define PA13_SPI0_NPCS1_A 13
+#define PA7_SPI0_NPCS1_B 7
+#define PA14_SPI0_NPCS2_A 14
+#define PB14_SPI0_NPCS2_B 14
+#define PA8_SPI0_NPCS2_B 8
+#define PA15_SPI0_NPCS3_A 15
+#define PA9_SPI0_NPCS3_B 9
+
+#define SPI0_PINS _BV(PA16_SPI0_MISO_A) | _BV(PA17_SPI0_MOSI_A) | _BV(PA18_SPI0_SPCK_A)
+#define SPI0_PIO_BASE PIOA_BASE
+#define SPI0_PSR_OFF PIO_ASR_OFF
+
+#define SPI0_CS0_PIN _BV(PA12_SPI0_NPCS0_A)
+#define SPI0_CS0_PIO_BASE PIOA_BASE
+#define SPI0_CS0_PSR_OFF PIO_ASR_OFF
+
+#ifndef SPI0_CS1_PIN
+#define SPI0_CS1_PIN _BV(PA13_SPI0_NPCS1_A)
+#define SPI0_CS1_PIO_BASE PIOA_BASE
+#define SPI0_CS1_PSR_OFF PIO_ASR_OFF
+#endif
+
+#ifndef SPI0_CS2_PIN
+#define SPI0_CS2_PIN _BV(PA14_SPI0_NPCS2_A)
+#define SPI0_CS2_PIO_BASE PIOA_BASE
+#define SPI0_CS2_PSR_OFF PIO_ASR_OFF
+#endif
+
+#ifndef SPI0_CS3_PIN
+#define SPI0_CS3_PIN _BV(PA15_SPI0_NPCS3_A)
+#define SPI0_CS3_PIO_BASE PIOA_BASE
+#define SPI0_CS3_PSR_OFF PIO_ASR_OFF
+#endif
+
+#define PA24_SPI1_MISO_B 24
+#define PA23_SPI1_MOSI_B 23
+#define PA22_SPI1_SPCK_B 22
+#define PA21_SPI1_NPCS0_B 21
+#define PA25_SPI1_NPCS1_B 25
+#define PB13_SPI0_NPCS1_B 13
+#define PA2_SPI1_NPCS1_B 2
+#define PB10_SPI1_NPCS1_B 10
+#define PA26_SPI1_NPCS2_B 26
+#define PA3_SPI1_NPCS2_B 3
+#define PB11_SPI1_NPCS2_B 11
+#define PB17_SPI0_NPCS3_B 17
+#define PA4_SPI1_NPCS3_B 4
+#define PA29_SPI1_NPCS3_B 29
+#define PB16_SPI1_NPCS3_B 16
+
+#define SPI1_PINS _BV(PA24_SPI1_MISO_B) | _BV(PA23_SPI1_MOSI_B) | _BV(PA22_SPI1_SPCK_B)
+#define SPI1_PIO_BASE PIOA_BASE
+#define SPI1_PSR_OFF PIO_BSR_OFF
+
+#define SPI1_CS0_PIN _BV(PA21_SPI1_NPCS0_B)
+#define SPI1_CS0_PIO_BASE PIOA_BASE
+#define SPI1_CS0_PSR_OFF PIO_BSR_OFF
+
+#ifndef SPI1_CS1_PIN
+#define SPI1_CS1_PIN _BV(PA25_SPI1_NPCS1_B)
+#define SPI1_CS1_PIO_BASE PIOA_BASE
+#define SPI1_CS1_PSR_OFF PIO_BSR_OFF
+#endif
+
+#ifndef SPI1_CS2_PIN
+#define SPI1_CS2_PIN _BV(PA26_SPI1_NPCS2_B)
+#define SPI1_CS2_PIO_BASE PIOA_BASE
+#define SPI1_CS2_PSR_OFF PIO_BSR_OFF
+#endif
+
+#ifndef SPI1_CS3_PIN
+#define SPI1_CS3_PIN _BV(PA29_SPI1_NPCS3_B)
+#define SPI1_CS3_PIO_BASE PIOA_BASE
+#define SPI1_CS3_PSR_OFF PIO_BSR_OFF
+#endif
+
+/*\}*/
+
+/** EMAC Interface Peripheral Multiplexing */
+/*\{*/
+#define PB0_ETXCK_EREFCK_A 0
+#define PB1_ETXEN_A 1
+#define PB2_ETX0_A 2
+#define PB3_ETX1_A 3
+#define PB4_ECRS_A 4
+#define PB5_ERX0_A 5
+#define PB6_ERX1_A 6
+#define PB7_ERXER_A 7
+#define PB8_EMDC_A 8
+#define PB9_EMDIO_A 9
+#define PB10_ETX2_A 10
+#define PB11_ETX3_A 11
+#define PB12_ETXER_A 12
+#define PB13_ERX2_A 13
+#define PB14_ERX3_A 14
+#define PB15_ERXDV_ECRSDV_A 15
+#define PB16_ECOL_A 16
+#define PB17_ERXCK_A 17
+#define PB18_EF100_A 18
+/*\}*/
+
+/** Debug Unit Peripheral Multiplexing */
+/*\{*/
+#define PA27_DRXD_A 27
+#define PA28_DTXD_A 28
+/*\}*/
+
+/** Synchronous Serial Controller Peripheral Multiplexing */
+/*\{*/
+#define PA23_TD_A 23 ///< Transmit data pin.
+#define PA24_RD_A 24 ///< Receive data pin.
+#define PA22_TK_A 22 ///< Transmit clock pin.
+#define PA25_RK_A 25 ///< Receive clock pin.
+#define PA21_TF_A 21 ///< Transmit frame sync. pin.
+#define PA26_RF_A 26 ///< Receive frame sync. pin.
+/*\}*/
+
+/** Two Wire Interface Peripheral Multiplexing */
+/*\{*/
+#define PA10_TWD_A 10 ///< Two wire serial data pin.
+#define PA11_TWCK_A 11 ///< Two wire serial clock pin.
+/*\}*/
+
+/** Timer/Counter Peripheral Multiplexing */
+/*\{*/
+#define PB23_TIOA0_A 23
+#define PB24_TIOB0_A 24
+#define PB12_TCLK0_B 12
+
+#define PB25_TIOA1_A 25
+#define PB26_TIOB1_A 26
+#define PB19_TCLK1_B 19
+
+#define PB27_TIOA2_A 27
+#define PB28_TIOB2_A 28
+#define PA15_TCLK2_B 15
+/*\}*/
+
+/** Clocks, Oscillators and PLLs Peripheral Multiplexing */
+/*\{*/
+#define PB0_PCK0_B 0
+#define PB20_PCK0_B 20
+#define PA13_PCK1_B 13
+#define PB29_PCK1_A 29
+#define PB21_PCK1_B 21
+#define PA30_PCK2_B 30
+#define PB30_PCK2_A 30
+#define PB22_PCK2_B 22
+#define PA27_PCK3_B 27
+/*\}*/
+
+/** Advanced Interrupt Controller Peripheral Multiplexing */
+/*\{*/
+#define PA29_FIQ_A 29
+#define PA30_IRQ0_A 30
+#define PA14_IRQ1_B 14
+/*\}*/
+
+/** ADC Interface Peripheral Multiplexing */
+/*\{*/
+#define PB18_ADTRG_B 18 ///< ADC trigger pin.
+/*\}*/
+
+/** CAN Interface Peripheral Multiplexing */
+/*\{*/
+#define PA19_CANRX_A 19
+#define PA20_CANTX_A 20
+/*\}*/
+
+/** PWM Peripheral Multiplexing */
+/*\{*/
+#define PB19_PWM0_A 19
+#define PB27_PWM0_B 27
+#define PB20_PWM1_A 20
+#define PB28_PWM1_B 28
+#define PB21_PWM2_A 21
+#define PB29_PWM2_B 29
+#define PB22_PWM3_A 22
+#define PB30_PWM3_B 30
+/*\}*/
+
+#endif /* AT91SAM7S_H */
--- /dev/null
+target remote localhost:3333\r
+monitor reset\r
+monitor sleep 500\r
+monitor poll\r
+monitor soft_reset_halt\r
+monitor arm7_9 sw_bkpts enable\r
+\r
+# WDT_MR, disable watchdog \r
+monitor mww 0xFFFFFD44 0x00008000\r
+\r
+# RSTC_MR, enable user reset\r
+monitor mww 0xfffffd08 0xa5000001\r
+\r
+# CKGR_MOR\r
+monitor mww 0xFFFFFC20 0x00000601\r
+monitor sleep 10\r
+\r
+# CKGR_PLLR\r
+monitor mww 0xFFFFFC2C 0x00481c0e\r
+monitor sleep 10\r
+\r
+# PMC_MCKR\r
+monitor mww 0xFFFFFC30 0x00000007\r
+monitor sleep 10\r
+\r
+# PMC_IER\r
+monitor mww 0xFFFFFF60 0x00480100\r
+monitor sleep 100\r
+\r
+#Remap RAM to address 0\r
+monitor mww 0xFFFFFF00 0x00000001\r
+monitor sleep 100\r
+\r
+break main\r
+load\r
+continue\r
--- /dev/null
+/****************************************************************************\r
+* Copyright (c) 2006 by Michael Fischer. All rights reserved.\r
+*\r
+* Redistribution and use in source and binary forms, with or without \r
+* modification, are permitted provided that the following conditions \r
+* are met:\r
+* \r
+* 1. Redistributions of source code must retain the above copyright \r
+* notice, this list of conditions and the following disclaimer.\r
+* 2. Redistributions in binary form must reproduce the above copyright\r
+* notice, this list of conditions and the following disclaimer in the \r
+* documentation and/or other materials provided with the distribution.\r
+* 3. Neither the name of the author nor the names of its contributors may \r
+* be used to endorse or promote products derived from this software \r
+* without specific prior written permission.\r
+*\r
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \r
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT \r
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS \r
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL \r
+* THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, \r
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS \r
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED \r
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, \r
+* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF \r
+* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF \r
+* SUCH DAMAGE.\r
+*\r
+****************************************************************************\r
+*\r
+* History:\r
+*\r
+* 30.03.06 mifi First Version\r
+****************************************************************************/\r
+\r
+\r
+ENTRY(ResetHandler)\r
+SEARCH_DIR(.)\r
+\r
+/*\r
+ * Define stack size here\r
+ */\r
+FIQ_STACK_SIZE = 0x0100;\r
+IRQ_STACK_SIZE = 0x0100;\r
+ABT_STACK_SIZE = 0x0100;\r
+UND_STACK_SIZE = 0x0100;\r
+SVC_STACK_SIZE = 0x0400;\r
+\r
+\r
+MEMORY\r
+{\r
+ ram : org = 0x00000000, len = 64k\r
+}\r
+\r
+/*\r
+ * Do not change the next code\r
+ */\r
+SECTIONS\r
+{\r
+ .text :\r
+ {\r
+ *(.vectors);\r
+ . = ALIGN(4);\r
+ *(.init);\r
+ . = ALIGN(4);\r
+ *(.text);\r
+ . = ALIGN(4);\r
+ *(.rodata);\r
+ . = ALIGN(4);\r
+ *(.rodata*);\r
+ . = ALIGN(4);\r
+ *(.glue_7t);\r
+ . = ALIGN(4);\r
+ *(.glue_7);\r
+ . = ALIGN(4);\r
+ etext = .;\r
+ } > ram\r
+\r
+ .data :\r
+ {\r
+ PROVIDE (__data_start = .);\r
+ *(.data)\r
+ . = ALIGN(4);\r
+ edata = .;\r
+ _edata = .;\r
+ PROVIDE (__data_end = .);\r
+ } > ram\r
+\r
+ .bss :\r
+ {\r
+ PROVIDE (__bss_start = .);\r
+ *(.bss)\r
+ *(COMMON)\r
+ . = ALIGN(4);\r
+ PROVIDE (__bss_end = .);\r
+ \r
+ . = ALIGN(256);\r
+ \r
+ PROVIDE (__stack_start = .);\r
+ \r
+ PROVIDE (__stack_fiq_start = .);\r
+ . += FIQ_STACK_SIZE;\r
+ . = ALIGN(4);\r
+ PROVIDE (__stack_fiq_end = .);\r
+\r
+ PROVIDE (__stack_irq_start = .);\r
+ . += IRQ_STACK_SIZE;\r
+ . = ALIGN(4);\r
+ PROVIDE (__stack_irq_end = .);\r
+\r
+ PROVIDE (__stack_abt_start = .);\r
+ . += ABT_STACK_SIZE;\r
+ . = ALIGN(4);\r
+ PROVIDE (__stack_abt_end = .);\r
+\r
+ PROVIDE (__stack_und_start = .);\r
+ . += UND_STACK_SIZE;\r
+ . = ALIGN(4);\r
+ PROVIDE (__stack_und_end = .);\r
+\r
+ PROVIDE (__stack_svc_start = .);\r
+ . += SVC_STACK_SIZE;\r
+ . = ALIGN(4);\r
+ PROVIDE (__stack_svc_end = .);\r
+ PROVIDE (__stack_end = .);\r
+ PROVIDE (__heap_start = .); \r
+ } > ram\r
+ \r
+}\r
+/*** EOF ***/\r
+\r
--- /dev/null
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos 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 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2004, 2005,2006, 2007 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 2004 Giovanni Bajo
+ *
+ * -->
+ *
+ * \brief CPU-specific definitions
+ *
+ * \author Giovanni Bajo <rasky@develer.com>
+ * \author Bernardo Innocenti <bernie@develer.com>
+ * \author Stefano Fedrigo <aleph@develer.com>
+ * \author Francesco Sacchi <batt@develer.com>
+ */
+#ifndef CPU_CPU_H
+#define CPU_CPU_H
+
+#include "detect.h"
+
+#include <cfg/compiler.h> /* for uintXX_t */
+#include <cfg/arch_config.h> /* ARCH_EMUL */
+
+
+/**
+ * \name Macros for determining CPU endianness.
+ * \{
+ */
+#define CPU_BIG_ENDIAN 0x1234
+#define CPU_LITTLE_ENDIAN 0x3412 /* Look twice, pal. This is not a bug. */
+/*\}*/
+
+/** Macro to include cpu-specific versions of the headers. */
+#define CPU_HEADER(module) PP_STRINGIZE(PP_CAT3(module, _, CPU_ID).h)
+
+/** Macro to include cpu-specific versions of implementation files. */
+#define CPU_CSOURCE(module) PP_STRINGIZE(PP_CAT3(module, _, CPU_ID).c)
+
+
+#if CPU_I196
+
+ #define NOP nop_instruction()
+ #define IRQ_DISABLE disable_interrupt()
+ #define IRQ_ENABLE enable_interrupt()
+
+ typedef uint16_t cpuflags_t; // FIXME
+ typedef unsigned int cpustack_t;
+
+ #define CPU_REG_BITS 16
+ #define CPU_REGS_CNT 16
+ #define CPU_STACK_GROWS_UPWARD 0
+ #define CPU_SP_ON_EMPTY_SLOT 0
+ #define CPU_BYTE_ORDER CPU_LITTLE_ENDIAN
+ #define CPU_HARVARD 0
+
+#elif CPU_X86
+
+ #define NOP asm volatile ("nop")
+
+ /* Get IRQ_* definitions from the hosting environment. */
+ #include <cfg/os.h>
+ #if OS_EMBEDDED
+ #define IRQ_DISABLE FIXME
+ #define IRQ_ENABLE FIXME
+ #define IRQ_SAVE_DISABLE(x) FIXME
+ #define IRQ_RESTORE(x) FIXME
+ typedef uint32_t cpuflags_t; // FIXME
+ #endif /* OS_EMBEDDED */
+
+
+ #define CPU_REGS_CNT 7
+ #define CPU_SAVED_REGS_CNT 7
+ #define CPU_STACK_GROWS_UPWARD 0
+ #define CPU_SP_ON_EMPTY_SLOT 0
+ #define CPU_BYTE_ORDER CPU_LITTLE_ENDIAN
+ #define CPU_HARVARD 0
+
+ #if CPU_X86_64
+ typedef uint64_t cpustack_t;
+ #define CPU_REG_BITS 64
+
+ #ifdef __WIN64__
+ /* WIN64 is an IL32-P64 weirdo. */
+ #define SIZEOF_LONG 4
+ #endif
+ #else
+ typedef uint32_t cpustack_t;
+ #define CPU_REG_BITS 32
+ #endif
+
+#elif CPU_ARM
+
+ typedef uint32_t cpuflags_t;
+ typedef uint32_t cpustack_t;
+
+ /* Register counts include SREG too */
+ #define CPU_REG_BITS 32
+ #define CPU_REGS_CNT 16
+ #define CPU_SAVED_REGS_CNT FIXME
+ #define CPU_STACK_GROWS_UPWARD 0
+ #define CPU_SP_ON_EMPTY_SLOT 0
+ #define CPU_BYTE_ORDER (__BIG_ENDIAN__ ? CPU_BIG_ENDIAN : CPU_LITTLE_ENDIAN)
+ #define CPU_HARVARD 0
+
+ #ifdef __IAR_SYSTEMS_ICC__
+
+ #include <inarm.h>
+
+ #if __CPU_MODE__ == 1 /* Thumb */
+ /* Use stubs */
+ extern cpuflags_t get_CPSR(void);
+ extern void set_CPSR(cpuflags_t flags);
+ #else
+ #define get_CPSR __get_CPSR
+ #define set_CPSR __set_CPSR
+ #endif
+
+ #define NOP __no_operation()
+ #define IRQ_DISABLE __disable_interrupt()
+ #define IRQ_ENABLE __enable_interrupt()
+
+ #define IRQ_SAVE_DISABLE(x) \
+ do { \
+ (x) = get_CPSR(); \
+ __disable_interrupt(); \
+ } while (0)
+
+ #define IRQ_RESTORE(x) \
+ do { \
+ set_CPSR(x); \
+ } while (0)
+
+ #define IRQ_GETSTATE() \
+ ((bool)(get_CPSR() & 0xb0))
+
+ #define BREAKPOINT /* asm("bkpt 0") DOES NOT WORK */
+
+ #else /* !__IAR_SYSTEMS_ICC__ */
+
+ #define NOP asm volatile ("mov r0,r0" ::)
+
+ #define IRQ_DISABLE \
+ do { \
+ asm volatile ( \
+ "mrs r0, cpsr\n\t" \
+ "orr r0, r0, #0xc0\n\t" \
+ "msr cpsr_c, r0" \
+ ::: "r0" \
+ ); \
+ } while (0)
+
+ #define IRQ_ENABLE \
+ do { \
+ asm volatile ( \
+ "mrs r0, cpsr\n\t" \
+ "bic r0, r0, #0xc0\n\t" \
+ "msr cpsr_c, r0" \
+ ::: "r0" \
+ ); \
+ } while (0)
+
+ #define IRQ_SAVE_DISABLE(x) \
+ do { \
+ asm volatile ( \
+ "mrs %0, cpsr\n\t" \
+ "orr r0, %0, #0xc0\n\t" \
+ "msr cpsr_c, r0" \
+ : "=r" (x) \
+ : /* no inputs */ \
+ : "r0" \
+ ); \
+ } while (0)
+
+ #define IRQ_RESTORE(x) \
+ do { \
+ asm volatile ( \
+ "msr cpsr_c, %0" \
+ : /* no outputs */ \
+ : "r" (x) \
+ ); \
+ } while (0)
+
+ #define IRQ_GETSTATE() \
+ ({ \
+ uint32_t sreg; \
+ asm volatile ( \
+ "mrs %0, cpsr\n\t" \
+ : "=r" (sreg) \
+ : /* no inputs */ \
+ ); \
+ !((sreg & 0xc0) == 0xc0); \
+ })
+
+ #endif /* !__IAR_SYSTEMS_ICC_ */
+
+#elif CPU_PPC
+ #define NOP asm volatile ("nop" ::)
+
+ #define IRQ_DISABLE FIXME
+ #define IRQ_ENABLE FIXME
+ #define IRQ_SAVE_DISABLE(x) FIXME
+ #define IRQ_RESTORE(x) FIXME
+ #define IRQ_GETSTATE() FIXME
+
+ typedef uint32_t cpuflags_t; // FIXME
+ typedef uint32_t cpustack_t; // FIXME
+
+ /* Register counts include SREG too */
+ #define CPU_REG_BITS (CPU_PPC32 ? 32 : 64)
+ #define CPU_REGS_CNT FIXME
+ #define CPU_SAVED_REGS_CNT FIXME
+ #define CPU_STACK_GROWS_UPWARD 0 //FIXME
+ #define CPU_SP_ON_EMPTY_SLOT 0 //FIXME
+ #define CPU_BYTE_ORDER (__BIG_ENDIAN__ ? CPU_BIG_ENDIAN : CPU_LITTLE_ENDIAN)
+ #define CPU_HARVARD 0
+
+#elif CPU_DSP56K
+
+ #define NOP asm(nop)
+ #define BREAKPOINT asm(debug)
+ #define IRQ_DISABLE do { asm(bfset #0x0200,SR); asm(nop); } while (0)
+ #define IRQ_ENABLE do { asm(bfclr #0x0200,SR); asm(nop); } while (0)
+
+ #define IRQ_SAVE_DISABLE(x) \
+ do { (void)x; asm(move SR,x); asm(bfset #0x0200,SR); } while (0)
+ #define IRQ_RESTORE(x) \
+ do { (void)x; asm(move x,SR); } while (0)
+
+ static inline bool irq_running(void)
+ {
+ extern void *user_sp;
+ return !!user_sp;
+ }
+ #define IRQ_RUNNING() irq_running()
+
+ static inline bool irq_getstate(void)
+ {
+ uint16_t x;
+ asm(move SR,x);
+ return !(x & 0x0200);
+ }
+ #define IRQ_GETSTATE() irq_getstate()
+
+ typedef uint16_t cpuflags_t;
+ typedef unsigned int cpustack_t;
+
+ #define CPU_REG_BITS 16
+ #define CPU_REGS_CNT FIXME
+ #define CPU_SAVED_REGS_CNT 8
+ #define CPU_STACK_GROWS_UPWARD 1
+ #define CPU_SP_ON_EMPTY_SLOT 0
+ #define CPU_BYTE_ORDER CPU_BIG_ENDIAN
+ #define CPU_HARVARD 1
+
+ /* Memory is word-addessed in the DSP56K */
+ #define CPU_BITS_PER_CHAR 16
+ #define SIZEOF_SHORT 1
+ #define SIZEOF_INT 1
+ #define SIZEOF_LONG 2
+ #define SIZEOF_PTR 1
+
+#elif CPU_AVR
+
+ #define NOP asm volatile ("nop" ::)
+ #define IRQ_DISABLE asm volatile ("cli" ::)
+ #define IRQ_ENABLE asm volatile ("sei" ::)
+
+ #define IRQ_SAVE_DISABLE(x) \
+ do { \
+ __asm__ __volatile__( \
+ "in %0,__SREG__\n\t" \
+ "cli" \
+ : "=r" (x) : /* no inputs */ : "cc" \
+ ); \
+ } while (0)
+
+ #define IRQ_RESTORE(x) \
+ do { \
+ __asm__ __volatile__( \
+ "out __SREG__,%0" : /* no outputs */ : "r" (x) : "cc" \
+ ); \
+ } while (0)
+
+ #define IRQ_GETSTATE() \
+ ({ \
+ uint8_t sreg; \
+ __asm__ __volatile__( \
+ "in %0,__SREG__\n\t" \
+ : "=r" (sreg) /* no inputs & no clobbers */ \
+ ); \
+ (bool)(sreg & 0x80); \
+ })
+
+ typedef uint8_t cpuflags_t;
+ typedef uint8_t cpustack_t;
+
+ /* Register counts include SREG too */
+ #define CPU_REG_BITS 8
+ #define CPU_REGS_CNT 33
+ #define CPU_SAVED_REGS_CNT 19
+ #define CPU_STACK_GROWS_UPWARD 0
+ #define CPU_SP_ON_EMPTY_SLOT 1
+ #define CPU_BYTE_ORDER CPU_LITTLE_ENDIAN
+ #define CPU_HARVARD 1
+
+ /**
+ * Initialization value for registers in stack frame.
+ * The register index is not directly corrispondent to CPU
+ * register numbers. Index 0 is the SREG register: the initial
+ * value is all 0 but the interrupt bit (bit 7).
+ */
+ #define CPU_REG_INIT_VALUE(reg) (reg == 0 ? 0x80 : 0)
+
+#else
+ #error No CPU_... defined.
+#endif
+
+/**
+ * Execute \a CODE atomically with respect to interrupts.
+ *
+ * \see IRQ_SAVE_DISABLE IRQ_RESTORE
+ */
+#define ATOMIC(CODE) \
+ do { \
+ cpuflags_t __flags; \
+ IRQ_SAVE_DISABLE(__flags); \
+ CODE; \
+ IRQ_RESTORE(__flags); \
+ } while (0)
+
+
+/// Default for macro not defined in the right arch section
+#ifndef CPU_REG_INIT_VALUE
+ #define CPU_REG_INIT_VALUE(reg) 0
+#endif
+
+
+#ifndef CPU_STACK_GROWS_UPWARD
+ #error CPU_STACK_GROWS_UPWARD should have been defined to either 0 or 1
+#endif
+
+#ifndef CPU_SP_ON_EMPTY_SLOT
+ #error CPU_SP_ON_EMPTY_SLOT should have been defined to either 0 or 1
+#endif
+
+/*
+ * Support stack handling peculiarities of a few CPUs.
+ *
+ * Most processors let their stack grow downward and
+ * keep SP pointing at the last pushed value.
+ */
+#if !CPU_STACK_GROWS_UPWARD
+ #if !CPU_SP_ON_EMPTY_SLOT
+ /* Most microprocessors (x86, m68k...) */
+ #define CPU_PUSH_WORD(sp, data) \
+ do { *--(sp) = (data); } while (0)
+ #define CPU_POP_WORD(sp) \
+ (*(sp)++)
+ #else
+ /* AVR insanity */
+ #define CPU_PUSH_WORD(sp, data) \
+ do { *(sp)-- = (data); } while (0)
+ #define CPU_POP_WORD(sp) \
+ (*++(sp))
+ #endif
+
+#else /* CPU_STACK_GROWS_UPWARD */
+
+ #if !CPU_SP_ON_EMPTY_SLOT
+ /* DSP56K and other weirdos */
+ #define CPU_PUSH_WORD(sp, data) \
+ do { *++(sp) = (cpustack_t)(data); } while (0)
+ #define CPU_POP_WORD(sp) \
+ (*(sp)--)
+ #else
+ #error I bet you cannot find a CPU like this
+ #endif
+#endif
+
+
+#if CPU_DSP56K
+ /*
+ * DSP56k pushes both PC and SR to the stack in the JSR instruction, but
+ * RTS discards SR while returning (it does not restore it). So we push
+ * 0 to fake the same context.
+ */
+ #define CPU_PUSH_CALL_CONTEXT(sp, func) \
+ do { \
+ CPU_PUSH_WORD((sp), (func)); \
+ CPU_PUSH_WORD((sp), 0x100); \
+ } while (0);
+
+#elif CPU_AVR
+ /*
+ * In AVR, the addresses are pushed into the stack as little-endian, while
+ * memory accesses are big-endian (actually, it's a 8-bit CPU, so there is
+ * no natural endianess).
+ */
+ #define CPU_PUSH_CALL_CONTEXT(sp, func) \
+ do { \
+ uint16_t funcaddr = (uint16_t)(func); \
+ CPU_PUSH_WORD((sp), funcaddr); \
+ CPU_PUSH_WORD((sp), funcaddr>>8); \
+ } while (0)
+
+#else
+ #define CPU_PUSH_CALL_CONTEXT(sp, func) \
+ CPU_PUSH_WORD((sp), (cpustack_t)(func))
+#endif
+
+
+/**
+ * \name Default type sizes.
+ *
+ * These defaults are reasonable for most 16/32bit machines.
+ * Some of these macros may be overridden by CPU-specific code above.
+ *
+ * ANSI C requires that the following equations be true:
+ * \code
+ * sizeof(char) <= sizeof(short) <= sizeof(int) <= sizeof(long)
+ * sizeof(float) <= sizeof(double)
+ * CPU_BITS_PER_CHAR >= 8
+ * CPU_BITS_PER_SHORT >= 8
+ * CPU_BITS_PER_INT >= 16
+ * CPU_BITS_PER_LONG >= 32
+ * \endcode
+ * \{
+ */
+#ifndef SIZEOF_CHAR
+#define SIZEOF_CHAR 1
+#endif
+
+#ifndef SIZEOF_SHORT
+#define SIZEOF_SHORT 2
+#endif
+
+#ifndef SIZEOF_INT
+#if CPU_REG_BITS < 32
+ #define SIZEOF_INT 2
+#else
+ #define SIZEOF_INT 4
+#endif
+#endif /* !SIZEOF_INT */
+
+#ifndef SIZEOF_LONG
+#if CPU_REG_BITS > 32
+ #define SIZEOF_LONG 8
+#else
+ #define SIZEOF_LONG 4
+#endif
+#endif
+
+#ifndef SIZEOF_PTR
+#if CPU_REG_BITS < 32
+ #define SIZEOF_PTR 2
+#elif CPU_REG_BITS == 32
+ #define SIZEOF_PTR 4
+#else /* CPU_REG_BITS > 32 */
+ #define SIZEOF_PTR 8
+#endif
+#endif
+
+#ifndef CPU_BITS_PER_CHAR
+#define CPU_BITS_PER_CHAR (SIZEOF_CHAR * 8)
+#endif
+
+#ifndef CPU_BITS_PER_SHORT
+#define CPU_BITS_PER_SHORT (SIZEOF_SHORT * CPU_BITS_PER_CHAR)
+#endif
+
+#ifndef CPU_BITS_PER_INT
+#define CPU_BITS_PER_INT (SIZEOF_INT * CPU_BITS_PER_CHAR)
+#endif
+
+#ifndef CPU_BITS_PER_LONG
+#define CPU_BITS_PER_LONG (SIZEOF_LONG * CPU_BITS_PER_CHAR)
+#endif
+
+#ifndef CPU_BITS_PER_PTR
+#define CPU_BITS_PER_PTR (SIZEOF_PTR * CPU_BITS_PER_CHAR)
+#endif
+
+#ifndef BREAKPOINT
+#define BREAKPOINT /* nop */
+#endif
+
+/*\}*/
+
+/* Sanity checks for the above definitions */
+STATIC_ASSERT(sizeof(char) == SIZEOF_CHAR);
+STATIC_ASSERT(sizeof(short) == SIZEOF_SHORT);
+STATIC_ASSERT(sizeof(long) == SIZEOF_LONG);
+STATIC_ASSERT(sizeof(int) == SIZEOF_INT);
+STATIC_ASSERT(sizeof(void *) == SIZEOF_PTR);
+STATIC_ASSERT(sizeof(int8_t) * CPU_BITS_PER_CHAR == 8);
+STATIC_ASSERT(sizeof(uint8_t) * CPU_BITS_PER_CHAR == 8);
+STATIC_ASSERT(sizeof(int16_t) * CPU_BITS_PER_CHAR == 16);
+STATIC_ASSERT(sizeof(uint16_t) * CPU_BITS_PER_CHAR == 16);
+STATIC_ASSERT(sizeof(int32_t) * CPU_BITS_PER_CHAR == 32);
+STATIC_ASSERT(sizeof(uint32_t) * CPU_BITS_PER_CHAR == 32);
+#ifdef __HAS_INT64_T__
+STATIC_ASSERT(sizeof(int64_t) * CPU_BITS_PER_CHAR == 64);
+STATIC_ASSERT(sizeof(uint64_t) * CPU_BITS_PER_CHAR == 64);
+#endif
+
+/**
+ * \def CPU_IDLE
+ *
+ * \brief Invoked by the scheduler to stop the CPU when idle.
+ *
+ * This hook can be redefined to put the CPU in low-power mode, or to
+ * profile system load with an external strobe, or to save CPU cycles
+ * in hosted environments such as emulators.
+ */
+#ifndef CPU_IDLE
+ #if defined(ARCH_EMUL) && (ARCH & ARCH_EMUL)
+ /* This emulator hook should yield the CPU to the host. */
+ EXTERN_C_BEGIN
+ void emul_idle(void);
+ EXTERN_C_END
+ #define CPU_IDLE emul_idle()
+ #else /* !ARCH_EMUL */
+ #define CPU_IDLE do { /* nothing */ } while (0)
+ #endif /* !ARCH_EMUL */
+#endif /* !CPU_IDLE */
+
+#endif /* CPU_CPU_H */
--- /dev/null
+#!/bin/bash
+
+DIRS="drv hw io scripts"
+if [ $# != 1 ]; then
+ echo "Create a new core tree with subdirs:"
+ echo $DIRS
+ echo "usage $0 <core>"
+ exit 1
+fi
+CORE=$1
+mkdir $CORE
+cd $CORE
+for dir in $DIRS
+do
+ mkdir $dir
+done
+cd ..