#include <pic.h>

#include <stdio.h>
//#include <pic1667.h>
#include "delay.h"

__CONFIG (0x3F72);

#define BUFF_SIZE 16
unsigned char RxBuff[BUFF_SIZE], RxHead, RxTail;
bank1 unsigned char RecBuff[64];
bank1 unsigned char RecHead, RecTail;

unsigned char sComRdy, sRecord, sPlayback, sBusFree, highest, lowest, CurrBuff;
unsigned int NumCyl, NumHead, NumSect;
unsigned int CurrCyl, CurrHead, CurrSect;

static unsigned char IDE_DATA_LOW @ (unsigned)&PORTB;
static unsigned char IDE_DATA_HIGH @ (unsigned)&PORTD;
static unsigned char IDE_ADDR @ (unsigned)&PORTE;
static bit IDE_WR @ (unsigned)&PORTC*8+0;
static bit IDE_RD @ (unsigned)&PORTC*8+1;
static bit IDE_CS0 @ (unsigned)&PORTC*8+2;
static bit IDE_CS1 @ (unsigned)&PORTC*8+3;
static bit IDE_RST @ (unsigned)&PORTC*8+4;
static bit DAC_WR @ (unsigned)&PORTC*8+5;

// IDE interface registers
#define	IDE_DATAREG		0x10
#define IDE_ERRORREG	0x11
#define IDE_SCOUNTREG	0x12
#define IDE_SSTARTREG	0x13
#define IDE_CYLREGL		0x14
#define IDE_CYLREGH		0x15
#define IDE_HEADREG		0x16
#define IDE_COMMANDREG	0x17
#define IDE_STATUSREG	0x17
#define IDE_INTREG		0x0E

// IDE command register commands
#define	IDE_RECAL		0x10
#define IDE_READRTY		0x20
#define IDE_READ		0x21
#define IDE_WRITERTY	0x30
#define IDE_WRITE		0x31
#define IDE_IDENTIFY	0xEC
#define IDE_SPINDOWN	0xE0
#define IDE_SPINUP		0xE1

// IDE status register bits
#define IDE_READY		0x40
#define	IDE_BUSY		0x80
#define IDE_DRQ			0x08


void 			init(void);
void 			putch(char c);
void interrupt 	isr(void);
unsigned char 	IDE_Status(void);
void			IDE_Command(unsigned char);
unsigned char	IDE_Read(void);
unsigned char	IDE_Write(void);
void			IDE_WriteReg(unsigned char, unsigned char);
unsigned char	IDE_ReadReg(unsigned char);
void			IDE_WriteData(unsigned int);
unsigned int	IDE_ReadData(void);
void			IDE_DumpSector(void);
void			IDE_Config(void);
void			DAC_Write(unsigned char);
unsigned char	char2hex(unsigned char);
char			IncCurrPos(void);
void			LoadCurrPos(void);

/* Note to self:
	Don't use a function call as a parameter to a printf call
*/

void IDE_Config(void)
{
	unsigned int count=0, data;
	
	while (!(IDE_Status() & IDE_READY) && (count++<3000)) 
		DelayMs(1);
	
	if (count == 3000) {
		printf("\n\rDrive not ready in 3s, check connections etc.");
		while (1);
	}

	IDE_WriteReg(IDE_INTREG, 0x01);
	IDE_WriteReg(IDE_HEADREG, 0xA0);

	// identify drive command
	while (IDE_Status() & IDE_BUSY) ;
	IDE_Command(IDE_IDENTIFY);
	while (IDE_Status() & IDE_BUSY) ;
	IDE_ReadData();
	NumCyl = IDE_ReadData();
	IDE_ReadData();
	NumHead = IDE_ReadData();
	IDE_ReadData();
	IDE_ReadData();
	NumSect = IDE_ReadData();
			
	printf("\n\rDrive detected, model : ");
	for (count=0; count<20; count++)	// skip
		IDE_ReadData();
	for (count=0; count<20; count++)
	{
		data = IDE_ReadData();
		putch((data & 0xFF00) >> 8);
		putch(data & 0x00FF);
	}
	printf("\n\r C/H/S = %d / %d / %d", NumCyl, NumHead, NumSect);

	IDE_RST = 0;
	DelayMs(50);
	IDE_RST = 1;
}

