#include <includes.h>
#include "sdcard.h"
#include "spi1.h"

#ifdef CONFIG_SDCARD_ENABLE

#define CAP_VER2_00	(1<<0)
#define CAP_SDHC	(1<<1)

#define SD_CS_SET()     {spi1_unselectChip(SPI1_CS_SD_CARD);}
#define SD_CS_CLR()     {spi1_selectChip(SPI1_CS_SD_CARD);}

#define SD_SPI_PEND()   {spi1_getSem();}
#define SD_SPI_POST()   {spi1_freeSem();}

#define spi_txrx    spi1_exchange

///
typedef struct
{
    int initialized;
    int sectors;
    int erase_sectors;
    int capabilities;
} HwInfo;

static HwInfo hw_info;

typedef enum 
{
    SD_SPEED_INVALID,
    SD_SPEED_400KHZ,
    SD_SPEED_10MHZ 
} SdSpeed;

static void spi_set_speed(SdSpeed speed)
{
    if (speed == SD_SPEED_400KHZ)
    {
        spi1_setSpeed(400000);
    }
    else if (speed == SD_SPEED_10MHZ)
    {
        spi1_setSpeed(10000000);
    }
}

///
static uint8_t crc7_one(uint8_t t, uint8_t data)
{
	int i;
	const uint8_t g = 0x89;
	t ^= data;
	for (i=0; i<8; i++) {
		if (t & 0x80)
			t ^= g;
		t <<= 1;
	}
	return t;
}

///
uint8_t crc7(const uint8_t *p, int len)
{
	int j;
	uint8_t crc = 0;
	for (j=0; j<len; j++)
		crc = crc7_one(crc, p[j]);

	return crc>>1;
}

/// 
static uint16_t crc16_ccitt(uint16_t crc, uint8_t ser_data)
{
	crc  = (uint8_t)(crc >> 8) | (crc << 8);
	crc ^= ser_data;
	crc ^= (uint8_t)(crc & 0xff) >> 4;
	crc ^= (crc << 8) << 4;
	crc ^= ((crc & 0xff) << 4) << 1;

	return crc;
}

///
static uint16_t crc16(const uint8_t *p, int len)
{
	int i;
	uint16_t crc = 0;

	for (i=0; i<len; i++)
		crc = crc16_ccitt(crc, p[i]);

	return crc;
}

///
static void sd_cmd(uint8_t cmd, uint32_t arg)
{
	uint8_t crc = 0;
	spi_txrx(0x40 | cmd);
	crc = crc7_one(crc, 0x40 | cmd);
	spi_txrx(arg >> 24);
	crc = crc7_one(crc, arg >> 24);
	spi_txrx(arg >> 16);
	crc = crc7_one(crc, arg >> 16);
	spi_txrx(arg >> 8);
	crc = crc7_one(crc, arg >> 8);
	spi_txrx(arg);
	crc = crc7_one(crc, arg);
	spi_txrx(crc | 0x1);	/* crc7, for cmd0 */
}

static uint8_t sd_get_r1()
{
	int tries = 1000;
	uint8_t r;

	while (tries--) {
		r = spi_txrx(0xff);
		if ((r & 0x80) == 0)
			return r;
	}
	return 0xff;
}

static uint16_t sd_get_r2()
{
	int tries = 1000;
	uint16_t r;

	while (tries--) {
		r = spi_txrx(0xff);
		if ((r & 0x80) == 0)
			break;
	}
	if (tries < 0)
		return 0xff;
	r = r<<8 | spi_txrx(0xff);

	return r;
}

/// r1, then 32-bit reply... same format as r3
static uint8_t sd_get_r7(uint32_t *r7)
{
	uint32_t r;
	r = sd_get_r1();
	if (r != 0x01)
		return r;

	r = spi_txrx(0xff) << 24;
	r |= spi_txrx(0xff) << 16;
	r |= spi_txrx(0xff) << 8;
	r |= spi_txrx(0xff);

	*r7 = r;
	return 0x01;
}

#define sd_get_r3 sd_get_r7

