#include "h/p33FJ64GP802.h"
#include "h/i2c.h"
#include "h/ezlcd.h"
#include "h/sd.h"
#include "h/sdcomm_spi.h"

unsigned int WriteI2CAddressed(BYTE, BYTE);

_FICD(BKBUG_ON & COE_ON & JTAGEN_OFF & ICS_PGD1);
_FBS(RBS_NO_RAM & BSS_NO_FLASH & BWRP_WRPROTECT_OFF);
_FSS(RSS_NO_RAM & SSS_NO_FLASH & SWRP_WRPROTECT_OFF);
_FGS(GSS_OFF & GCP_OFF & GWRP_OFF);
_FOSCSEL(FNOSC_PRI & IESO_OFF);
_FOSC(FCKSM_CSDCMD & OSCIOFNC_OFF & POSCMD_HS);
_FWDT(FWDTEN_OFF);
// Global Variables
BYTE commandBuffer[6];
BYTE receiveBuffer[32];

void delayMs(int x)
{
	int i;
	while(x>0)
	{
		for(i=0; i<4100; i++)
		{
		asm("nop");
		}
		x--;
	}
}

void sendc_dbguart(BYTE x)
{
	int i;
	WriteI2CAddressed(0x6F, x);
	for(i=0; i<2000; i++)
	{
	asm("nop");
	}
}

void InitSPI(int speed)
{
	//
	// remap SPI1 pins as follows:
	// RP11= /SS
	// RP10= SDO1
	// RP7=  SCKL
	// RP6=  SDI1
	//
	SPI1STAT=0;
	SPI1CON1=0;
	SPI1CON2=0;

	TRISBbits.TRISB11=0;
	TRISBbits.TRISB10=0;
	TRISBbits.TRISB7=0;
	TRISBbits.TRISB6=1;

	LATBbits.LATB11=1;
	RPOR5=0x0007;
	RPOR3=0x0800;
	RPINR20=0x0706;
	IFS0bits.SPI1IF=0;
	SPI1CON1=(speed & 0x000F);
	SPI1CON1bits.SMP=1;
	SPI1CON1bits.CKP=0;			// clock Polarity 0: idle low, active high 1: idle high, active low
	SPI1CON1bits.CKE=1;			// clock edge selection
	SPI1CON1bits.MSTEN=1;		// enable Master Mode
	SPI1CON2=0x0000;
	SPI1STATbits.SPIROV=0;
	SPI1STATbits.SPIEN=1;
}

BYTE WriteSPI(BYTE x)
{
	SPI1CON1bits.MSTEN=1;		// set Master Mode
	LATBbits.LATB11=0;
	SPI1BUF=x;
	while(SPI1STATbits.SPITBF);
	while(SPI1STATbits.SPIRBF==0);
	asm("nop");
	asm("nop");
	LATBbits.LATB11=1;
	x=SPI1BUF;
	return x;
}

BYTE WriteSPIWithoutSS(BYTE x)
{
	LATBbits.LATB11=1;
	SPI1BUF=x;
	while(SPI1STATbits.SPITBF);
	while(SPI1STATbits.SPIRBF==0);
	x=SPI1BUF;
	return x;
}

unsigned int CRC7(BYTE* chr, int cnt)
{
	int i,a; 
	unsigned char crc,Data; 

	crc=0; 
	for (a=0;a<cnt;a++) 
	{ 
		Data=chr[a]; 
		for (i=0;i<8;i++)
		{ 
		crc <<= 1; 
		if ((Data & 0x80)^(crc & 0x80)) 
		crc ^=0x09; 
		Data <<= 1; 
		} 
	} 
	crc=(crc<<1)|1; 
	return((unsigned int)crc); 
}

