106 lines
3.3 KiB
C
106 lines
3.3 KiB
C
/***************************************************************************
|
|
* Copyright (C) 01/2008 by Olaf Rempel *
|
|
* razzor@kopf-tisch.de *
|
|
* *
|
|
* This program 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; version 2 of the License *
|
|
* *
|
|
* 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., *
|
|
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
|
|
***************************************************************************/
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
#include "AT91SAM7S256.h"
|
|
#include "board.h"
|
|
#include "at91_sysc.h"
|
|
#include "at91_dbgu.h"
|
|
#include "memalloc.h"
|
|
|
|
uint32_t * init_ctx(uint32_t *stack, void *code, void *arg)
|
|
{
|
|
uint32_t org_stack = (uint32_t)stack;
|
|
|
|
*stack-- = (uint32_t) code; // r15 (pc)
|
|
*stack-- = 0x14141414; // r14 (lr)
|
|
*stack-- = org_stack -4; // r13 (sp)
|
|
*stack-- = 0x12121212; // r12
|
|
*stack-- = 0x11111111; // r11
|
|
*stack-- = 0x10101010; // r10
|
|
*stack-- = 0x09090909; // r9
|
|
*stack-- = 0x08080808; // r8
|
|
*stack-- = 0x07070707; // r7
|
|
*stack-- = 0x06060606; // r6
|
|
*stack-- = 0x05050505; // r5
|
|
*stack-- = 0x04040404; // r4
|
|
*stack-- = 0x03030303; // r3
|
|
*stack-- = 0x02020202; // r2
|
|
*stack-- = 0x01010101; // r1
|
|
*stack-- = (uint32_t) arg; // r0 (function parameter)
|
|
*stack = 0xF0000013; // SPSR (SVC, ARM, IRQ & FIQ enabled)
|
|
|
|
return stack;
|
|
}
|
|
|
|
/* we're in the scheduler, SVC mode */
|
|
void restore_ctx(uint32_t *stack)
|
|
{
|
|
asm volatile (
|
|
/* restore spsr */
|
|
"mov lr, r0 \n\t"
|
|
"ldmia lr!, {r0} \n\t"
|
|
"msr spsr, r0 \n\t"
|
|
|
|
/* restore all registers */
|
|
"ldmia lr, {r0-r13} \n\t"
|
|
"nop \n\t"
|
|
"ldmia sp!, {r14-r15}^ \n\t"
|
|
"nop \n\t"
|
|
);
|
|
}
|
|
|
|
|
|
void store_ctx(void)
|
|
{
|
|
asm volatile (
|
|
/* push r0 on (task)stack */
|
|
"stmdb sp!, {r0} \n\r"
|
|
|
|
// TODO: fill r0 with address TCB
|
|
|
|
/* return address */
|
|
"stmdb r0!, {lr} \n\r"
|
|
);
|
|
}
|
|
|
|
void testfunc(void *p)
|
|
{
|
|
printf("bla: %p\n\r", p);
|
|
while(1);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
/* LED outputs */
|
|
*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
|
|
*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
|
|
|
|
at91_sysc_init();
|
|
at91_dbgu_init();
|
|
at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
|
|
|
|
uint32_t *blub = static_alloc(1024);
|
|
printf("blub: %p\n\r", blub);
|
|
|
|
uint32_t *tmp = init_ctx(blub + (1024/4), testfunc, 0);
|
|
restore_ctx(tmp);
|
|
}
|