void interrupt isr(void)
{

	if (TXIF) {
		TXIF = 0;
	}

	if (RCIF) {	// does not check for overflow
		if ((RxBuff[RxHead++] = RCREG) == 0x0D)
		{
			sComRdy = 1;
			RxHead = 0;
		}
		if (RxHead == BUFF_SIZE) RxHead = 0;
		RCIF = 0;
	}

	if (T0IF) {
		TMR0 = 219;
		if (sRecord == 1) ADGO = 1;
		if (sPlayback == 1) 
		{
			TRISB = 0x00;
			PORTB = RecBuff[RecTail];
			DAC_WR = 0;
			DAC_WR = 1;
			if (++RecTail == 64) RecTail = 0;
			TRISB = 0xFF;
			sBusFree = 1;
		}
		T0IF = 0;
	}

	if (ADIF) {
		if (sRecord == 1)
		{
			RecBuff[RecHead++] = ADRESH;
			if (RecHead == 64) RecHead = 0;
		}
		ADIF = 0;
	}
}

unsigned char char2hex(unsigned char ch)
{
	if ((ch >= '0') && (ch <= '9'))
		return (ch - '0');
	if ((ch >= 'a') && (ch <= 'f'))
	{
		ch -= 'a';
		ch += 10;
		return (ch);
	}
	return 0;
}

void Record()
{
	unsigned int data, count;

	RecHead = RecTail = 0;
	CurrSect = CurrHead = CurrCyl = 1;
	sRecord = 1;
				
	do
	{
		if (IDE_Status() & IDE_DRQ) 
			printf("\n\rRecord didn't write full sector.");

		while (IDE_Status() & IDE_BUSY) ;

		LoadCurrPos();
		IDE_Command(IDE_WRITE);
//		printf("\n\r%d, %d, %d, %c", CurrCyl, CurrHead, CurrSect, (count==512)?'g':'b');
					
		while (!(IDE_Status() & IDE_DRQ)) ;
					
		for (count=0; count<512; count++)
		{
			while (RecHead==RecTail) ;		// wait for buffer to advance

			while (!(IDE_Status() & IDE_DRQ)) ;	// wait for disk to be ready
						
			if (count & 0x0001) // high byte
			{
				data += (RecBuff[RecTail] << 8);
				IDE_WriteData(data);
			//	IDE_WriteData(count);
			}
			else data = RecBuff[RecTail];

			if (++RecTail == 64) RecTail = 0;
		}
					
	} while (!IncCurrPos() && CurrHead != 4);

	RecHead = RecTail = 0;
	sRecord = 0;

}

void Playback()
{
	unsigned int data, count;

	while (IDE_Status() & IDE_BUSY) ;
				
	CurrHead = CurrSect = CurrCyl = 1;
	RecHead = RecTail = 0;
	sBusFree = 0;
	sPlayback = 0;
				
	do
	{
		if (sPlayback) while (!sBusFree) ;
		sBusFree = 0;

		LoadCurrPos();
		IDE_Command(IDE_READ);
//		printf("\n\r%d, %d, %d, %c", CurrCyl, CurrHead, CurrSect, (count==256)?'g':'b');
		while (!(IDE_Status() & IDE_DRQ)) ;
					
		for (count=0; count<256; count++)
		{
			if (sPlayback) while (!sBusFree) ;
			data = IDE_ReadData();
			sBusFree = 0;

			if (sPlayback) while (RecHead == RecTail) ;
			RecBuff[RecHead++] = data & 0x00FF;
			if (sPlayback) while (RecHead == RecTail) ;						
			RecBuff[RecHead++] = (data & 0xFF00) >> 8;
			if (RecHead == 32) sPlayback = 1;
			if (RecHead == 64) RecHead = 0;
		}
					
	} while (!IncCurrPos() && CurrHead != 4);

	sPlayback = 0;
}