/// Nec (=Ncr? which is limited to [0,8]) dummy bytes before lowering CS, as described in sandisk doc, 5.4.
static void sd_nec()
{
	int i;
	for (i=0; i<8; i++)
		spi_txrx(0xff);
}

/// SD card initialization
/// returns 0 if OK
static int sd_guts_init(void)
{
    int i;
    int r;
    uint32_t r7;
    uint32_t r3;
    int tries;
    uint32_t hcs;

    // start with 100-400 kHz clock
    spi_set_speed(SD_SPEED_400KHZ);

    hw_info.capabilities = 0;
    
    // cmd0 - reset..
    SD_CS_SET();
    // 74+ clocks with CS high
    for (i=0; i<10; i++) spi_txrx(0xff);
    
    // reset
    SD_CS_CLR();
    sd_cmd(0, 0);
    r = sd_get_r1();
    sd_nec();
    SD_CS_SET();
    
    if (r == 0xff)
    {
        spi_set_speed(SD_SPEED_10MHZ);
        return -1;
    }
    
    if (r != 0x01) 
    {
        // fail
        spi_set_speed(SD_SPEED_10MHZ);
        return -2;
    }
    
    // cmd8 - voltage..
    // ask about voltage supply
    SD_CS_CLR();
    sd_cmd(8, 0x1aa); // VHS = 1
    r = sd_get_r7(&r7);
    sd_nec();
    SD_CS_SET();
    
    hw_info.capabilities |= CAP_VER2_00;
    
    if (r == 0xff)
    {
        spi_set_speed(SD_SPEED_10MHZ);
        return -1;
    }
    
    if (r == 0x01)
    {
        // success, SD v2.x
    }
    else if (r & 0x4)
    {
        // not implemented, SD v1.x
        hw_info.capabilities &= ~CAP_VER2_00;
    }
    else
    {
        // fail
        spi_set_speed(SD_SPEED_10MHZ);
        return -2;
    }
    
    // cmd58 - ocr..
    // ask about voltage supply
    SD_CS_CLR();
    sd_cmd(58, 0);
    r = sd_get_r3(&r3);
    sd_nec();
    SD_CS_SET();
    
    if (r == 0xff)
    {
        spi_set_speed(SD_SPEED_10MHZ);
        return -1;
    }
    
    if (r != 0x01 && !(r & 0x4))
    { 
        // allow it to not be implemented - old cards
        spi_set_speed(SD_SPEED_10MHZ);
        return -2;
    }
    
    // cmd41 - hcs..
    tries = 1000;
    hcs = 0;
    // say we support SDHC
    if (hw_info.capabilities & CAP_VER2_00)
        hcs = 1<<30;
    
    // needs to be polled until in_idle_state becomes 0
    do 
    {
        // send we don't support SDHC
        SD_CS_CLR();
        // next cmd is ACMD
        sd_cmd(55, 0);
        r = sd_get_r1();
        sd_nec();
        SD_CS_SET();
        
        if (r == 0xff)
        {
            spi_set_speed(SD_SPEED_10MHZ);
            return -1;
        }
        
        // well... it's probably not idle here, but specs aren't clear
        if (r & 0xfe) 
        {
            // fail
            spi_set_speed(SD_SPEED_10MHZ);
            return -2;
        }
        
        SD_CS_CLR();
        sd_cmd(41, hcs);
        r = sd_get_r1();
        sd_nec();
        SD_CS_SET();
        
        if (r == 0xff)
        {
            spi_set_speed(SD_SPEED_10MHZ);
            return -1;
        }
        
        if (r & 0xfe)
        {
            // fail
            spi_set_speed(SD_SPEED_10MHZ);
            return -2;
        }
        
    } while (r != 0 && tries--);
    
    if (tries == -1)
    {
        // timeouted
        spi_set_speed(SD_SPEED_10MHZ);
        return -1;
    }
    
    // Seems after this card is initialized which means bit 0 of R1 will be cleared. Not too sure.
    if (hw_info.capabilities & CAP_VER2_00)
    {
        // cmd58 - ocr, 2nd time...
        // ask about voltage supply
        SD_CS_CLR();
        sd_cmd(58, 0);
        r = sd_get_r3(&r3);
        sd_nec();
        SD_CS_SET();
        
        if (r == 0xff)
        {
            spi_set_speed(SD_SPEED_10MHZ);
            return -1;
        }
        
        if (r & 0xfe)
        {
            // fail
            spi_set_speed(SD_SPEED_10MHZ);
            return -2;
        }
        else 
        {
            if (r3>>30 & 1)
            {
                hw_info.capabilities |= CAP_SDHC;
            }
        }
    }
    
    // with SDHC block length is fixed to 1024
    if ((hw_info.capabilities & CAP_SDHC) == 0)
    {
        // cmd16 - block length...
        SD_CS_CLR();
        sd_cmd(16, 512);
        r = sd_get_r1();
        sd_nec();
        SD_CS_SET();
        
        if (r == 0xff)
        {
            spi_set_speed(SD_SPEED_10MHZ);
            return -1;
        }
        
        if (r & 0xfe)
        {
            // fail
            spi_set_speed(SD_SPEED_10MHZ);
            return -2;
        }
    }
    
    // cmd59 - enable crc...
    // crc on
    SD_CS_CLR();
    sd_cmd(59, 0);
    r = sd_get_r1();
    sd_nec();
    SD_CS_SET();
    
    if (r == 0xff)
    {
        spi_set_speed(SD_SPEED_10MHZ);
        return -1;
    }
    
    if (r & 0xfe)
    {
        // fail
        spi_set_speed(SD_SPEED_10MHZ);
        return -2;
    }
    
    // now we can up the clock to <= 25 MHz
    spi_set_speed(SD_SPEED_10MHZ);
    return 0;
}

