#include "sd.h"
#include "spi.h"
#include "util.h"

static uint8_t sd_is_block_device = 0;

uint8_t sd_send_command_util(uint8_t cmd, uint32_t data) {
	uint8_t i;
	uint8_t x = 0x01;
	
	if (cmd == SD_GO_IDLE_STATE)
		x = 0x95; /* CRC */
	else if (cmd == SD_SEND_IF_COND)
		x = 0x87; /* CRC */

	cmd |= 0x40;

	SD_DESELECT();
	spi_tx(0xff);
	SD_SELECT();
	spi_tx(0xff);

	spi_tx(cmd);
	spi_tx((uint8_t) (data >> 24));
	spi_tx((uint8_t) (data >> 16));
	spi_tx((uint8_t) (data >> 8));
	spi_tx((uint8_t) (data));
	spi_tx(x);

	i = 10;
	do {
		x = spi_rx();
	} while ((x & 0x80) && --i);

	return x;
}

uint8_t sd_send_command(uint8_t cmd, uint32_t data) {
	if (cmd & 0x80) {
		uint8_t res = sd_send_command_util(SD_APP_CMD, 0);
		if (res > 1)
			return res;
		cmd &= 0x7f;
	}
	return sd_send_command_util(cmd, data);
}

void sd_wait(void) {
	uint8_t resp;
	uint8_t i = 255;
	do {
		resp = sd_send_command(SD_SEND_OP_COND, 0);
	} while (resp != 0 && --i);
}

char sd_init(void) {
	uint8_t i;
	uint16_t tmr;
	
	spi_init();

	SD_CHIP_SELECT_TRIS = 0;

	SD_SELECT();
	for (i = 10; i; i--)
		delay_100us();
	SD_DESELECT();

	for (i = 0; i < 20; i++)
		spi_tx(0xff);

	SD_SELECT();
	tmr = 1000;
	do {
		for (i = 10; i; i--)
			delay_100us();
		i = sd_send_command(SD_GO_IDLE_STATE, 0);
	} while (--tmr && i != 1);

	if (sd_send_command(SD_SEND_IF_COND, 0x1aa) == 1) {
		/* SD v2 */
		uint8_t ocr[4];
		ocr[0] = spi_rx();
		ocr[1] = spi_rx();
		ocr[2] = spi_rx();
		ocr[3] = spi_rx();

#ifdef DEBUG
		printf("SD v2 %x:%x:%x:%x\r\n", ocr[0], ocr[1], ocr[2], ocr[3]);
#endif

		if (ocr[2] == 0x01 && ocr[3] == 0xaa) {
			tmr = 10000;
			do {
				delay_100us();
				i = sd_send_command(SD_OP_COND_SDC, 1UL << 30);
			} while (--tmr && i);

#ifdef DEBUG
			printf("Done %u %u\r\n", tmr, i);
#endif

			i = sd_send_command(SD_READ_OCR, 0);

			if (tmr && i == 0) {
				ocr[0] = spi_rx();
				ocr[1] = spi_rx();
				ocr[2] = spi_rx();
				ocr[3] = spi_rx();

#ifdef DEBUG
				printf("OCR: %x:%x:%x:%x\r\n", ocr[0], ocr[1], ocr[2], ocr[3]);
#endif

				if (ocr[0] & 0x40)
					sd_is_block_device = 1;
			}
		}
	} else {
		/* SD v1 */
#ifdef DEBUG
		printf("SD v1\r\n");
#endif
	}

	SD_DESELECT();
	spi_rx();

	return 0;
}