void main(void)
{

	unsigned char addr, val;
	unsigned int data, count;
	
	init();	

	printf("\n\rIDE DISK DRIVER\n\r");

	IDE_Config();
	
	while(1)
	{
		printf("\n\r>");
		while (!sComRdy) ;	// wait for command

		switch (RxBuff[0]) {
			case 'h':
				printf("\n\rHelp:");
				printf("\n\rs\tStatus Reg");
				printf("\n\rwaavv\tWrite vv to register aa");
				printf("\n\rraa\tRead register aa");
				printf("\n\rd\tDump data sector buffer");
				printf("\n\rfvv\tWrite data buffer with vv");
				printf("\n\rcvv\tWrite command vv");
				printf("\n\rz\tReset drive");
				printf("\n\ra\tADC current value");
				printf("\n\rovv\tWrite vv to DAC");
				printf("\n\rR\tStart recording");
				printf("\n\rP\tPlayback");
				break; 
			case 'd' : IDE_DumpSector();
			case 's':
				val = IDE_Status();
				printf("\n\rStat 0x%2X", val);
				val = IDE_ReadReg(IDE_SCOUNTREG);
				printf(" Cnt 0x%2X", val);
				val = IDE_ReadReg(IDE_SSTARTREG);
				printf(" Strt 0x%2X", val);
				val = IDE_ReadReg(IDE_CYLREGH);
				printf(" Cyl 0x%2X", val);
				val = IDE_ReadReg(IDE_CYLREGL);
				printf("%2X", val);
				val = IDE_ReadReg(IDE_HEADREG);
				printf(" Head 0x%2X", val);
				break;
			case 'w':
				addr = char2hex(RxBuff[1]);
				addr = addr << 4;
				addr += char2hex(RxBuff[2]);
				val = char2hex(RxBuff[3]);
				val = val << 4;
				val += char2hex(RxBuff[4]);
				IDE_WriteReg(addr, val);				
				printf("Wrote 0x%2X to 0x%2X", val, addr);
				break;
			case 'r':
				addr = char2hex(RxBuff[1]);
				addr = addr << 4;
				addr += char2hex(RxBuff[2]);
				val = IDE_ReadReg(addr);
				printf("Register 0x%2X is 0x%2X", addr, val);
				break;
			case 'f' :
				val = char2hex(RxBuff[1]);
				val = val << 4;
				val += char2hex(RxBuff[2]);
				for (data=0; data<256; data++)
					IDE_WriteData(val);
				break;
			case 'z' :
				IDE_RST = 0;
				for (val=255; val!=0; val--);
				IDE_RST = 1;
				break;
			case 'c' : 
				val = char2hex(RxBuff[1]);
				val = val << 4;
				val += char2hex(RxBuff[2]);
				IDE_Command(val);
				break;
			case 'a' :
				printf("ADC %2X", ADRESH);
				break;
			case 'o' :
				val = char2hex(RxBuff[1]);
				val = val << 4;
				val += char2hex(RxBuff[2]);
				TRISB = 0x00;
				PORTB = val;
				DAC_WR = 0;
				DAC_WR = 1;
				TRISB = 0xFF;
				break; 
			case 'R' :
				printf("\n\rRecording...");
				Record();
				break;

			case 'P' : 
				printf("\n\rPlayback...");
				Playback();
				break;
				
			default  : printf("huh?");
				break;
		}
		sComRdy = 0;		// finished with buffer
			
	}

}

void DAC_Write(unsigned char data)
{
	PORTB = data;
	TRISB = 0x00;
	DAC_WR = 0;
	DAC_WR = 1;
	TRISB = 0xFF;
}

void IDE_DumpSector(void)
{
	unsigned char x, y;
	unsigned int data;

	printf("\n\rWord 0    1    2    3    4    5    6    7    8    9    A    B    C    D    E    F");
	for (y=0; y<16; y++)
	{
		printf("\n\r%03x: ", y*16);
		for(x=0; x<16; x++)
		{
			data = IDE_ReadData();
			printf("%04X ", data);
		}
	}
}

void LoadCurrPos(void)
{
	IDE_WriteReg(IDE_SCOUNTREG, 0x01);	// stupid thing changes itself
	IDE_WriteReg(IDE_SSTARTREG, CurrSect);
	IDE_WriteReg(IDE_CYLREGL, CurrCyl & 0x00FF);
	IDE_WriteReg(IDE_CYLREGH, (CurrCyl & 0xFF00) >> 8);
	IDE_WriteReg(IDE_HEADREG, CurrHead);
}

