#include <msp430x552x.h>
#include <stdint.h>
#include "hal_pmm.h"

#define CS			(BIT7)
#define SPI_CLK 	(BIT0)
#define SPI_MOSI 	(BIT4)
#define SPI_MISO	(BIT5)

#define CS_PxSEL	(P3SEL)
#define CS_PDIR		(P3DIR)
#define CS_PxIN		(P3IN)
#define CS_PxOUT	(P3OUT)

#define SPI_PxSEL	(P4SEL)
#define SPI_PxDIR	(P4DIR)
#define SPI_PxIN	(P4IN)
#define SPI_PxOUT	(P4OUT)


#define TXBUFREADY (UCA1IFG&UCTXIFG) //Ready if 1. Buffer empty.
#define RXBUFREADY (UCA1IFG&UCRXIFG) //Ready if 1. Data present in buff.

#define ISBUSY	(UCA1STAT&UCBUSY)


#define DUMMY_BYTE 	0xFF

#define CMD0		0
#define CMD1		1
#define CMD8		8
#define CMD9		9
#define CMD10		10
#define CMD55		55

uint8_t dummy_string[] = {0xFF};
uint8_t test_string[] = {0xaa};
uint8_t spi_cmd_string[6];

#define START_BIT 	(1<<6)
#define STOP_BIT  	BIT0


inline void set_cs_high() {
	P3OUT |= CS;
}

inline void set_cs_low() {
	P3OUT &= 0x7F;
}

inline void set_bandwidth(unsigned int bw) {
	UCA1BR0 = (bw & 0xFF);
	UCA1BR1 = (bw>>8);
}


inline void set_spi_clk() {
	UCA1CTL1 |= UCSSEL_2;
}

inline void reset_state_machine() {
	UCA1CTL1 |= UCSWRST;
}

inline void set_state_machine() {
	UCA1CTL1 &= ~UCSWRST;
}

inline void spi_init() {

	SPI_PxSEL = ((SPI_CLK)|(SPI_MOSI)|(SPI_MISO));
	SPI_PxDIR |= SPI_CLK;
	SPI_PxDIR |= SPI_MOSI;
	SPI_PxDIR &= ~SPI_MISO;
	SPI_PxOUT |= ((SPI_CLK)|(SPI_MOSI));


	UCA1CTL0 = UCMSB|UCMST|UCSYNC|UCCKPL;
	reset_state_machine();

	set_spi_clk();

	set_bandwidth(3);
	set_state_machine();
}

inline void send_dummy_byte(int times) {
	
	unsigned int i = 0;

	for(i = 0;i<times;i++) {
		while(!(UCA1IFG & UCTXIFG))  //Loop until Tx Buf is empty.
			;
		
		UCA1TXBUF = DUMMY_BYTE;
	}
}


inline void create_sd_command(uint8_t cmd,uint32_t command_arg) {
	spi_cmd_string[0] = START_BIT|cmd;
	spi_cmd_string[1] = (command_arg)>>24;
	spi_cmd_string[2] = (command_arg)>>16;
	spi_cmd_string[3] = (command_arg)>>8;
	spi_cmd_string[4] = (command_arg);
	spi_cmd_string[5] = 0xFF;
}

inline void sd_write(unsigned char *buff,int bufflen) {
	unsigned int i = 0;

	for(i = 0;i<bufflen;i++) {
		while(TXBUFREADY==0)  //Loop until Tx Buf is empty.
			;
		
		UCA1TXBUF = buff[i];
	}

	while(TXBUFREADY==0)  //Loop until Tx Buf is empty.
		;

	//set_cs_high();
}

inline void send_response_dummy_bytes() {

	while(ISBUSY){ //Loop until empty.
		;
	}
	UCA1TXBUF = DUMMY_BYTE;
}

inline uint8_t sd_read() {
	char ch;	
	
	while(ISBUSY);
		
	ch = UCA1RXBUF;
						
	return ch;	
}

inline uint8_t sd_get_response() {
	send_response_dummy_bytes();
	return sd_read();
}

inline void send_idle_startup_cycles() {
	set_cs_high();
	send_dummy_byte(100);
}


inline void sd_send_cmd() {
	uint8_t i = 0;
	unsigned char ch;

	while(1) {
		sd_write(spi_cmd_string,6);
		i = 0;
		while(i++<=10) { 
			ch = sd_get_response();
			if(!ch) 
				goto done;
		}
	}

done:
	;
}

inline void glow_green() {
	while(1) {
		P5OUT ^= 0x01;
		__delay_cycles(50000);
		__delay_cycles(50000);
	}
}

inline void glow_blue() {
	while(1) {
		P5OUT ^= 0x02;
		__delay_cycles(50000);
		__delay_cycles(50000);
	}
}

inline uint8_t sd_init() {
	int i = 0;
	char ch;

	send_idle_startup_cycles();

	/* Send CMD0 */
	create_sd_command(CMD0,0);
	spi_cmd_string[5] = 0x95;

	set_cs_low();
	sd_write(spi_cmd_string,6);
	i = 0;
	while(i++<=8) {
		ch = sd_get_response();
		if(ch == 1)  {
			set_cs_high();
			break;

		}
	}
	
	/* Send CMD8 */
	create_sd_command(CMD8,0x1AA);
	spi_cmd_string[5] = 0x87;
	while(1) {
		set_cs_low();
		sd_write(spi_cmd_string,6);
		i = 0;
		while(i++<=10) {
			ch = sd_get_response();
			if(ch == 1)  {
				send_dummy_byte(20);
				set_cs_high();

				goto cmd8out;
			}
		}
		set_cs_high();
	}

cmd8out:
//	send_dummy_byte(1000);
	
	/* Send CMD1 */
	while(1) {
		set_cs_low();
		/*create_sd_command(CMD55,0x0);
		sd_write(spi_cmd_string,6);*/
		
		create_sd_command(CMD1,0x40000000);
		sd_write(spi_cmd_string,6);
		//send_dummy_byte(20);
		set_cs_high();

		set_cs_low();
		i = 0;
		while(i++<=100) {
			ch = sd_get_response();
			if(!ch) {
				set_cs_high();
				goto out;
			}else if(ch == 1) {
				set_cs_high();
				send_dummy_byte(8);
				set_cs_low();

			}
		}
		set_cs_high();
	}


out:
	
	/* Send CMD10 */
	create_sd_command(CMD10,0);
	set_cs_low();
	sd_send_cmd();
	set_cs_high();

end:
	i = 0;
	set_cs_low();
	while(i<22) {
		ch = sd_get_response();
		i++;
	}
	set_cs_high();

	glow_blue();
	return 1;
}


int main(int argc,char **argv) {
	WDTCTL = WDTPW | WDTHOLD;

	P5DIR |= 0x03; //LED

	P3DIR |= CS;	//Chip Select.
	
	SetVCoreUp(PMMCOREV_3);

	spi_init();
	sd_init();

	set_cs_high();
	
	__bis_SR_register(GIE);

	while(1) 
		__no_operation();
	
	return 0;
}