/// 
static int sd_read_status(void)
{
	uint16_t r2;

	SD_CS_CLR();
	sd_cmd(13, 0);
	r2 = sd_get_r2();
	sd_nec();
	SD_CS_SET();
    
	if (r2 & 0x8000)
		return -1;

	return 0;
}

// 0xfe marks data start, then len bytes of data and crc16
static int sd_get_data(uint8_t *buf, int len)
{
	int tries = 20000;
	uint8_t r;
	uint16_t _crc16;
	uint16_t calc_crc;
	int i;

	while (tries--) {
		r = spi_txrx(0xff);
		if (r == 0xfe)
			break;
	}
    
	if (tries < 0)
		return -1;

	for (i=0; i<len; i++)
		buf[i] = spi_txrx(0xff);

	_crc16 = spi_txrx(0xff) << 8;
	_crc16 |= spi_txrx(0xff);

	calc_crc = crc16(buf, len);
	if (_crc16 != calc_crc)
    {
		return -1;
	}

	return 0;
}

static int sd_put_data(const uint8_t *buf, int len)
{
	uint8_t r;
	int tries = 10;
	volatile uint8_t b[16];
	int bi = 0;
	uint16_t crc;

    // data start
	spi_txrx(0xfe);

	while (len--)
		spi_txrx(*buf++);

	crc = crc16(buf, len);
	// crc16
	spi_txrx(crc>>8);
	spi_txrx(crc);

	// normally just one dummy read in between... specs don't say how many
	while (tries--)
    {
		b[bi++] = r = spi_txrx(0xff);
		if (r != 0xff)
			break;
	}
	if (tries < 0)
		return -1;

	// poll busy, about 300 reads for 256 MB card
	tries = 100000;
	while (tries--)
    {
		if (spi_txrx(0xff) == 0xff)
			break;
	}
	if (tries < 0)
		return -2;

	// data accepted, WIN
	if ((r & 0x1f) == 0x05)
		return 0;

	return r;
}

