/*-----------------------------------------------------------------------*/
/* Low level disk I/O module skeleton for Petit FatFs (C)ChaN, 2014      */
/*-----------------------------------------------------------------------*/

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

static void spi_rx_multi(BYTE *buff, UINT cnt) {
	do {
		*buff++ = spi_rx();
	} while (--cnt);
}

static int rcv_datablock(BYTE *buff, UINT btr) {
	uint8_t token;
	uint16_t count = 10000;

	do token = spi_rx(); while (token == 0xff && --count);

	if (token != 0xfe)
		return 0;

	spi_rx_multi(buff, btr);
	spi_rx();
	spi_rx();

	return 1;
}

/*-----------------------------------------------------------------------*/
/* Initialize Disk Drive                                                 */
/*-----------------------------------------------------------------------*/

static DSTATUS status = STA_NODISK;

DSTATUS disk_status(BYTE pdrv) {
	if (pdrv)
		return STA_NOINIT;
	return status;
}

DSTATUS disk_initialize (BYTE pdrv)
{
	uint8_t n, cmd;
	uint32_t tmr;

	if (!status)
		return 0;

#ifdef DEBUG
	printf("Init SD\r\n");
#endif

	if (sd_init())
		return STA_NOINIT;

#ifdef DEBUG
	printf("Done disk_initialize\r\n");
#endif

	status = 0;

	return 0;
}



/*-----------------------------------------------------------------------*/
/* Read Partial Sector                                                   */
/*-----------------------------------------------------------------------*/

DRESULT disk_read (
	BYTE pdrv,      /* Physical drive number (0) */
	BYTE* buff,		/* Pointer to the destination object */
	DWORD sector,	/* Sector number (LBA) */
	UINT count		/* Byte count (bit15:destination) */
)
{
	DRESULT res = RES_ERROR;
	BYTE rc;
	UINT bc;

	if (pdrv || !count)
		return RES_PARERR;
	if (status && STA_NOINIT)
		return RES_NOTRDY;

	sector *= 512;

	if (count == 1) {
		if ((sd_send_command(SD_READ_SINGLE_BLOCK, sector) == 0)
				&& rcv_datablock(buff, 512))
			count = 0;
	} else if (sd_send_command(SD_READ_MULTIPLE_BLOCK, sector) == 0) {
		do {
			if (!rcv_datablock(buff, 512))
				break;
			buff += 512;
		} while (--count);
		sd_send_command(SD_STOP_TRANSMISSION, 0);
	}

	SD_DESELECT();
	spi_rx();

	return count ? RES_ERROR : RES_OK;
}