char IncCurrPos(void)
{
	if (++CurrSect > NumSect)
	{
		CurrSect = 1;
		CurrHead++;
	}
	if (CurrHead == NumHead)
	{
		CurrHead = 0;
		CurrCyl++;
	}
	if (CurrCyl > NumCyl) 
		return -1;
	else
		return 0;
}	

void init(void)
{
	INTCON = 0x00;
	PIR1 = 0x00;
	PIR2 = 0x00;
	PIE1 = 0x60;
	PIE2 = 0x00;
	OPTION = 0x03;
	INTCON = 0xE0;
	
	ADCON0 = 0x81;
	ADCON1 = 0x0E;

	TRISA = 0x01;
	TRISB = 0xFF;
	TRISC = 0x80;
	TRISD = 0xFF;
	TRISE = 0x00;
	
	PORTA = 0xFF;
	PORTB = 0xFF;
	
	RCSTA = 0x90;
	TXSTA = 0x24;
	SPBRG = 64;

	IDE_ADDR = 0x00;
	IDE_RST = 1;
	IDE_RD = 1;
	IDE_WR = 1;
	IDE_CS0 = 1;
	IDE_CS1 = 1;
	DAC_WR = 1;

	RxHead = RxTail = RecHead = RecTail = 0;
	sComRdy = sRecord = sPlayback = sBusFree = 0;
	highest = 0;
	lowest = 0xFF;

	B0T = B0H = B1T = B1H = CurrBuff = 0;
	
	NumCyl = NumHead = NumSect = 0;
	CurrCyl = CurrHead = CurrSect = 1;
}

unsigned int IDE_ReadData(void)
{
	unsigned char high, low;

//	GIE = 0;
	
	TRISB = 0xFF;
	TRISD = 0xFF;
	
	IDE_ADDR = 0x00;
	IDE_CS0 = 0;
	IDE_CS1 = 1;
	
	IDE_RD = 0;

	low = PORTB;
	high = PORTD;

	IDE_RD = 1;

//	GIE = 1;
	
	return (low + (high << 8));
}

void IDE_WriteData(unsigned int data)
{
//	unsigned char high, low;

//	GIE = 0;
	
	TRISB = 0x00;
	TRISD = 0x00;

	PORTB = (data & 0x00FF);
	PORTD = (data & 0xFF00) >> 8;
	
	IDE_ADDR = 0x00;
	IDE_CS0 = 0;
	IDE_CS1 = 1;
	
	IDE_WR = 0;

	IDE_WR = 1;

	TRISB = 0xFF;
	TRISD = 0xFF;

//	GIE = 1;
}

void IDE_Command(unsigned char command)
{
//	GIE = 0;
	
	TRISB = 0x00;

	IDE_ADDR = 0x07;

	IDE_CS0 = 0;
	IDE_CS1 = 1;
	
	IDE_DATA_LOW = command;

	IDE_WR = 0;
	IDE_WR = 1;

	TRISB = 0xFF;

//	GIE =1 ;
}

unsigned char IDE_ReadReg(unsigned char reg)
{
	unsigned char result;

//	GIE = 0;
	
	TRISB = 0xFF;
	
	IDE_ADDR = reg & 0x07;

	IDE_CS0 = (reg & 0x08) >> 3;
	IDE_CS1 = (reg & 0x10) >> 4;

	IDE_RD = 0;

	result = IDE_DATA_LOW;

	IDE_RD = 1;

//	GIE = 1;
	
	return result;
}

void IDE_WriteReg(unsigned char reg, unsigned char data)
{
//	GIE = 0;
	
	TRISB = 0x00;
	IDE_DATA_LOW = data;

	IDE_ADDR = reg & 0x07;

	IDE_CS0 = (reg & 0x08) >> 3;
	IDE_CS1 = (reg & 0x10) >> 4;

	IDE_WR = 0;
	IDE_WR = 1;

	TRISB = 0xFF;

//	GIE = 1;
}
	
unsigned char IDE_Status(void)
{
	unsigned char result;

//	GIE = 0;
	
	TRISB = 0xFF;
	
	IDE_ADDR = 0x07;
	
	IDE_CS0 = 0;
	IDE_CS1 = 1;

	IDE_RD = 0;

	result = IDE_DATA_LOW;

	IDE_RD = 1;

//	GIE = 1;
	
	return result;
}

void putch(char c)
{
	while (!TRMT) ;
	TXREG = c;
}