/***************************************************************************
 *   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();
}