/*****************************************************************
* File: sd_spi.c
* Copyright (c) 2013 Mango Communications, all rights reseved
* Released under the WARP License
* See http://warpproject.org/license for details
*****************************************************************/
#include "sd_spi.h"

inline u8 simple_spi_read(u32 baseaddr) {
	Xil_Out32(baseaddr + SSPI_REG_SPITX, 0xFF);
	return (u8)(Xil_In32(baseaddr + SSPI_REG_SPIRX) & 0xFF);
}

inline void simple_spi_write(u32 baseaddr, u8 txByte) {
	Xil_Out32(baseaddr + SSPI_REG_SPITX, (u32)(txByte & 0xFF));
	return;
}

void sd_send_cmd(u32 ba, u8 cmd, u32 args, u8 last_byte) {

	simple_spi_write(ba, 0xFF); //flush that Sandisk cards seem to require...
	simple_spi_write(ba, cmd);
	simple_spi_write(ba, (u8)(0xFF & (args>>24)));
	simple_spi_write(ba, (u8)(0xFF & (args>>16)));
	simple_spi_write(ba, (u8)(0xFF & (args>> 8)));
	simple_spi_write(ba, (u8)(0xFF & (args>> 0)));
	simple_spi_write(ba, last_byte);
}


int sd_read_block(u32 ba, u32 blk_offset, u8* buf, u32 blk_size) {
	int i;
	u8 rx_byte;

	sd_send_cmd(ba, SD_CMD17, blk_offset, 0x01);

	for(i=0; i<8; i++) {
		rx_byte = simple_spi_read(ba);
		if(rx_byte == 0xFF) continue;
		if(rx_byte == 0x00) break;
	}

	for(i=0; i<100; i++) {
		usleep(10000);
		rx_byte = simple_spi_read(ba);
		if(rx_byte == SD_DATA_TOKEN_RD) break;
	}
	if(i==100) {xil_printf("sd_read_block: No token received - Quitting\n"); return -1;}

	//Read exactly blk_size bytes
	for(i=0; i<blk_size; i++) {
		buf[i] = simple_spi_read(ba);
	}

	//Read and discard CRC bytes
	simple_spi_read(ba);
	simple_spi_read(ba);

	return 0;
}

int sd_write_block(u32 ba, u32 blk_offset, u8* buf, u32 blk_size) {
	int i;
	u8 rx_byte;

	sd_send_cmd(ba, SD_CMD24, blk_offset, 0x01);

	for(i=0; i<8; i++) {
		rx_byte = simple_spi_read(ba);
		if(rx_byte == 0xFF) continue;
		if(rx_byte == 0x00) break;
	}
	if(i==8) {xil_printf("CMD24 error - invalid response\n"); return -1;}

	//Send 1-byte write token
	simple_spi_write(ba, SD_DATA_TOKEN_WR_SINGLE);

	//Send blk_size bytes
	for(i=0; i<blk_size; i++) {
		simple_spi_write(ba, buf[i]);
	}

	//Write dummy 2-byte CRC (CRC not checked in SPI mode)
	simple_spi_write(ba, 0xFF);
	simple_spi_write(ba, 0xFF);

	//Read data response
	rx_byte = simple_spi_read(ba);
	if((rx_byte & 0x1F) != 0x05) {xil_printf("Write data response indicated error[%2d]: 0x%02x\n", i, rx_byte); return -1;}

	//Wait for SD card to indicate write is finished
	rx_byte = 0x00; i=0;
	while(rx_byte != 0xFF) {
		rx_byte = simple_spi_read(ba);
		i++;
	}
	//xil_printf("Idle after %d byte reads\n", i);

	return 0;
}

int sd_rw_init(u32 ba) {
	int i, j;
	int init_tries;
	u8 sd_idle;
	u8 rx_byte;

	//SD cards I tested don't seem to require a slow clock for init

	//CLK_DIV param selects bit index in clk counter
	// Higher index -> faster SCLK
	// For 80MHz bus clock CLK_DIV=6 -> SCLK=20MHz
	simple_spi_set_clkDiv(ba, 6);

	//CMD0 sometimes fails on the first try - try it twice before punting
	for(j=0; j<2; j++) {
		//SD init requires >74 SCLK cycles with CS de-asserted and MOSI = 1
		simple_spi_set_cs(ba, 0);
		for(i=0; i<20; i++) {
			simple_spi_write(ba, 0xFF);
		}

		//Wait for a while before sending IDLE command
		usleep(10000);

		//Assert CS and leave asserted
		simple_spi_set_cs(ba, 1);

		//Send GO_IDLE command
		// 0x95 is valid CRC for CMD0 msg
		// Later cmds don't need CRC (ignored in SPI mode)
		sd_send_cmd(ba, SD_CMD0, 0x0, 0x95);

		//Wait for command response
		// Response should be 0x01 = No errors, IDLE=1
		for(i=0; i<32; i++) {
			rx_byte = simple_spi_read(ba);

			if(rx_byte == 0xFF) continue;
			else if(rx_byte == 0x01) break;
		}
		if(j==1 && i==32) {
			//Punt if both CMD0 attempts fail
			xil_printf("Error in CMD0 - no valid response\n");
			return -1;
		}
	}

	//Start SD card init with CMD55-ACMD41 sequence
	// Keep sending these commands until the card reports not-IDLE
	sd_idle = 1;
	init_tries = 0;
	while((sd_idle == 1) && (init_tries < 64)) {
		sd_send_cmd(ba, SD_CMD55, 0x0, 0x01);
		for(i=0; i<8; i++) {
			rx_byte = simple_spi_read(ba);
			if(rx_byte == 0xFF) continue;
			//xil_printf("CMD55 [%02d]: rx_byte = 0x%02x\n", i, rx_byte);
			if((rx_byte & 0x1) == 0) {sd_idle = 0; break;}
		}

		sd_send_cmd(ba, SD_ACMD41, 0x0, 0x01);
		for(i=0; i<8; i++) {
			rx_byte = simple_spi_read(ba);
			if(rx_byte == 0xFF) continue;
			//xil_printf("ACMD41 [%02d]: rx_byte = 0x%02x\n", i, rx_byte);
			if((rx_byte & 0x1) == 0) {sd_idle = 0; break;}
		}
		usleep(100000);
		init_tries++;
	}
	if(sd_idle == 1) {xil_printf("Error in SD init - ACMD41 never reported not IDLE\n"); return -1;}

	//Set block length to 512B
	sd_send_cmd(ba, SD_CMD16, SD_BLK_SIZE, 0x01);
	for(i=0; i<8; i++) {
		rx_byte = simple_spi_read(ba);
		if(rx_byte == 0xFF) continue;
		if(rx_byte == 0x00) break;
	}
	if(i==8) {xil_printf("Error in CMD16 - no valid response\n"); return -1;}

	return 0;
}