// Cygnal C2 Interface Adapter
// for AT90S2313

#include <io2313.h>
#include <iomacros.h>
#include <sig-avr.h>
#include <interrupt.h>

#include "c2.h"
#include "config.h"

#define VERSION 0x200

#define NULL 0
typedef unsigned char bool;
enum { false, true };

extern volatile unsigned char timerevent;


void waitloop(unsigned int n)
{
	do {
		asm("nop");
		asm("nop");
		asm("nop");
		asm("nop");
	} while (--n);
}

#define wait_2us()	waitloop(1)
#define wait_20us()	waitloop(20 * CLOCK / 8000000)
#define wait_20ms()	waitloop(20 * CLOCK / 8000)


struct Queue {
	unsigned char pos;
	unsigned char len;
	unsigned char buf[8];
};

void qinit(struct Queue *q)
{
	q->pos = 0;
	q->len = 0;
}

inline int qpeek(struct Queue *q)
{
	return q->len > 0 ? (long)q->buf[q->pos] : -1;
}

int qget(struct Queue *q)
{
	int c = qpeek(q);
	if (c >= 0) {
		q->pos = (q->pos + 1) % sizeof(q->buf);
		q->len--;
	}
	return c;
}

bool qput(struct Queue *q, int c)
{
	if (q->len < sizeof(q->buf)) {
		q->buf[(q->pos + q->len) % sizeof(q->buf)] = c;
		q->len++;
		return true;
	}
	return false;
}

struct Queue uart_rx_queue;
struct Queue uart_tx_queue;

SIGNAL(SIG_UART_RECV)
{
	if (inp(USR) & 0x80) {
		int c = inp(UDR);
		qput(&uart_rx_queue, c);
	}
}

SIGNAL(SIG_UART_DATA)
{
	if (inp(USR) & 0x20) {
		int c = qget(&uart_tx_queue);
		if (c >= 0) {
			outp(c, UDR);
		}
		if (qpeek(&uart_tx_queue) < 0) {	// if tx buffer is empty
			outp(inp(UCR) & ~0x20, UCR);	// disable interrupt
		}
	}
}



bool uart_tx_capable(unsigned char n)
{
	return uart_tx_queue.len + n < sizeof(uart_tx_queue.buf);
}

bool uart_put(int c)
{
	bool f;
	cli();
	f = qput(&uart_tx_queue, c);
	outp(inp(UCR) | 0x20, UCR);		// UDRIE
	sei();
	return f;
}

int uart_get()
{
	int c;
	cli();
	c = qget(&uart_rx_queue);
	sei();
	return c;
}

void uart_putchar(int c)
{
	while (!uart_put(c));
}


void uart_putcrlf()
{
	uart_putchar(13);
	uart_putchar(10);
}

#if 0
void _uart_puthex(unsigned char n)
{
	n &= 15;
	uart_putchar((n < 10 ? '0' : ('a' - 10)) + n);
}

void uart_puthex8(unsigned char n)
{
	_uart_puthex(n >> 4);
	_uart_puthex(n);
}

void uart_puthex16(unsigned short n)
{
	uart_puthex8(n >> 8);
	uart_puthex8(n);
}
#endif

//

bool c2GetData()
{
	return (inp(PIND) & 0x08) ? true : false;
}

void c2SetData0()
{
//	outp(inp(DDRD) | 0x08, DDRD);
//	outp(inp(PIND) & ~0x08, PORTD);
	asm("in r24,0x10");		// PIND
	asm("cbr r24,0x08");
	asm("sbi 0x11,3");		// DDRD
	asm("out 0x12,r24");	// PORTD
}
void c2SetData1()
{
//	outp(inp(PIND) | 0x08, PORTD);
//	outp(inp(DDRD) & ~0x08, DDRD);
	asm("in r24,0x10");		// PIND
	asm("sbr r24,0x08");
	asm("out 0x12,r24");	// PORTD
	asm("cbi 0x11,3");		// DDRD
}

void c2SetClock0()
{
//	outp(inp(DDRD) | 0x04, DDRD);
//	outp(inp(PIND) & ~0x04, PORTD);
	asm("in r24,0x10");		// PIND
	asm("cbr r24,0x04");
	asm("sbi 0x11,2");		// DDRD
	asm("out 0x12,r24");	// PORTD
}
void c2SetClock1()
{
//	outp(inp(PIND) | 0x04, PORTD);
//	outp(inp(DDRD) & ~0x04, DDRD);
	asm("in r24,0x10");		// PIND
	asm("sbr r24,0x04");
	asm("out 0x12,r24");	// PORTD
	asm("cbi 0x11,2");		// DDRD
}

