175 lines
4.4 KiB
C
175 lines
4.4 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 <string.h>
|
|
|
|
#include "AT91SAM7S256.h"
|
|
#include "board.h"
|
|
#include "at91_sysc.h"
|
|
#include "at91_dbgu.h"
|
|
#include "memalloc.h"
|
|
|
|
struct register_context {
|
|
uint32_t r0;
|
|
uint32_t r1;
|
|
uint32_t pc;
|
|
uint32_t cpsr;
|
|
uint32_t r2;
|
|
uint32_t r3;
|
|
uint32_t r4;
|
|
uint32_t r5;
|
|
uint32_t r6;
|
|
uint32_t r7;
|
|
uint32_t r8;
|
|
uint32_t r9;
|
|
uint32_t r10;
|
|
uint32_t r11;
|
|
uint32_t r12;
|
|
uint32_t sp;
|
|
uint32_t r14;
|
|
};
|
|
|
|
struct context {
|
|
struct register_context regs;
|
|
uint32_t stacksize;
|
|
};
|
|
|
|
struct context * create_ctx(uint32_t stacksize, void *code, void *arg)
|
|
{
|
|
void *stack = static_alloc(sizeof(struct context) + stacksize);
|
|
memset(stack, 0, stacksize);
|
|
|
|
struct context *ctx = (struct context *)((uint8_t *)stack + stacksize);
|
|
ctx->regs.r0 = (uint32_t)arg;
|
|
ctx->regs.pc = (uint32_t)code;
|
|
ctx->regs.cpsr = 0x0000001F;
|
|
ctx->regs.sp = (uint32_t)ctx;
|
|
|
|
ctx->regs.r1 = 0x01010101;
|
|
ctx->regs.r2 = 0x02020202;
|
|
ctx->regs.r3 = 0x03030303;
|
|
ctx->regs.r4 = 0x04040404;
|
|
ctx->regs.r5 = 0x05050505;
|
|
ctx->regs.r6 = 0x06060606;
|
|
ctx->regs.r7 = 0x07070707;
|
|
ctx->regs.r8 = 0x08080808;
|
|
ctx->regs.r9 = 0x09090909;
|
|
ctx->regs.r10 = 0x10101010;
|
|
ctx->regs.r11 = 0x11111111;
|
|
ctx->regs.r12 = 0x12121212;
|
|
ctx->regs.r14 = 0x14141414;
|
|
|
|
return ctx;
|
|
}
|
|
|
|
struct context *current_ctx;
|
|
|
|
/* we're in the scheduler, SVC mode */
|
|
void restore_ctx(void)
|
|
{
|
|
asm volatile (
|
|
// debug: assume we're in svc mode
|
|
"msr cpsr_c, 0x13 \n\r"
|
|
|
|
"ldr r0, =current_ctx \n\t"
|
|
"ldr r0, [r0] \n\t"
|
|
|
|
// get r0, r1 and pc(in lr)
|
|
"ldmia r0!, {r1-r2,r14} \n\t"
|
|
|
|
// put r0, r1 on (svc)stack
|
|
"stmdb sp!, {r1-r2} \n\t"
|
|
|
|
// get spsr(in r1), r2-r14 (usermode!)
|
|
"ldmia r0, {r1-r14}^ \n\t"
|
|
"nop \n\t"
|
|
|
|
// store spsr
|
|
"msr spsr, r1 \n\t"
|
|
|
|
// get r0 & r1 from (svc)stack
|
|
"ldmia sp!, {r0, r1} \n\t"
|
|
|
|
// jump & restore cpsr from spsr
|
|
"movs pc, lr \n\t"
|
|
);
|
|
}
|
|
|
|
__attribute__((naked)) void SWI_Handler(void)
|
|
{
|
|
asm volatile (
|
|
"stmdb sp!, {r0-r1} \n\t"
|
|
|
|
"ldr r0, =current_ctx \n\t"
|
|
"ldr r0, [r0] \n\t"
|
|
"add r0, r0, #68 \n\t"
|
|
|
|
"mrs r1, spsr \n\t"
|
|
|
|
"stmdb r0, {r1-r14}^ \n\t"
|
|
"nop \n\t"
|
|
"sub r0, r0, #56 \n\t"
|
|
|
|
"ldmia sp!, {r1, r2} \n\t"
|
|
"stmdb r0!, {r1, r2, r14} \n\t"
|
|
);
|
|
|
|
printf("swi: ok\n\r");
|
|
|
|
asm volatile (
|
|
"ldr r0, =current_ctx \n\t"
|
|
"ldr r0, [r0] \n\t"
|
|
|
|
"ldmia r0!, {r1-r2,r14} \n\t"
|
|
"stmdb sp!, {r1-r2} \n\t"
|
|
|
|
"ldmia r0, {r1-r14}^ \n\t"
|
|
"nop \n\t"
|
|
|
|
"msr spsr, r1 \n\t"
|
|
"ldmia sp!, {r0, r1} \n\t"
|
|
"movs pc, lr \n\t"
|
|
);
|
|
}
|
|
|
|
void testfunc(void *p)
|
|
{
|
|
printf("testfunc: %p\n\r", p);
|
|
|
|
asm volatile ("swi");
|
|
|
|
printf("testfunc: %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");
|
|
|
|
current_ctx = create_ctx(1024, testfunc, 0);
|
|
restore_ctx();
|
|
}
|