static int sd_read_csd(void)
{
	uint8_t buf[16];
	int r;
	int capacity;

	SD_CS_CLR();
	sd_cmd(9, 0);
	r = sd_get_r1();
	if (r == 0xff)
    {
		SD_CS_SET();
		return -1;
	}
	if (r & 0xfe)
    {
		SD_CS_SET();
		return -2;
	}

	r = sd_get_data(buf, 16);
	sd_nec();
	SD_CS_SET();
	if (r < 0)
    {
		// failed to get csd
		return -3;
	}

	if ((buf[0] >> 6) + 1 == 1)
    {
        // CSD v1
        capacity = (((buf[6]&0x3)<<10 | buf[7]<<2 | buf[8]>>6)+1) << (2+(((buf[9]&3) << 1) | buf[10]>>7)) << ((buf[5] & 0xf) - 9);
	}
    else
    {
        // CSD v2
		// this means the card is HC
		hw_info.capabilities |= CAP_SDHC;
        capacity = buf[7]<<16 | buf[8]<<8 | buf[9]; // in 512 kB
        capacity *= 1024; // in 512 B sectors
	}

	hw_info.sectors = capacity;

	// if erase_blk_en = 0, then only this many sectors can be erased at once this is NOT yet tested
	hw_info.erase_sectors = 1;
	if (((buf[10]>>6)&1) == 0)
		hw_info.erase_sectors = ((buf[10]&0x3f)<<1 | buf[11]>>7) + 1;

	return 0;
}

static int sd_read_cid(void)
{
	uint8_t buf[16];
	int r;

	SD_CS_CLR();
	sd_cmd(10, 0);
	r = sd_get_r1();
	if (r == 0xff)
    {
		SD_CS_SET();
		return -1;
	}
	if (r & 0xfe)
    {
		SD_CS_SET();
		return -2;
	}

	r = sd_get_data(buf, 16);
	sd_nec();
	SD_CS_SET();
	if (r < 0)
    {
		// failed to get cid
		return -3;
	}
    
	return 0;
}

/// Read 1 sector (512 bytes)
/// address - sector number
int sd_readsector(uint32_t address, uint8_t *buf)
{
    int r;
    
    SD_SPI_PEND();
    
    SD_CS_CLR();
    if (hw_info.capabilities & CAP_SDHC)
        sd_cmd(17, address); // read single block
    else
        sd_cmd(17, address*512); // read single block
    
    r = sd_get_r1();
    if (r == 0xff)
    {
        SD_CS_SET();
        r = -1;
        SD_SPI_POST();
        return -1;
    }
    if (r & 0xfe) 
    {
        SD_CS_SET();
        SD_SPI_POST();
        return -2;
    }
    
    r = sd_get_data(buf, 512);
    sd_nec();
    SD_CS_SET();
    if (r < 0)
    {
        SD_SPI_POST();
        return -3;
    }
    
    SD_SPI_POST();
    return 0;
}

/// Write 1 sector (512 bytes)
/// address - sector number
int sd_writesector(uint32_t address, const uint8_t *buf)
{
    int r;
    
    SD_SPI_PEND();
    
    SD_CS_CLR();
    if (hw_info.capabilities & CAP_SDHC)
        sd_cmd(24, address); // write block
    else
        sd_cmd(24, address*512); // write block
    
    r = sd_get_r1();
    if (r == 0xff)
    {
        SD_CS_SET();
        SD_SPI_POST();
        return -1;
    }
    if (r & 0xfe)
    {
        SD_CS_SET();
        SD_SPI_POST();
        return -2;
    }
    
    spi_txrx(0xff); // Nwr (>= 1) high bytes
    r = sd_put_data(buf, 512);
    sd_nec();
    SD_CS_SET();
    
    if (r != 0)
    {
        SD_SPI_POST();
        return r;
    }

    SD_SPI_POST();    
    return 0;
}


/// SD card full initialization
int sd_init(void)
{
    int tries = 10;
    
    SD_SPI_PEND();
    
    hw_info.initialized = 0;
    
    while (tries--)
    {
        if (sd_guts_init() == 0)
                break;
    }
    if (tries == -1)
    {
        SD_SPI_POST();
        return -1;
    }
    
    // read status register
    sd_read_status();
    
    sd_read_cid();
    
    if (sd_read_csd() != 0)
    {
        SD_SPI_POST();
        return -1;
    }
    
    hw_info.initialized = 1;
    
    SD_SPI_POST();
    return 0;
}

/// get sector count
int sd_get_sector_count(void)
{
    if (hw_info.initialized) return hw_info.sectors;
    return 0;
}

#endif