void c2Strobe()
{
//	c2SetClock0();
//	c2SetClock1();
	asm("sbi 0x11,2");		// DDRD
	asm("in r24,0x10");		// PIND
	asm("mov r25,r24");
	asm("cbr r24,0x04");
	asm("sbr r25,0x04");
	asm("out 0x12,r24");	// PORTD c2ck to L
	asm("out 0x12,r25");	// PORTD c2ck to H
	asm("cbi 0x11,2");		// DDRD
}

void c2Reset()
{
	c2SetClock0();
	wait_20us();
	c2SetClock1();
}


bool c2ReadCycle()
{
	bool f;
	cli();
	f = c2GetData();
	c2Strobe();
	sei();
	return f;
}

void c2WriteCycle(bool f)
{
	cli();
	if (f)	c2SetData1();
	else	c2SetData0();
	c2Strobe();
	sei();
}


void c2WaitWhileBusy()
{
	unsigned char i;
	
	c2SetData1();
	for (i = 250; i > 0; i--) {
		if (c2ReadCycle()) break;
	}
}

void c2OutputBits(unsigned char data, unsigned char len)
{
	while (len > 0) {
		c2WriteCycle(data & 1);
		data >>= 1;
		len--;
	}
}

unsigned char c2ByteInput()
{
	unsigned char i;
	unsigned char c = 0;
	for (i = 0; i < 8; i++) {
		c >>= 1;
		if (c2ReadCycle())
			c |= 0x80;
	}
	return c;
}

//

unsigned char c2ReadAR()
{
	unsigned char addr;

    //                  1 0 1 1
	// START field      ^
	// INS field          ^ ^
	// dummy                  ^
	c2OutputBits(0x0d, 4);

	// ADDRESS field
	addr = c2ByteInput();

	// STOP field
	c2SetData1();

	return addr;
}

void c2WriteAR(unsigned char addr)
{
    //                  1 1 1 x x x x x x x x
	// START field      ^
	// INS field          ^ ^
	// ADDRESS field          ^ ^ ^ ^ ^ ^ ^ ^
	c2OutputBits(0x07, 3);
	c2OutputBits(addr, 8);

	// dummy
	c2WriteCycle(true);

	// STOP field
	c2SetData1();
}

unsigned char c2ReadDR()
{
	unsigned char data;

    //                  1 0 0 0 0 1
	// START field      ^
	// INS field          ^ ^
	// LENGTH field           ^ ^
	// dummy                      ^
	c2OutputBits(0x21, 6);

	// WAIT field
	c2WaitWhileBusy();

	// DATA field
	data = c2ByteInput();

	// STOP field
	c2SetData1();

	return data;
}

void c2WriteDR(unsigned char data)
{
    //                  1 1 0 0 0 x x x x x x x x
	// START field      ^
	// INS field          ^ ^
	// LENGTH field           ^ ^
	// DATA field                 ^ ^ ^ ^ ^ ^ ^ ^
	c2OutputBits(0x03, 5);
	c2OutputBits(data, 8);

	// dummy
	c2WriteCycle(true);

	// WAIT field
	c2WaitWhileBusy();

	// STOP field
	c2SetData1();
}

//

void c2FlashProgramMode()
{
	c2Reset();
	wait_2us();
	c2WriteAR(C2_R_FPCTL);
	c2WriteDR(2);
	c2WriteDR(1);
	wait_20ms();
}

void _c2_poll_out_ready()
{
	unsigned char i;
	for (i = 250; i > 0; i--) {
		if (c2ReadAR() & 1) break;
	}
}

void _c2_poll_in_busy()
{
	unsigned char i;
	for (i = 250; i > 0; i--) {
		if (!(c2ReadAR() & 2)) break;
	}
}


bool _write_command(unsigned char command)
{
	c2WriteAR(C2_R_FPDAT);
	c2WriteDR(command);
	_c2_poll_in_busy();
	_c2_poll_out_ready();
	return (c2ReadDR() == C2_S_COMMAND_OK);
}

void _write_address_length(unsigned short addr, unsigned char len)
{
	c2WriteDR((unsigned char)(addr >> 8));
	_c2_poll_in_busy();
	c2WriteDR((unsigned char)addr);
	_c2_poll_in_busy();
	c2WriteDR(len);
	_c2_poll_in_busy();
}

