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