/*
** Copyright 2001, Brian Swetland <swetland@frotz.net>
** All Rights Reserved.  Please read the provided LICENSE file.
*/

#include <xproto.h>

void dputchar(int c);

static int xcmd_ping(xframe *f)
{
	return 0;
}

static int xcmd_read(xframe *f)
{
	int i;
	u1 *ptr;
	u4 len;

	if(f->len != 16) return -1;
	
	ptr = (u1*) f->u.w[0];
	len = f->u.w[1];

	if(len > MAXDATA) return -1;

	if(f->cmd == XCMD_READ32) {
		if((len & 3) || (((u4) ptr) & 3)) return -1;
		for(i = 0; i < (len/4); i++) {
			f->u.w[i] = *((u4*)ptr);
			ptr += 4;
		}
	} else if(f->cmd == XCMD_READ16) {
		if((len & 1) || (((u4) ptr) & 1)) return -1;
		for(i = 0; i < (len/2); i++) {
			f->u.h[i] = *((u2*)ptr);
			ptr += 2;
		}
	} else {
		for(i = 0; i < len; i++){
			f->u.b[i] = *ptr++;
		}
	}
	return len;
}

static int xcmd_write(xframe *f)
{
	int i;
	u1 *ptr;
	u4 len;

	if(f->len < 16) return -1;
	
	ptr = (u1*) f->u.w[0];
	len = f->u.w[1];

	if(len != (f->len - 16)) return -1;

	if(f->cmd == XCMD_WRITE32) {
		if((len & 3) || (((u4) ptr) & 3)) return -1;
		for(i = 0; i < (len/4); i++) {
			*((u4*)ptr) = f->u.w[2 + i];
			ptr += 4;
		}
	} else if(f->cmd == XCMD_WRITE16) {
		if((len & 1) || (((u4) ptr) & 1)) return -1;
		for(i = 0; i < (len/2); i++) {
			*((u2*)ptr) = f->u.h[4 + i];
			ptr += 2;
		}
	} else {
		for(i = 0; i < len; i++){
			*ptr++ = f->u.b[8 + i];
		}
	}
	return len;
}

#if FLASH
void boot(u4 addr);
#endif

static int xcmd_call(xframe *f)
{
#ifndef FLASH
	void (*func)();
#endif
	
	if(f->len != 12) return -1;
#if FLASH
	boot(f->u.w[0]);
#else
	func = (void(*)()) f->u.w[0];
	func();
#endif
	return 0;	
}

void reboot();

static int xcmd_reboot(xframe *f)
{
	reboot();
	return 0;
}

#ifdef FLASH

#include "cartlib.h"

extern short rescan_flash;

static int xcmd_flash(xframe *f)
{
	u4 addr, len;
	int res;
	
	if(f->len != 20) return -1;

	addr = f->u.w[1];
	len = f->u.w[2];

	if(get_pc() >= 0x08000000) {
		printf("WRITE FLASH: oops! running from rom!\n");
		return -1;
	}
	
	if(addr <  0x08000000) return -1;
	if(addr >= 0x0c000000) return -1;
	if(addr &  0x0003ffff) return -1;
	
	SetVisolyFlashRWMode();
	
	switch(f->u.w[0]){
	case FLASH_WRITE:
		if(len & 63) len = (len + 63) & (~64);
		printf("FLASH: write %08x @%08x\n", len, addr);
		res = WriteTurboFACart(0x02000000, addr, len / 64);
		if(res) {
			u4 *src, *dst;
			printf("FLASH: write OKAY\n");
			src = (u4*) 0x02000000;
			dst = (u4*) addr;
			while(len > 0) {
				if(*src++ != *dst++) {
					printf("FLASH: verify FAIL @ %p\n",dst);
					goto oops;
				}
				len -= 4;
			}
			printf("FLASH: verify OKAY\n");
		} else {
			printf("FLASH: write FAIL\n");
		}
		break;
		
	case FLASH_ERASE:
		printf("FLASH: erase %08x @%08x\n", 0x40000, addr);
		if(EraseTurboFABlocks(addr, 1)){
			printf("FLASH: erase OKAY\n");
		} else {
			printf("FLASH: erase FAIL\n");
		}
		break;

	default:
		return -1;
	}

oops:
	rescan_flash = 1;
	return 0;	
}

void VisolyModePreamble();

int xcmd_visoly(xframe *f)
{
	if(f->len != 16) return -1;

	VisolyModePreamble();
	*((u1*) f->u.w[0]) = f->u.w[1];
	
	return 0;
}

#endif

xcmdmap xcommands[] = {
	{ XCMD_WRITE32, xcmd_write },
	{ XCMD_WRITE16, xcmd_write },
	{ XCMD_WRITE8, xcmd_write },
	{ XCMD_READ32, xcmd_read },
	{ XCMD_READ16, xcmd_read },
	{ XCMD_READ8, xcmd_read },
	{ XCMD_CALL, xcmd_call },
	{ XCMD_PING, xcmd_ping },
	{ XCMD_REBOOT, xcmd_reboot },
#ifdef FLASH
	{ XCMD_FLASH, xcmd_flash },
	{ XCMD_VISOLY, xcmd_visoly },
#endif
	{ 0, 0 }
};

inline void xputchar(int c)
{
	dputchar(c);
}

int xio_wait(XIO *xio, xframe *f)
{
	return 0;
}

void xio_out(const u1 *data, int len)
{
	u1 x;

	xputchar(XFRAME);
	while(len > 0) {
		x = *data++;
		if(x == 0xae) {
			xputchar(XESCAPE);
			xputchar(XESC_AE);
		} else if(x == 0xad){
			xputchar(XESCAPE);
			xputchar(XESC_AD);
		} else {
			xputchar(x);
		};
		len--;
	}
	xputchar(XFRAME);
}