int sendCommandSDCardSPI(BYTE cmd, BYTE response_type, BYTE* response, BYTE *argument)
{
	int i;
	BYTE response_length;
	BYTE tmp;
	BYTE ibuffer[6];

	ibuffer[0]=((cmd & 0x3F) | 0x40);
	ibuffer[1]=argument[0];
	ibuffer[2]=argument[1];
	ibuffer[3]=argument[2];
	ibuffer[4]=argument[3];
	ibuffer[5]=CRC7(&ibuffer[0], 5);
	LATBbits.LATB11=0;
	/* All data is sent MSB first, and MSb first */
	/* Send the header/command */
	/* Format:
	cmd[7:6] : 01
	cmd[5:0] : command 
	*/
	for (i=0; i<=5; i++)
	{
	WriteSPI(ibuffer[i]);
	}
	response_length = 0;

	switch (response_type)
	{

		case R1:
		case R1B:
			response_length = 1;
		break;

		case R2:
			response_length = 2;
		break;

		case R3:
			response_length = 5;
		break;

		case R7:
			response_length = 5;
		break;

		case R1X18:
			response_length = 18;
		break;
		
		default:
		break;
	}
	/* Wait for a response. A response can be recognized by the
	start bit (a zero) */

	for(i=0; i<response_length; i++)response[i]=0xFF;
	i=0;
	do
	{
	tmp = WriteSPI(0xFF);
	i++;
	}
	while (((tmp & 0x80) != 0) && (i < SD_CMD_TIMEOUT));

	/* Just bail if we never got a response */
	if (i >= SD_CMD_TIMEOUT)
	{
	LATBbits.LATB11=1;
	return 0;
	}

	if(response_type!=R1X18)
	{
	 for (i=0; i< response_length; i++)
	 {
	 response[i] = tmp;
	 /* This handles the trailing-byte requirement. */
	 tmp = WriteSPI(0xFF);
	 }
	} else
	{
	// this type of response is for SPI Mode only and only for CMD9 and CMD10 that
	// send the CID and CSD registers back in a 16 byte + 2 byte packet
	// the 16 byte packet contains the register while the 2 byte suffix is the CRC16 field.
	// the first response is a R1 type, then we wait for the card to response by polling
	// bit 0.
	response[0]=tmp;
	i=0;
	do
	{
	tmp=WriteSPI(0xFF);
	i++;
	}
	while ((tmp & 1) && (i < SD_CMD_TIMEOUT));
	tmp=WriteSPI(0xFF);
    for (i=1; i<=response_length; i++)
	{
	response[i] = tmp;
	/* This handles the trailing-byte requirement. */
	tmp = WriteSPI(0xFF);
	}
    }
	/* If the response is a "busy" type (R1B), then there's some
	* special handling that needs to be done. The card will
	* output a continuous stream of zeros, so the end of the BUSY
	* state is signaled by any nonzero response. The bus idles
	* high.
	*/
	i=0;
	if (response_type == R1B)
	{
	do	
	{
	i++;
	tmp = WriteSPI(0xFF);
	}
	/* This should never time out, unless SDI is grounded.
	* Don't bother forcing a timeout condition here. */
	while (tmp != 0xFF);
	WriteSPI(0xFF);
	}
	LATBbits.LATB11=1;
	return 1;
}

void showReceiveBuffer(int y, BYTE numBytes)
{
	int i;
	setcursor_ezlcd(0, y);
	writestring_ezlcd("Received: ");
	for(i=0; i<numBytes; i++)
	{
	disafree(receiveBuffer[i]);
	writec_ezlcd('.');
	}
}

void disableSPI(void)
{
	LATB=0xFF7F;
	TRISBbits.TRISB11=0;
	TRISBbits.TRISB10=0;
	TRISBbits.TRISB7=0;
	TRISBbits.TRISB6=1;
	RPOR5=0;
	RPOR3=0;
	RPINR20=0;
	SPI1STAT=0;
	SPI1CON1=0;
	SPI1CON2=0;
}

BYTE sendData(BYTE x)
{
	int i, j;
	for(i=0; i<8; i++)
	{
	LATBbits.LATB7=0;
	for(j=0; j<50; j++);
	if((x & 0x80)!=0)
	LATBbits.LATB10=1; else LATBbits.LATB10=0;
	LATBbits.LATB7=1;
	for(j=0; j<50; j++);

	x=x<<1;
	}
	return 0;
}

void sendInitialCMD0WithSS(void)
{
	// sending 0x40 0x00 0x00 0x00 0x00 0x95
	disableSPI();
	LATBbits.LATB11=1;			// CS=1
	LATBbits.LATB10=1;			// Data = 1
	LATBbits.LATB7=0;			// clock = 0
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	sendData(0xFF);
	LATBbits.LATB11=0;			// CS=0	
	sendData(0x40);
	sendData(0x00);
	sendData(0x00);
	sendData(0x00);
	sendData(0x00);
	sendData(0x95);
	delayMs(10);
	LATBbits.LATB11=1;
	delayMs(10);
}