bool _block_read(unsigned char *ptr, unsigned short addr, unsigned char len)
{
	if (!_write_command(C2_C_BLOCK_READ)) {
		return false;
	}
	_write_address_length(addr, len);

	_c2_poll_out_ready();
	if (c2ReadDR() != C2_S_COMMAND_OK) {
		return false;
	}

	while (len > 0) {
		if (c2ReadAR() & 1) {
			*ptr++ = c2ReadDR();
			len--;
		}
	}

	return true;
}

bool _block_write(const unsigned char *ptr, unsigned short addr, unsigned char len)
{
	if (!_write_command(C2_C_BLOCK_WRITE)) {
		return false;
	}
	_write_address_length(addr, len);

	_c2_poll_out_ready();
	if (c2ReadDR() != C2_S_COMMAND_OK) {
		return false;
	}

	while (len > 0) {
		c2WriteDR(*ptr);
		_c2_poll_in_busy();
		ptr++;
		len--;
	}
	_c2_poll_out_ready();

	return true;
}




void uart_putstr(const char *ptr)
{
	while (*ptr) uart_putchar(*ptr++);
}


int main()
{
	int c, n;
	unsigned char linelen;
	unsigned char linebuf[35];
	unsigned char mostbits;

	outp(0xff, DDRB);
	outp(0x00, PORTB);

	outp(0x16, DDRD);
	outp(0x00, PORTD);

	outp(0x98, UCR);							// 8bit
	outp(CLOCK / 16 / BAUDRATE - 1, UBRR);		// baud rate

	qinit(&uart_rx_queue);
	qinit(&uart_tx_queue);

	sei();

	//

	c2SetClock1();
	c2SetData1();

	linelen = 0;
	mostbits = 0;

	while (1) {
		c = uart_get();
		if (c >= 0) {
			if (c < 0x80) {
				switch (c) {
				case C2_A_NONE:
					linelen = 0;
					mostbits = 0;
					break;
				case C2_A_ECHO:
					uart_putchar('!');
					break;
				case C2_A_GET_VERSION:
					uart_putchar('C');
					uart_putchar('2');
					uart_putchar(VERSION >> 8);
					uart_putchar(VERSION);
					break;
				case C2_A_HELLO_WORLD:
					uart_putstr("Hello, world\r\n");
					break;
				case C2_A_RESET:
					c2Reset();
					break;
				case C2_A_WRITE_ADDRESS:
					c2WriteAR(linebuf[0]);
					break;
				case C2_A_READ_ADDRESS:
					n = c2ReadAR();
					uart_putchar(n);
					break;
				case C2_A_WRITE_DATA:
					c2WriteDR(linebuf[0]);
					break;
				case C2_A_READ_DATA:
					n = c2ReadDR();
					uart_putchar(n);
					break;
				case C2_A_FLASH_PROGRAM_MODE:
					c2FlashProgramMode();
					uart_put(0);
					break;
				case C2_A_TRANSFER:
					uart_putchar(linelen);
					for (n = c = 0; n < linelen; n++) {
						uart_putchar(linebuf[n]);
						c += linebuf[n];
					}
					uart_putchar(c);
					break;
				case C2_A_CHECKSUM:
					c = 0;
					for (n = 0; n < linelen; n++) {
						c += linebuf[n];
					}
					uart_putchar(c);
					break;
				case C2_A_BLOCK_WRITE:
					{
						unsigned short addr = (linebuf[0] << 8) | linebuf[1];
						unsigned char len = linebuf[2];
						if (len > 1 && len <= sizeof(linebuf) - 3) {
							if (_block_write(linebuf + 3, addr, len)) {
								uart_putchar(0);
							}
						}
					}
					break;
				case C2_A_BLOCK_READ:
					{
						unsigned short addr = (linebuf[0] << 8) | linebuf[1];
						linelen = linebuf[2];
						if (_block_read(linebuf, addr, linelen)) {
							uart_putchar(0);
						} else {
							linelen = 0;
						}
					}
					break;
				}
			} else {
				if (!mostbits) {
					mostbits = (c << 1) | 1;
				} else if (linelen < sizeof(linebuf)) {
					linebuf[linelen] = (c & 0x7f) | (mostbits & 0x80);
					linelen++;
					mostbits <<= 1;
					if (mostbits == 0x80) mostbits = 0;
				}
			}
		}
	}
	return 0;
}