int InitSDCardSPIOld(void)
{
	int i;
	setcursor_ezlcd(0, 0);
	writestring_ezlcd((BYTE*)"Initialising SD Card.");
	setcursor_ezlcd(0, 10);
	writestring_ezlcd((BYTE*)"Sending CMD0 Native.");
	sendInitialCMD0WithSS();

	InitSPI(0x0);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	WriteSPIWithoutSS(0xFF);
//	commandBuffer[0]=0;
//	commandBuffer[1]=0;
//	commandBuffer[2]=0;
//	commandBuffer[3]=0;
//	sendCommandSDCardSPI(CMD0, CMD0_R, &receiveBuffer[0], &commandBuffer[0]);
//	showReceiveBuffer(10, 1);
	delayMs(200);
	i=0;
	do
	{
	sendCommandSDCardSPI(CMD55, CMD55_R, &receiveBuffer[0], &commandBuffer[0]);
	sendCommandSDCardSPI(ACMD41, ACMD41_R, &receiveBuffer[0], &commandBuffer[0]);
	delayMs(5);
	i++;
	} while ((receiveBuffer[0] & 1) && (i<SD_IDLE_WAIT_MAX));
	if(i>=SD_IDLE_WAIT_MAX)
	{
	setcursor_ezlcd(0, 20);
	writestring_ezlcd((BYTE*)"SD Internal Init Failed!");
	return 0;			// Failed!
	}
	setcursor_ezlcd(0, 20);
	writestring_ezlcd((BYTE*)"SD Internal Init Complete.");
	showReceiveBuffer(30, 1);

	setcursor_ezlcd(0, 40);
	writestring_ezlcd((BYTE*)"Reading OCR.");
	commandBuffer[0]=0;
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	sendCommandSDCardSPI(CMD58, CMD58_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(50,5);
	setcursor_ezlcd(0, 60);
	writestring_ezlcd((BYTE*)"Checking 3.3V Operation: ");
	if((receiveBuffer[2] & 0x10)!=0)writestring_ezlcd((BYTE*)"Yes.");	// bit 20 of OCR corresponds to 3.3V operation!
	else writestring_ezlcd((BYTE*)"No.");
	setcursor_ezlcd(0, 70);
	writestring_ezlcd((BYTE*)"Done. Now Reading CID Register...");
	sendCommandSDCardSPI(CMD10, CMD10_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(80, 19);
	setcursor_ezlcd(0, 110);
	writestring_ezlcd((BYTE*)"Done. Now Reading CSD Register...");
	sendCommandSDCardSPI(CMD9, CMD9_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(120, 19);
}

int InitSDCardSPI(void)
{
	int i;
	setcursor_ezlcd(0, 0);
	writestring_ezlcd((BYTE*)"Initialising SD Card v2.0.");
	setcursor_ezlcd(0, 10);
	writestring_ezlcd((BYTE*)"Sending CMD0 Native.");
	sendInitialCMD0WithSS();
	InitSPI(0x0);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	WriteSPIWithoutSS(0xFF);
	delayMs(200);
	commandBuffer[0]=0x00;
	commandBuffer[1]=0x00;
	commandBuffer[2]=0x01;	// [11:8]=VHS   0001b = 2.7 - 3.6V voltage range from Host
	commandBuffer[3]=0xAA;	// check Pattern
	sendCommandSDCardSPI(CMD8, CMD8_R, &receiveBuffer[0], &commandBuffer[0]);
	setcursor_ezlcd(0, 20);
	writestring_ezlcd((BYTE*)"Sent CMD8.");
	showReceiveBuffer(30, 5);
	setcursor_ezlcd(0, 40);
	writestring_ezlcd((BYTE*)"Reading OCR.");
	commandBuffer[0]=0;
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	sendCommandSDCardSPI(CMD58, CMD58_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(50,5);
	return 1;

	i=0;
	do
	{
	sendCommandSDCardSPI(CMD55, CMD55_R, &receiveBuffer[0], &commandBuffer[0]);
	sendCommandSDCardSPI(ACMD41, ACMD41_R, &receiveBuffer[0], &commandBuffer[0]);
	delayMs(5);
	i++;
	} while ((receiveBuffer[0] & 1) && (i<SD_IDLE_WAIT_MAX));
	if(i>=SD_IDLE_WAIT_MAX)
	{
	setcursor_ezlcd(0, 20);
	writestring_ezlcd((BYTE*)"SD Internal Init Failed!");
	return 0;			// Failed!
	}
	setcursor_ezlcd(0, 20);
	writestring_ezlcd((BYTE*)"SD Internal Init Complete.");
	showReceiveBuffer(30, 1);



	setcursor_ezlcd(0, 60);
	writestring_ezlcd((BYTE*)"Checking 3.3V Operation: ");
	if((receiveBuffer[2] & 0x10)!=0)writestring_ezlcd((BYTE*)"Yes.");	// bit 20 of OCR corresponds to 3.3V operation!
	else writestring_ezlcd((BYTE*)"No.");
	setcursor_ezlcd(0, 70);
	writestring_ezlcd((BYTE*)"Done. Now Reading CID Register...");
	sendCommandSDCardSPI(CMD10, CMD10_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(80, 19);
	setcursor_ezlcd(0, 110);
	writestring_ezlcd((BYTE*)"Done. Now Reading CSD Register...");
	sendCommandSDCardSPI(CMD9, CMD9_R, &receiveBuffer[0], &commandBuffer[0]);
	showReceiveBuffer(120, 19);
}

int main(void)
{
	int i;

	i=0;
	PORTB=0xFFFF;
	TRISB=0;
	InitI2C();
	init_ezlcd();
	delayMs(500);
	setcursor_ezlcd(0, 60);
	InitSDCardSPI();
	delayMs(1000);
	init_ezlcd();
	delayMs(100);
	InitSDCardSPIOld();


	delayMs(500);
	while(1)
	{
	disdec(0, 150, i);
	i++;
	LATBbits.LATB2=0;
	LATBbits.LATB3=1;
	delayMs(50);
	LATBbits.LATB2=1;
	LATBbits.LATB3=0;
	delayMs(50);
	}
}


/*
BYTE bitShift(BYTE* inptr, int numBits, int numReqBits)
{
	// take numReqBits out of bit stream of numBits starting at inptr
	BYTE result, temp;
	int ix, iy, j;

	ix=(numBits/8)-1;
	result=0;
	j=numReqBits;
	while(j>0)
	{
	iy=ix;
	result=0x7F & (result>>1);
	if((*(inptr+iy)) & 1)result|=0x80;
	while(iy>=0)
	{
	 temp=*(inptr+iy);
	 temp=0x7F & (temp>>1);
 	 if((iy>0)&&((*(inptr+iy-1)) & 1))temp|=0x80;
	*(inptr+iy)=temp;
	iy--;
	}
	j--;
	}
	result=result>>(8-numReqBits);
	return result;
}

unsigned int computeCRC7(BYTE* inptr, int numBits)
{
	unsigned int CRCResult;
	BYTE	bitResult;
	int i;

	CRCCON = 0x0006;	// Configure the polynomial length (PLEN) 				 
	CRCXOR = 0x0088; 	// In CRCXOR, configure for the polynomial x^7 + x^3 + 1 
	CRCWDAT = 0x00; 	// Clear CRCWDAT										 
	setcursor_ezlcd(0, 80);
	i=numBits;
	CRCDAT=0;
	while(i>=7)
	{
		bitResult=bitShift(inptr, numBits, 7);
		disafree(bitResult);
		writec_ezlcd(',');
		CRCDAT=bitResult;  			// Populate the FIFO
		if(CRCCONbits.CRCFUL)
		{
		CRCCONbits.CRCGO = 1; 		// CRC calculation start 
		}
		while(CRCCONbits.CRCFUL); 	// Wait for available FIFO register 
		i-=7;
	}
	if(i>0)
	{
	bitResult=bitShift(inptr, numBits, 7);
	CRCDAT = bitResult;
	disafree(bitResult);
	}
	CRCDAT = 0;							// Do this to shift the last word out of the CRC shift register 
	CRCCONbits.CRCGO=1;
	while (!IFS4bits.CRCIF); 			// Check for interrupt flag bit 
	IFS4bits.CRCIF = 0;
	while(!CRCCONbits.CRCMPT);			// Wait for CRC shifter to clear FIFO 
	CRCResult = CRCWDAT; 				// Output result 
	CRCWDAT = 0;
	return (unsigned int)CRCResult;
}
*/

/*
void sendCommand(BYTE* com)
{
	int b, byteCount;
	BYTE y;

	commandBuffer[0]|=0x40;
	setcursor_ezlcd(0, 100);
	writestring_ezlcd((BYTE*)"Sending:  ");
	commandBuffer[5]=(BYTE)(CRC7(com, 5));
	for(byteCount=0; byteCount<6; byteCount++)
	{
	disafree(commandBuffer[byteCount]);
	writec_ezlcd('.');
	receiveBuffer[byteCount]=WriteSPI(commandBuffer[byteCount]);
	}

	b=6;
	y=0;
	while((b<16)&&(y!=0xFF))
	{
	y=WriteSPI(0xFF);
	receiveBuffer[b]=y;
	b++;
	}

	setcursor_ezlcd(0, 110);
	writestring_ezlcd("Received: ");
	for(byteCount=0; byteCount<16; byteCount++)
	{
	disafree(receiveBuffer[byteCount]);
	writec_ezlcd('.');
	}
}
	receiveBuffer[0]=1;
	while(receiveBuffer[0]!=0)
	{
	commandBuffer[0]=1;		// CMD1
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	commandBuffer[4]=0;
	sendCommand(&commandBuffer[0]);
	delayMs(100);
	}

	commandBuffer[0]=55;	// CMD55
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	commandBuffer[4]=0;
	sendCommand(&commandBuffer[0]);	

	delayMs(200);

	commandBuffer[0]=41;	// CMD41
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	commandBuffer[4]=0;
	sendCommand(&commandBuffer[0]);

	delayMs(200);

	commandBuffer[0]=58;	// CMD58
	commandBuffer[1]=0;
	commandBuffer[2]=0;
	commandBuffer[3]=0;
	commandBuffer[4]=0;
	sendCommand(&commandBuffer[0]);
	
	delayMs(200);


*/

