Ardupilot2/libraries/AP_HAL_F4Light/hardware/sd/FatFs/drivers/sd.c

#pragma GCC optimize ("O2")


/*
(c) 2017 night_ghost@ykoctpa.ru
 
    SD card and Dataflash low-level driver
    

*/

#include "sd.h"
#include "../diskio.h"
#include "../ff.h"
#include <stdlib.h>
#include <stdint.h>
#include <syscalls.h>


#define CS_HIGH()	spi_chipSelectHigh()
#define CS_LOW()	spi_chipSelectLow(1)
#define	MMC_CD		spi_detect() /* Card detect (yes:true, no:false, default:true) */
#define	MMC_WP		0 /* Write protected (yes:true, no:false, default:false) */



/*--------------------------------------------------------------------------

   Module Private Functions

---------------------------------------------------------------------------*/


/* MMC/SD command */
#define CMD0	(0)		/* GO_IDLE_STATE */
#define CMD1	(1)		/* SEND_OP_COND (MMC) */
#define	ACMD41	(0x80+41)	/* SEND_OP_COND (SDC) */
#define CMD8	(8)		/* SEND_IF_COND */
#define CMD9	(9)		/* SEND_CSD */
#define CMD10	(10)		/* SEND_CID */
#define CMD12	(12)		/* STOP_TRANSMISSION */
#define CMD13	(13)	        /* Get_STATUS */
#define ACMD13	(0x80+13)	/* SD_STATUS (SDC) */
#define CMD16	(16)		/* SET_BLOCKLEN */
#define CMD17	(17)		/* READ_SINGLE_BLOCK */
#define CMD18	(18)		/* READ_MULTIPLE_BLOCK */
#define CMD23	(23)		/* SET_BLOCK_COUNT (MMC) */
#define	ACMD23	(0x80+23)	/* SET_WR_BLK_ERASE_COUNT (SDC) */
#define CMD24	(24)		/* WRITE_BLOCK */
#define CMD25	(25)		/* WRITE_MULTIPLE_BLOCK */
#define CMD32	(32)		/* ERASE_ER_BLK_START */
#define CMD33	(33)		/* ERASE_ER_BLK_END */
#define CMD38	(38)		/* ERASE */
#define CMD55	(55)		/* APP_CMD */
#define CMD58	(58)		/* READ_OCR */


static inline  uint32_t micros() {  return timer_get_count32(TIMER5); }

static inline  uint32_t millis() {  return systick_uptime(); }

typedef uint8_t (*spi_WaitFunc)(uint8_t b);

// utility function 
extern uint8_t spi_spiSend(uint8_t b);
extern uint8_t spi_spiRecv(void);
extern uint8_t spi_spiXchg(uint8_t b);
extern void spi_spiTransfer(const uint8_t *send, uint32_t send_len,  uint8_t *recv, uint32_t recv_len);
extern void spi_chipSelectHigh(void);
extern bool spi_chipSelectLow(bool take_sem);
extern void spi_yield();
extern uint8_t spi_waitFor(uint8_t out, spi_WaitFunc cb, uint32_t dly);
extern uint8_t spi_detect();
extern uint32_t get_fattime ();


static volatile DSTATUS Stat = STA_NOINIT;	/* Physical drive status */

static volatile uint16_t Timer1, Timer2;	        /* 1kHz decrement timer stopped at zero (disk_timerproc()) */
static uint32_t sd_max_sectors=0;



extern int printf(const char *msg, ...);

#if defined(BOARD_SDCARD_CS_PIN)

#define WAIT_IN_ISR

static uint8_t CardType;			/* Card type flags */
static uint8_t no_CMD13 = 0;
static uint8_t csd[16]; // for DMA reads

static int8_t xmit_datablock(const uint8_t *buff,	uint8_t token);


uint8_t sd_get_type() {
    return CardType;
}

/*-----------------------------------------------------------------------*/
/* SPI controls (Platform dependent)                                     */
/*-----------------------------------------------------------------------*/

/* Initialize MMC interface */


/* Exchange a byte */
static inline
uint8_t xchg_spi (
	uint8_t dat	/* Data to send */
)
{
    return spi_spiXchg(dat);
}

static inline 
uint8_t rcvr_spi(){
    return xchg_spi(0xFF);
}

/* Receive multiple byte */
static inline
void rcvr_spi_multi (
	uint8_t *buff,		/* Pointer to data buffer */
	uint16_t btr		/* Number of bytes to receive (even number) */
){
    spi_spiTransfer(NULL,0, buff, btr);
}


/* Send multiple byte */
static inline
void xmit_spi_multi (
	const uint8_t *buff,	/* Pointer to the data */
	uint16_t btx		/* Number of bytes to send (even number) */
)
{
    spi_spiTransfer(buff, btx, NULL, 0);
}

/*
void power_on(void){ // Power on the SD-Card Socket if hardware allows
}

void power_off(void){
}
*/



/*-----------------------------------------------------------------------*/
/* Wait for card ready                                                   */
/*-----------------------------------------------------------------------*/

// TODO: remove all active waiting by transferring check to callback 

static uint8_t wait_ff(uint8_t b){
    return b==0xff;
}

static
int wait_ready (	/* 1:Ready, 0:Timeout */
	uint16_t wt		/* Timeout [ms] */
)
{
    uint8_t d;


#if defined(WAIT_IN_ISR)
    d=spi_waitFor(0xFF,wait_ff,wt*1000);

    return d == 0xFF;
#else

    Timer2 = wt;
    do {
	d = xchg_spi(0xFF);		
	spi_yield(); /* This loop takes a time. Insert rot_rdq() here for multitask envilonment. */
    } while (d != 0xFF && Timer2);	/* Wait for card goes ready or timeout */

    if(d == 0xFF) {
        return 1;
    }
    return 0;
#endif
}



/*-----------------------------------------------------------------------*/
/* Deselect card and release SPI                                         */
/*-----------------------------------------------------------------------*/

static
void deselect_cs (void)
{
    CS_HIGH();		/* Set CS# high */
    xchg_spi(0xFF);	/* Dummy clock (force DO hi-z for multiple slave SPI) */
}



/*-----------------------------------------------------------------------*/
/* Select card and wait for ready                                        */
/*-----------------------------------------------------------------------*/

static
int select_cs (void)	/* 1:OK, 0:Timeout */
{
    if(!CS_LOW()) { /* take semaphore and Set CS# low */
        return 0;		
    }
    xchg_spi(0xFF);	/* Dummy clock (force DO enabled) */
    if (wait_ready(500)) return 1;	/* Wait for card ready */

    deselect_cs();
    return 0;	/* Timeout */
}



/*-----------------------------------------------------------------------*/
/* Receive a data packet from the MMC                                    */
/*-----------------------------------------------------------------------*/

static uint8_t wait_noFF(uint8_t b){
    return b!=0xff;
}


static
int8_t rcvr_datablock (	/* 1:OK, 0:Error */
	uint8_t *buff,	/* Data buffer */
	uint16_t btr	/* Data block length (byte) */
)
{
        uint8_t ret=0;

#if defined(WAIT_IN_ISR)
        ret=spi_waitFor(0xff, wait_noFF, 200000);

        if(ret != 0xFE) goto done;		// Function fails if invalid DataStart token or timeout 
#else
	uint8_t token;

	Timer1 = 200;
	do {					/* Wait for DataStart token in timeout of 200ms */
    	    token = xchg_spi(0xFF);
	    spi_yield();	        /* This loop will take a time. Insert rot_rdq() here for multitask environment. */
	} while ((token == 0xFF) && Timer1);

	if(token != 0xFE) {
	    goto done;		        /* Function fails if invalid DataStart token or timeout */
	}
#endif
	rcvr_spi_multi(buff, btr);		/* Store trailing data to the buffer */
	xchg_spi(0xFF); xchg_spi(0xFF);		/* Discard CRC */

        ret=1; 				        /* Function succeeded */
done:
	return ret;
}



/*-----------------------------------------------------------------------*/
/* Send a data packet to the MMC                                         */
/*-----------------------------------------------------------------------*/

static int8_t xmit_datablock (	        /* 1:OK, 0:Failed */
	const uint8_t *buff,	        /* Ponter to 512 byte data to be sent */
	uint8_t token			/* Token */
)
{
	uint8_t resp;


	if (!wait_ready(500)) return 0;		/* Wait for card ready */

	xchg_spi(token);			/* Send token */
	if (token != 0xFD) {			/* Send data if token is other than StopTran */
	    xmit_spi_multi(buff, 512);		/* Data */
	    xchg_spi(0xFF); xchg_spi(0xFF);	/* Dummy CRC */

	    resp = xchg_spi(0xFF);		/* Receive data resp */
	    if ((resp & 0x1F) != 0x05) {
	        return 0;	/* Function fails if the data packet was not accepted */
	    }
	}
	return 1;
}


/*-----------------------------------------------------------------------*/
/* Send a command packet to the MMC                                      */
/*-----------------------------------------------------------------------*/
static uint8_t buf[6]; // to use DMA

static uint8_t wait_0x80(uint8_t b){
    return (b & 0x80)==0;
}


static
uint8_t send_cmd (	        	/* Return value: R1 resp (bit7==1:Failed to send) */
	uint8_t cmd,		/* Command index */
	uint32_t arg		/* Argument */
)
{
	uint8_t crc, res;


	if (cmd & 0x80) {	/* Send a CMD55 prior to ACMD<n> */
		cmd &= 0x7F;
		res = send_cmd(CMD55, 0);
		if (res > 1) {
		    return res;
		}
	}

	/* Select the card and wait for ready except to stop multiple block read */
	if (cmd != CMD12) {
		deselect_cs();
//		spi_yield(); // sync quant so no interrupts when receiving answer
		if (!select_cs()) {
		    return 0xFF;
		}
	}

	crc = 0x01;				/* Dummy CRC + Stop */
	if (cmd == CMD0) crc = 0x95;		/* Valid CRC for CMD0(0) */
	if (cmd == CMD8) crc = 0x87;		/* Valid CRC for CMD8(0x1AA) */


	/* Send command packet */
	buf[0] = 0x40 | cmd;			/* Start + command index */
	buf[1] = (uint8_t)(arg >> 24);		/* Argument[31..24] */
	buf[2] = (uint8_t)(arg >> 16);		/* Argument[23..16] */
	buf[3] = (uint8_t)(arg >> 8);		/* Argument[15..8] */
	buf[4] = (uint8_t)arg;			/* Argument[7..0] */
        buf[5] = crc;                           /* CRC + Stop */

        xmit_spi_multi(buf, 6);	        // entire command in one packet

	/* Receive command resp */
	if (cmd == CMD12) xchg_spi(0xFF);	/* Discard following one byte when CMD12 */

#if defined(WAIT_IN_ISR)
        res=spi_waitFor(0xff, wait_0x80, 20000);

        return res;

#else

	uint8_t n = 255;		        /* Wait for response (10 bytes max) */
	do {
	    res = xchg_spi(0xFF);
	    spi_yield();	                /* This loop will take a time. Insert rot_rdq() here for multitask environment. */
	} while ((res & 0x80) && --n);

	return res;				/* Return received response */
#endif
}



/*--------------------------------------------------------------------------

   Public Functions

---------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------*/
/* Initialize disk drive                                                 */
/*-----------------------------------------------------------------------*/

DSTATUS sd_initialize() {
	uint8_t n, cmd, ty, ocr[4] ;
	
	if (Stat & STA_NODISK) return Stat;	/* Is card existing in the soket? */
	if(!(Stat & STA_NOINIT) )  return Stat; // already done


	ty = 0;
	n = send_cmd(CMD0, 0);
	if (n == 1 || n==0) {			                /* Put the card SPI/Idle state */
	    Timer1 = 5000;	
	    n=send_cmd(CMD8, 0x1AA);		                /* Initialization timeout = 5 sec */
	    if((n&4) == 0) rcvr_spi_multi(ocr, 4);              /* Get 32 bit return value of R7 resp if not illegal command */
	    if (n == 1) {	                /* SDv2? */
		if (ocr[2] == 0x01 && ocr[3] == 0xAA) {		/* Is the card supports vcc of 2.7-3.6V? and did echoing */
			while(Timer1){                 	        /* Wait for end of initialization with ACMD41(HCS) */
			    n = send_cmd(ACMD41, 1UL << 30);
			    if(n==0) break;
			}
			if (Timer1){
			    n = send_cmd(CMD58, 0);
			    if((n&4) == 0) rcvr_spi_multi(ocr, 4); /* Get 32 bit return value of R7 resp if not illegal command */
			    if( n <= 1) {	                /* Check CCS bit in the OCR */				
				ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2;	/* Card id SDv2 */
			    } else {
			        printf("\nSD CMD58 failed!\n");
			    }
			}else{
			    printf("\nSD timeout!\n");
			}
		}
	    } else {	                              /* Not SDv2 card */
		if (send_cmd(ACMD41, 0) <= 1) 	{	/* SDv1 or MMC? */
			ty = CT_SD1; cmd = ACMD41;	/* SDv1 (ACMD41(0)) */
		} else {
			ty = CT_MMC; cmd = CMD1;	/* MMCv3 (CMD1(0)) */
		}
		while(Timer1) {         		/* Wait for end of initialization */
		    n=send_cmd(cmd, 0);
		    if(n==0) break;
		}
			
		if (Timer1){
		    if(send_cmd(CMD16, 512) != 0) {	/* Set block length: 512 */
        		ty = 0;
        		printf("\nSD CMD16 failed!\n");
        	    }
		} else {
		    ty = 0;
		    printf("\nSD timeout!\n");
		}
	    }
	} else {
	    ty=0;
	}
	CardType = ty;	/* Card type */

	if (ty) {			/* OK */
	    Stat &= ~STA_NOINIT;	/* Clear STA_NOINIT flag */
	    uint8_t i;
	    for(i=0;i<15;i++){  //      15 tries to get size
                sd_getSectorCount(&sd_max_sectors);
                if(sd_max_sectors!=0) break;
            }
        } else {			/* Failed */
	    Stat = STA_NOINIT;
	}

	deselect_cs();

	return Stat;
}

static uint8_t restart_card(){
    Stat = STA_NOINIT;
    
    sd_initialize();
    
    if(!(Stat & STA_NOINIT) )  return true; // OK
    return false;
}

uint8_t sd_getSectorCount(uint32_t *ptr){
    uint32_t csize;
    
    if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) {
        if ((csd[0] >> 6) == 1) {	/* SDC ver 2.00 */
	    csize = csd[9] + ((uint16_t)csd[8] << 8) + ((uint32_t)(csd[7] & 63) << 16) + 1;
	    *ptr = csize << 10;
	} else {					/* SDC ver 1.XX or MMC ver 3 */
	    uint8_t n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2;
	    csize = (csd[8] >> 6) + ((uint16_t)csd[7] << 2) + ((uint16_t)(csd[6] & 3) << 10) + 1;
	    *ptr = csize << (n - 9);
	}

	return RES_OK;
    }
    return RES_ERROR;

}

/*-----------------------------------------------------------------------*/
/* Get disk status                                                       */
/*-----------------------------------------------------------------------*/

DSTATUS sd_status (){

	return Stat;	/* Return disk status */
}

uint8_t sd_get_state(){
    if(!no_CMD13) {
        if(send_cmd(CMD13, 0)<=1){
            uint8_t ret = xchg_spi(0xFF);
            return ret;
        }
        
        no_CMD13=1;
    }
    
    return 0; // CMD13 not supported so all OK

/*
7 - out of range / CSD overwrite
6 • Erase param: An invalid selection for erase, sectors or groups.
5 • Write protect violation: The command tried to write a write-protected block.
4 • Card ECC failed: Card internal ECC was applied but failed to correct the data.
3 • CC error: Internal card controller error.
2 • Error: A general or an unknown error occurred during the operation.
1 • Write protect erase skip | lock/unlock command failed: This status bit has two functions over-
loaded. It is set when the host attempts to erase a write-protected sector or makes a sequence or
password errors during card lock/unlock operation.
0 • Card is locked: Set when the card is locked by the user. Reset when it is unlocked.

*/

}


/*-----------------------------------------------------------------------*/
/* Read sector(s)                                                        */
/*-----------------------------------------------------------------------*/

static bool single_sector_card=false;


DRESULT sd_read (
	uint8_t *buff,		/* Pointer to the data buffer to store read data */
	uint32_t sector,        /* Start sector number (LBA) */
	uint16_t count		/* Number of sectors to read (1..128) */
)
{
        uint8_t ret;
        uint16_t sectorInc=1;
        
	if (!count) return RES_PARERR;		        /* Check parameter */
	if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check if drive is ready */
	
	if(sd_max_sectors && sector > sd_max_sectors) return RES_PARERR;		        /* Check parameter */

	if (!(CardType & CT_BLOCK)) {
	    sectorInc = 512;
	    sector *= sectorInc;	/* LBA to BA conversion (byte addressing cards) */
	}


	if (count == 1) {	                /* Single sector read */
		if (( (ret=send_cmd(CMD17, sector)) == 0)	/* READ_SINGLE_BLOCK */
		    && rcvr_datablock(buff, 512)) {
		        count = 0;
		}
		    
	} else {				/* Multiple sector read */
	    if(single_sector_card) {
		do {
		    if ((ret=send_cmd(CMD17, sector)) == 0){	/* READ_SINGLE_BLOCK */
			if(rcvr_datablock(buff, 512)) {
			    buff   += 512;
			    sector += sectorInc;
			} else {
			    break;
			}
		    } else {
		        break;
		    }
                    deselect_cs();
		    spi_yield();
		} while(--count);

	    } else {
//	        send_cmd(CMD23, count);
		if (CardType & CT_SDC) send_cmd(ACMD23, count);	/* Predefine number of sectors */

    	        uint16_t got=0;
            	if ((ret=send_cmd(CMD18, sector)) == 0) {	/* READ_MULTIPLE_BLOCK */
    		    do {
		        if (!rcvr_datablock(buff, 512)) {
		            break;
		        }
		        got++;
		        buff += 512;
		    } while (--count);
		    if(got) send_cmd(CMD12, 0);				/* STOP_TRANSMISSION */
		    
		}
	        if(count) {     // readed only 1 block or none
		    single_sector_card=true;
		    sector+=got*sectorInc;
		    restart_card();
		    return sd_read(buff, sector/sectorInc, count); // read remaining in single sector mode
		}
	    }
	}
	deselect_cs();
        if(count==0) return  RES_OK;	/* Return result */
        Stat = STA_NOINIT;        
	return RES_ERROR;
}



/*-----------------------------------------------------------------------*/
/* Write sector(s)                                                       */
/*-----------------------------------------------------------------------*/

DRESULT sd_write (
	const uint8_t *buff,	/* Ponter to the data to write */
	uint32_t sector,		/* Start sector number (LBA) */
	uint16_t count			/* Number of sectors to write (1..128) */
)
{
	if (!count) return RES_PARERR;		/* Check parameter */
	if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check drive status */
	if (Stat & STA_PROTECT) return RES_WRPRT;	/* Check write protect */
	if(sd_max_sectors && sector > sd_max_sectors) return RES_PARERR;		        /* Check parameter */

        uint16_t sectorInc = 1;


	if (!(CardType & CT_BLOCK)){
	    sectorInc = 512;
	    sector *= sectorInc;	/* LBA ==> BA conversion (byte addressing cards) */
	}

	if (count == 1) {	/* Single sector write */
		if (send_cmd(CMD24, sector) == 0) {	/* WRITE_BLOCK */
		    if( xmit_datablock(buff, 0xFE)) {
			count = 0;
		    }		    
		}
	}
	else {	
	    if(single_sector_card) {
	        do {
    		    if ((send_cmd(CMD24, sector) == 0)	/* WRITE_BLOCK */
			&& xmit_datablock(buff, 0xFE)) {
			sector+=sectorInc;
			buff += 512;
		    } else break; // error
	        } while(--count);
	    } else {
				/* Multiple sector write */
		if (CardType & CT_SDC) send_cmd(ACMD23, count);	/* Predefine number of sectors */
                uint32_t sent=0;
		if (send_cmd(CMD25, sector) == 0) {	/* WRITE_MULTIPLE_BLOCK */
		    do {
			if (!xmit_datablock(buff, 0xFC)) break;
			buff += 512;
			sent++;
		    } while (--count);
		    if (!xmit_datablock(0, 0xFD)) count = 1;	/* STOP_TRAN token */
		}
		if(count) {
		    single_sector_card=true;
	            sector+=sent*sectorInc;
		    restart_card();
		    return sd_write(buff, sector/sectorInc, count); // write remaining in single sector mode
		}
	    }
	}

	if(count) {
            deselect_cs();
            Stat = STA_NOINIT;        
	    return RES_ERROR;
	}

        uint8_t ret = sd_get_state(); // reset errors

	deselect_cs();
	
	if(ret) return RES_ERROR;

	return RES_OK;	/* Return result */
}


/*-----------------------------------------------------------------------*/
/* Miscellaneous drive controls other than data read/write               */
/*-----------------------------------------------------------------------*/

DRESULT sd_ioctl (
	uint8_t cmd,		/* Control command code */
	void *buff		/* Pointer to the conrtol data */
)
{
    DRESULT res;
    uint8_t n;
    uint32_t *dp, st, ed;

    uint8_t *ptr = (uint8_t *)buff;

    res = RES_ERROR;

    if (cmd== CTRL_POWER){

        switch (*ptr) {
        case 0: // Sub control code == 0 (POWER_OFF) 
//             if (SD_PRESENT())
//                 power_off(); // Power off 
             res = RES_OK;
             break;
         case 1: // Sub control code == 1 (POWER_ON) 
//             power_on(); // Power on 
             res = RES_OK;
             break;
         case 2: // Sub control code == 2 (POWER_GET) 
             *(ptr + 1) = (uint8_t) MMC_CD;
             break;
         default:
             res = RES_PARERR;
             break;
        }

    } else {
    
        if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check if drive is ready */

	switch (cmd) {
	case CTRL_SYNC :		/* Wait for end of internal write process of the drive */
		if (select_cs()) res = RES_OK;
		break;

	case GET_SECTOR_COUNT :	/* Get drive capacity in unit of sector (uint32_t) */
	        res=sd_getSectorCount((uint32_t*)buff);
		break;

	case GET_BLOCK_SIZE :	/* Get erase block size in unit of sector (uint32_t) */
		if (CardType & CT_SD2) {	/* SDC ver 2.00 */
		    if (send_cmd(ACMD13, 0) == 0) {	/* Read SD status */
			xchg_spi(0xFF);
			if (rcvr_datablock(csd, 16)) {				/* Read partial block */
				for (n = 64 - 16; n; n--) xchg_spi(0xFF);	/* Purge trailing data */
				*(uint32_t*)buff = 16UL << (csd[10] >> 4);
				res = RES_OK;
			}
		    }
		} else {				/* SDC ver 1.XX or MMC */
		    if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) {	/* Read CSD */
			if (CardType & CT_SD1) {	/* SDC ver 1.XX */
				*(uint32_t*)buff = (((csd[10] & 63) << 1) + ((uint16_t)(csd[11] & 128) >> 7) + 1) << ((csd[13] >> 6) - 1);
			} else {			/* MMC */
				*(uint32_t*)buff = ((uint16_t)((csd[10] & 124) >> 2) + 1) * (((csd[11] & 3) << 3) + ((csd[11] & 224) >> 5) + 1);
			}
			res = RES_OK;
		    }
		}
		break;

	case CTRL_TRIM :	                                /* Erase a block of sectors (used when _USE_ERASE == 1) */
		if (!(CardType & CT_SDC)) break;		/* Check if the card is SDC */
		if (sd_ioctl(MMC_GET_CSD, csd)) break;        	/* Get CSD */
		if (!(csd[0] >> 6) && !(csd[10] & 0x40)) break;	/* Check if sector erase can be applied to the card */
		dp = buff; st = dp[0]; ed = dp[1];		/* Load sector block */
		if (!(CardType & CT_BLOCK)) {
			st *= 512; ed *= 512;
		}
		if (send_cmd(CMD32, st) == 0 && send_cmd(CMD33, ed) == 0 && send_cmd(CMD38, 0) == 0 && wait_ready(30000)) {	/* Erase sector block */
			res = RES_OK;	                        /* FatFs does not check result of this command */
		}
		break;
/*
        case MMC_GET_TYPE: // Get card type flags (1 byte) 
             *ptr = g_card_type;
             res = RES_OK;
             break;
*/
         case MMC_GET_CSD: /* Receive CSD as a data block (16 bytes) */
             if (send_cmd(CMD9, 0) == 0 /* READ_CSD */
             && rcvr_datablock(ptr, 16))
                 res = RES_OK;
             break;

         case MMC_GET_CID: /* Receive CID as a data block (16 bytes) */
            if (send_cmd(CMD10, 0) == 0 /* READ_CID */
            && rcvr_datablock(ptr, 16)){
/*
    sdcard.manufacturerID = cid[0];
    sdcard.oemID = (cid[1] << 8) | cid[2];
    sdcard.productName[0] = cid[3];
    sdcard.productName[1] = cid[4];
    sdcard.productName[2] = cid[5];
    sdcard.productName[3] = cid[6];
    sdcard.productName[4] = cid[7];
    sdcard.productRevisionMajor = cid[8] >> 4;
    sdcard.productRevisionMinor = cid[8] & 0x0F;
    sdcard.productSerial = (cid[9] << 24) | (cid[10] << 16) | (cid[11] << 8) | cid[12];
    sdcard.productionYear = (((cid[13] & 0x0F) << 4) | (cid[14] >> 4)) + 2000;
    sdcard.productionMonth = cid[14] & 0x0F;
*/
                 res = RES_OK;
            }
            break;
            
         case MMC_GET_OCR: /* Receive OCR as an R3 resp (4 bytes) */
             if (send_cmd(CMD58, 0) == 0) { /* READ_OCR */
                 // for (n = 4; n; n--) *ptr++ = rcvr_spi();
                 rcvr_spi_multi(ptr, 4);
                 res = RES_OK;
             }
             break;

         case MMC_GET_SDSTAT: /* Receive SD status as a data block (64 bytes) */
             if (send_cmd(ACMD13, 0) == 0) { /* SD_STATUS */
                 rcvr_spi();
                 if (rcvr_datablock(ptr, 64))
                     res = RES_OK;
             }
             break;

	default:
		res = RES_PARERR;
	}

	deselect_cs();
    }
    return res;
}


/*-----------------------------------------------------------------------*/
/* Device timer function                                                 */
/*-----------------------------------------------------------------------*/
/* This function must be called from timer interrupt routine in period
/  of 1 ms to generate card control timing.
*/

void sd_timerproc (void)
{
	uint16_t n;
	uint8_t s;


	n = Timer1;						/* 1kHz decrement timer stopped at 0 */
	if (n) Timer1 = --n;
	n = Timer2;
	if (n) Timer2 = --n;

	s = Stat;
	if (MMC_WP) {	/* Write protected */
		s |= STA_PROTECT;
	} else {		/* Write enabled */
		s &= ~STA_PROTECT;
	}
	if (MMC_CD) {	/* Card is in socket */
		s &= ~STA_NODISK;
	} else {		/* Socket empty */
		s |= (STA_NODISK | STA_NOINIT);
	}
	Stat = s;
}


#elif defined(BOARD_DATAFLASH_FATFS)

// emulate SD card on board's dataflash

// flash size
#define DF_NUM_PAGES 0x1f00
#define DF_PAGE_SIZE 256L

#define DF_RESET BOARD_DATAFLASH_CS_PIN 

//Winbond M25P16 Serial Flash Embedded Memory 16 Mb, 3V
#define JEDEC_WRITE_ENABLE           0x06
#define JEDEC_WRITE_DISABLE          0x04
#define JEDEC_READ_STATUS            0x05
#define JEDEC_WRITE_STATUS           0x01
#define JEDEC_READ_DATA              0x03
#define JEDEC_FAST_READ              0x0b
#define JEDEC_DEVICE_ID              0x9F
#define JEDEC_PAGE_WRITE             0x02

#define JEDEC_WREAR                  0xC5
#define JEDEC_ENTER_4B_MODE          0xB7

#define JEDEC_BULK_ERASE             0xC7 // full chip erase
#define JEDEC_SECTOR_ERASE           0x20 // 4k erase
#define JEDEC_PAGE_ERASE             0xD8 // 64K erase

#define JEDEC_STATUS_BUSY            0x01
#define JEDEC_STATUS_WRITEPROTECT    0x02
#define JEDEC_STATUS_BP0             0x04
#define JEDEC_STATUS_BP1             0x08
#define JEDEC_STATUS_BP2             0x10
#define JEDEC_STATUS_TP              0x20
#define JEDEC_STATUS_SEC             0x40
#define JEDEC_STATUS_SRP0            0x80

//#define expect_memorytype            0x20
//#define expect_capacity              0x15

#define MAX_ERASE_SIZE 16384

static bool flash_died=false;
static uint8_t df_manufacturer;
static uint16_t df_device;
static uint32_t erase_size = BOARD_DATAFLASH_ERASE_SIZE;
static uint8_t addr_cmd_length = 4;
static uint8_t addr_cmd_start = 1;
static bool  addr_4bit = 0;

 #if BOARD_DATAFLASH_ERASE_SIZE  >= 65536
   static uint8_t erase_cmd=JEDEC_PAGE_ERASE;
 #else
   static uint8_t erase_cmd=JEDEC_SECTOR_ERASE;
 #endif

#pragma pack(push,1)
static struct {
    uint8_t cmd[5]; // for DMA transfer
    uint8_t sector[DF_PAGE_SIZE]; // we ALWAYS can use DMA!
} buf;
#pragma pack(pop)


//[ task management for USB MSC mode
void  hal_set_task_active(void * handle);
void  hal_context_switch_isr();
void *hal_register_task(voidFuncPtr task, uint32_t stack);
void  hal_set_task_priority(void * handle, uint8_t prio);
bool hal_is_armed();
//]
    
#define FLASH_ERASE_QUEUE_SIZE 16

static void *_flash_erase_task IN_CCM;
static volatile uint16_t fe_read_ptr IN_CCM;
static volatile uint16_t fe_write_ptr IN_CCM;
typedef struct {
    uint32_t b;
    uint32_t e;
} fe_item;

static fe_item flash_erase_queue[FLASH_ERASE_QUEUE_SIZE] IN_CCM;
static void * _flash_erase_task IN_CCM;

static void sd_flash_eraser();
    


static bool chip_is_clear=false;

uint8_t sd_get_type() {
    return 0;
}

/*-----------------------------------------------------------------------*/
/* SPI controls (Platform dependent)                                     */
/*-----------------------------------------------------------------------*/

/* Initialize MMC interface */


/* Exchange a byte */
static inline uint8_t spi_write(
	uint8_t dat	/* Data to send */
)
{
    return spi_spiXchg(dat);
}

static inline uint8_t spi_read(){
    return spi_spiXchg(0xFF);
}

/* Receive multiple byte */
static inline void read_spi_multi (
	uint8_t *buff,		/* Pointer to data buffer */
	uint16_t btr		/* Number of bytes to receive (even number) */
){

    spi_spiTransfer(NULL,0, buff, btr);
}


/* Send multiple byte */
static inline void write_spi_multi (
	const uint8_t *buff,	/* Pointer to the data */
	uint16_t btx	        /* Number of bytes to send (even number) */
)
{

    spi_spiTransfer(buff, btx,NULL, 0);
}


static inline
void cs_release (void)
{
	CS_HIGH();		/* Set CS# high */
}




static
bool cs_assert(void)	/* 1:OK, 0:Timeout */
{
	if(!CS_LOW()) { /* take semaphore and Set CS# low */
	    return 0;		
	}
	return 1;	
}



// This function is mainly to test the device
static void ReadManufacturerID()
{
    // activate dataflash command decoder
    if (!cs_assert()) return;

    // Read manufacturer and ID command...
    buf.cmd[0] = JEDEC_DEVICE_ID;

    spi_spiTransfer(buf.cmd, 1, buf.cmd, addr_cmd_length);
    
    df_manufacturer =  buf.cmd[0];
    df_device       = (buf.cmd[1] << 8) | buf.cmd[2]; //capacity

    // release SPI bus for use by other sensors
    cs_release();
}
            
            
// Read the status register
static uint8_t ReadStatusReg()
{
    uint8_t tmp;

    if (!cs_assert()) return JEDEC_STATUS_BUSY;

    // Read status command
    buf.cmd[0] = JEDEC_READ_STATUS;

    spi_spiTransfer(buf.cmd, 1, &buf.cmd[1], 1);
    tmp = buf.cmd[1];

    cs_release();
    return tmp;
}

static uint8_t ReadStatus()
{
  // We only want to extract the READY/BUSY bit
    uint8_t status = ReadStatusReg();

    return status & JEDEC_STATUS_BUSY;
}

/*-----------------------------------------------------------------------*/
// Wait for dataflash
/*-----------------------------------------------------------------------*/

static int wait_ready (	/* 1:Ready, 0:Timeout */
	uint16_t wt			/* Timeout [ms] */
)
{
    if(flash_died) return 0;

    Timer2 = wt;
    do {
        if(ReadStatus()==0) return 1; //ready

	spi_yield();    /* This loop takes a time. Insert rot_rdq() here for multitask envilonment. */
    } while(Timer2);	/* Wait for card goes ready or timeout */

    flash_died = true;
    
    return 0;
}






static void Flash_Jedec_WriteEnable(void){ 
    if (!wait_ready(500)) return;  /* Wait for card ready */

    if (!cs_assert()) return;
    spi_write(JEDEC_WRITE_ENABLE);
    cs_release();
}


static void Flash_Enter4B_Mode(){ 
    if (!wait_ready(500)) return;  /* Wait for card ready */

    if (!cs_assert()) return;
    spi_write(JEDEC_ENTER_4B_MODE);
    cs_release();
}


static bool read_page( uint8_t *ptr, uint32_t pageNum){
    uint32_t PageAdr = pageNum * DF_PAGE_SIZE;

    if (!wait_ready(500)) return 0;  /* Wait for card ready */

    if (!cs_assert()) return 0;

    {    
        uint8_t i = 0;

        buf.cmd[i++] = JEDEC_READ_DATA;
        if(addr_4bit) {
            buf.cmd[i++] = (PageAdr >> 24) & 0xff;
        }
        buf.cmd[i++] = (PageAdr >> 16) & 0xff;
        buf.cmd[i++] = (PageAdr >>  8) & 0xff;
        buf.cmd[i++] = (PageAdr >>  0) & 0xff;
    }
    
    spi_spiTransfer(&buf.cmd[0], addr_cmd_length, buf.sector, DF_PAGE_SIZE);

    cs_release();

    uint16_t i;
    for(i=0; i<DF_PAGE_SIZE;i++){
        ptr[i] = ~buf.sector[i];    // let filesystem will be inverted, this allows extend files without having to Read-Modify-Write on FAT
    }                               // original: 0xFF is clear and 0 can be programmed any time
                                    // inverted: 0 is clear and 1 can be programmed any time
                                    // to mark cluster as used it should be set 1 in the FAT. Also new entries in dirs can be created without RMW
    return 1;
}


static bool write_page(const uint8_t *ptr, uint32_t pageNum){
    uint32_t PageAdr = pageNum * DF_PAGE_SIZE;

    {
        uint16_t i;
        for(i=0; i<DF_PAGE_SIZE;i++){
            buf.sector[i] = ~ptr[i];       // let filesystem will be inverted, this allows extend files without having to Read-Modify-Write on FAT
        }
    }
    
    Flash_Jedec_WriteEnable();

    if (!cs_assert()) return 0;

    uint8_t i = addr_cmd_start;

    buf.cmd[i++] = JEDEC_PAGE_WRITE;
    if(addr_4bit) {
        buf.cmd[i++] = (PageAdr >> 24) & 0xff;
    }
    buf.cmd[i++] = (PageAdr >> 16) & 0xff;
    buf.cmd[i++] = (PageAdr >>  8) & 0xff;
    buf.cmd[i++] = (PageAdr >>  0) & 0xff;

#if 0 
    write_spi_multi(&buf.cmd[addr_cmd_start], addr_cmd_length);
    write_spi_multi(buf.sector, DF_PAGE_SIZE);
#else
    write_spi_multi(&buf.cmd[addr_cmd_start], addr_cmd_length + DF_PAGE_SIZE);
#endif
    cs_release();
    return 1;
}

static bool erase_page(uint16_t pageNum)
{

    Flash_Jedec_WriteEnable();

    uint32_t PageAdr = pageNum * DF_PAGE_SIZE;

    uint8_t i=0;
    buf.cmd[i++] = erase_cmd;
    if(addr_4bit) {
        buf.cmd[i++] = (PageAdr >> 24) & 0xff;
    }
    buf.cmd[i++] = (PageAdr >> 16) & 0xff;
    buf.cmd[i++] = (PageAdr >>  8) & 0xff;
    buf.cmd[i++] = (PageAdr >>  0) & 0xff;

    if (!cs_assert()) return 0;
    write_spi_multi(buf.cmd, addr_cmd_length);
    cs_release();
    return 1;
}

static void ChipErase()
{

    buf.cmd[0] = JEDEC_BULK_ERASE;

    Flash_Jedec_WriteEnable();
    
    if (!cs_assert()) return;

    write_spi_multi(buf.cmd, 1);    
    
    chip_is_clear=true;
    
    cs_release();
}

uint8_t sd_getSectorCount(uint32_t *ptr){

    uint8_t capacity = df_device & 0xFF;
    uint8_t memtype =  (df_device>>8) & 0xFF;
    uint32_t size=0;

    const char * mfg=NULL;
    
    switch(df_manufacturer){
    case 0xEF: //  Winbond Serial Flash 
        if (memtype == 0x40) {
            mfg="Winbond";
            size = (1 << ((capacity & 0x0f) + 8));
/*
  const uint8_t _capID[11]      = {0x10,  0x11,   0x12,   0x13,   0x14, 0x15, 0x16, 0x17, 0x18,  0x19,  0x43};
  const uint32_t _memSize[11]  = {64L*K, 128L*K, 256L*K, 512L*K, 1L*M, 2L*M, 4L*M, 8L*M, 16L*M, 32L*M, 8L*M}; megabytes
  const uint32_t _memSize[11]  = {64L*K, 128L*K, 256L*K, 512L*K, 1L*M, 2L*M, 4L*M, 8L*M, 16L*M, 32L*M, 8L*M}; megabytes
*/
            erase_size=4096;
            erase_cmd=JEDEC_SECTOR_ERASE;
        }
        break;
    case 0xbf: // SST
        if (memtype == 0x25) {
            mfg="Microchip";
            size = (1 << ((capacity & 0x07) + 12));
        }
        break;
        
    case 0x20: // micron
        if (memtype == 0xba){// JEDEC_ID_MICRON_N25Q128        0x20ba18
            mfg="Micron";
            size = (1 << ((capacity & 0x0f) + 8));
            erase_size=4096;
            erase_cmd=JEDEC_SECTOR_ERASE;
        } else if(memtype==0x20) {  // JEDEC_ID_MICRON_M25P16         0x202015
            mfg="Micron";
            size = (1 << ((capacity & 0x0f) + 8));
        }
        break;
        
    case 0xC2:     //JEDEC_ID_MACRONIX_MX25L6406E   0xC22017
        if (memtype == 0x20) { 
            mfg="MACRONIX";
            size = (1 << ((capacity & 0x0f) + 8));
            erase_size=4096;
            erase_cmd=JEDEC_SECTOR_ERASE;
        }
        break;
    
    case 0x9D: // ISSI
        if (memtype == 0x40 || memtype == 0x30) {
            mfg = "ISSI";
            size      = (1 << ((capacity & 0x0f) + 8));
        }
        break;

    default:
        break;
    }

    if(mfg && size) {
        printf("%s SPI Flash found sectors=%ld\n", mfg, size);
    } else  {
        printf("unknown Flash! mfg=%x type=%x cap=%x\n ",df_manufacturer, memtype, capacity);
        size = BOARD_DATAFLASH_PAGES; // as defined 
    } 

    if(size>65537) {
        addr_cmd_length = 5;
        addr_cmd_start  = 0;
        addr_4bit       = true;
        Flash_Enter4B_Mode();
    }

    if(erase_size > MAX_ERASE_SIZE)   size -= (erase_size/DF_PAGE_SIZE); // reserve for RMW ops
        
    *ptr = size / (FAT_SECTOR_SIZE/DF_PAGE_SIZE);    // in 512b blocks
        
    return RES_OK;

}

/*--------------------------------------------------------------------------

   Public Functions

---------------------------------------------------------------------------*/


/*-----------------------------------------------------------------------*/
/* Initialize disk drive                                                 */
/*-----------------------------------------------------------------------*/

DSTATUS sd_initialize () {
    static bool initialized=0;

    if(initialized) return Stat;

    ReadManufacturerID();

    sd_getSectorCount(&sd_max_sectors);

    initialized=1;

    Stat=0;
    return Stat;
}


/*-----------------------------------------------------------------------*/
/* Get disk status                                                       */
/*-----------------------------------------------------------------------*/

DSTATUS sd_status (){

	return Stat;	/* Return disk status */
}


/*-----------------------------------------------------------------------*/
/* Read sector(s)                                                        */
/*-----------------------------------------------------------------------*/

DRESULT sd_read (
	uint8_t *buff,		/* Pointer to the data buffer to store read data */
	uint32_t sector,        /* Start sector number (LBA) */
	uint16_t count		/* Number of sectors to read (1..128) */
)
{
        
	if (!count) return RES_PARERR;		        /* Check parameter */
	if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check if drive is ready */
	if(sector > sd_max_sectors) return RES_PARERR;		        /* Check parameter */

        count  *= FAT_SECTOR_SIZE/DF_PAGE_SIZE; // 256bytes page from 512byte sector
        sector *= FAT_SECTOR_SIZE/DF_PAGE_SIZE;
    

	do {
	    if(!read_page(buff, sector)) break;
	    buff   += DF_PAGE_SIZE;
	    sector += 1;
	} while(--count);

	return count ? RES_ERROR : RES_OK;	/* Return result */
}



/*-----------------------------------------------------------------------*/
/* Write sector(s)                                                       */
/*-----------------------------------------------------------------------*/


bool sd_move_block(uint32_t sec_from, uint32_t sec_to, const uint8_t *buff, uint32_t sec, uint16_t count);

/*
    в системе нет памяти, а выполнять Read-Modify-Write для флешки как-то надо. придется использовать свободный блок 
    флеши для буфферизации. Ресурс это конечно уменьшит, зато быстрее перепаяют на нормальную :) 
    по-уму надо читать FAT и выбирать случайный свободный кластер, но использование для этого функций самой FatFs 
    приводит к смещению окна в неожиданное время, например когда запись вызывается для сброса измененных структур FAT
    
*/
bool sd_move_block(uint32_t sec_from, uint32_t sec_to, const uint8_t *buff, uint32_t sec, uint16_t count){

    if(!erase_page(sec_to)) return RES_ERROR;

    uint8_t *sbuffer = malloc(DF_PAGE_SIZE); // read just the needed sector, not in stack for not in CCM
    if(!sbuffer) return false;

    uint16_t cnt;
    
    for(cnt=erase_size/DF_PAGE_SIZE; cnt>0; cnt--){
        if(buff && sec_from >= sec && sec_from < (sec + count) ) { // if replacement data fit to sector
            memcpy(sbuffer, buff, DF_PAGE_SIZE);       // will use it
            buff+=DF_PAGE_SIZE;
        } else {                                // read from source
            if(!read_page(sbuffer, sec_from)) break;
        }
        if(!write_page(sbuffer, sec_to)) break;
        sec_from++;
        sec_to++;    
    }
    
    free(sbuffer);
    
    return cnt==0;
}


DRESULT sd_write (
	const uint8_t *buff,	/* Ponter to the data to write */
	uint32_t sector,		/* Start sector number (LBA) */
	uint16_t count			/* Number of sectors to write (1..128) */
)
{
	if (!count) return RES_PARERR;		/* Check parameter */
	if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check drive status */
	if(sector > sd_max_sectors) return RES_PARERR;		        /* Check parameter */


        uint16_t pos = sector % erase_size/FAT_SECTOR_SIZE; // number of sector in erase block
        if( pos == 0 && count >= erase_size/FAT_SECTOR_SIZE){ // begin of erase block - write full cluster
            count  *= FAT_SECTOR_SIZE/DF_PAGE_SIZE; // 256bytes page from 512byte sector
            sector *= FAT_SECTOR_SIZE/DF_PAGE_SIZE;
            if(!erase_page(sector)) return RES_ERROR;
    	    do {
    	        if(!write_page(buff, sector)) break;
	        buff   += DF_PAGE_SIZE;
	        sector += 1;
	    } while(--count);
	    
        } else { // read-modify-write
            uint8_t *ptr;
            bool need_erase = false; // check for free space for write
            
            {
                uint8_t *sbuffer = malloc(FAT_SECTOR_SIZE*count); // read just the needed sectors
                if(!sbuffer) return RES_ERROR; // no memory at all
                
                ptr = sbuffer;

                uint32_t r_sector = sector;
                uint16_t r_count  = count;

                r_sector *= FAT_SECTOR_SIZE/DF_PAGE_SIZE;
                r_count  *= FAT_SECTOR_SIZE/DF_PAGE_SIZE;      // read data will be rewritten

	        do {
	            if(!read_page(ptr, r_sector)) break;
	            ptr   += DF_PAGE_SIZE;
	            r_sector += 1;
                } while(--r_count);
                
                
                const uint8_t *pp;
                for(ptr = sbuffer, pp=buff; ptr < sbuffer+FAT_SECTOR_SIZE*count;ptr++,pp++){
//                    if(~*ptr & *pp){ // у нас не должно быть нулей там где нужна 1
                // filesystem is inverted, so - у нас не должно быть 1 там где нужен 0
                // пример: чистая FF - инверсия 00, можно писАть любой байт
                //         считали F0 - инверсия 0F - можно писАть *0
                    if(*ptr & *pp){ 
                        need_erase=true;
                        break;
                    }
                }
                free(sbuffer);// don't need more
            }
            
            if(need_erase){// write do dirty block

                uint8_t *cluster = malloc(erase_size);
                if(!cluster) { // we can try to allocate up to 64K so absense of memory should not be error!
                    if(erase_size <= MAX_ERASE_SIZE) return RES_ERROR; // we have no reserved page
                    
                    // we can use any free sector of flash as buffer, too                    
                    uint32_t fr_sec = sd_max_sectors * (FAT_SECTOR_SIZE/DF_PAGE_SIZE); // the last page

                    // read data from beginning of erase block up to part that will be rewritten
                    uint32_t r_sector = (sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE)) & ~(erase_size/DF_PAGE_SIZE - 1);

                    // move data to sectors of free block
                    if(!sd_move_block(r_sector, fr_sec, NULL, 0, 0)) return RES_ERROR; 
                    // move back with inserting of data to write
                    if(!sd_move_block(fr_sec, r_sector, buff, sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE), count * (FAT_SECTOR_SIZE/DF_PAGE_SIZE))) return RES_ERROR; // move back
                    count=0; // all OK

                    
                } else { // we have enough memory for cluster buffer
            
                    ptr = cluster;

                    // read data from beginning of erase block up to part that will be rewritten
                    uint32_t r_sector = (sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE)) & ~(erase_size/DF_PAGE_SIZE - 1);
                    uint32_t w_sector = r_sector;
                    uint16_t r_count = erase_size/DF_PAGE_SIZE;      // read full page
        	    do {
	                if(r_sector >= sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE)) break;
	                if(!read_page(ptr, r_sector)) break;
	                ptr   += DF_PAGE_SIZE;
	                r_sector += 1;
                    } while(--r_count);

                    // Yes I know that we could to go out from the buffer end - but this used only for FAT_FS which writes FAT per one sector
//                  memcpy(cluster+FAT_SECTOR_SIZE*pos, (uint8_t *)buff, FAT_SECTOR_SIZE*count); // insert sectors to write to right place
                    memcpy(ptr, (uint8_t *)buff, FAT_SECTOR_SIZE*count); // insert sectors to write to right place

                    // skip part that will be rewritten
                    r_count  -= (FAT_SECTOR_SIZE/DF_PAGE_SIZE) * count;
                    r_sector += (FAT_SECTOR_SIZE/DF_PAGE_SIZE) * count;
                    ptr      +=  FAT_SECTOR_SIZE * count;

                    // read the tail
        	    while(r_count) {
	                if(!read_page(ptr, r_sector)) break;
	                ptr   += DF_PAGE_SIZE;
	                r_sector += 1;
	                --r_count;
                    };
                    
                    if(r_count) return RES_ERROR;    
        
                    // erase page
                    if(!erase_page(w_sector)) return RES_ERROR;
            
                    ptr = cluster;
                    uint16_t w_count = erase_size/DF_PAGE_SIZE;
    	            do {
    	                if(!write_page(ptr, w_sector)) break;
	                ptr   += DF_PAGE_SIZE;
    	                w_sector += 1;
        	    } while(--w_count);
            
                    count = w_count;
                    
                    free(cluster);
                }
                
            } else { // block is ready to write - just write needed part
                count  *= FAT_SECTOR_SIZE/DF_PAGE_SIZE; // 256bytes page from 512byte sector
                sector *= FAT_SECTOR_SIZE/DF_PAGE_SIZE;
    	        do {
        	    if(!write_page(buff, sector)) break;
    	            buff   += DF_PAGE_SIZE;
    	            sector += 1;
    	        } while(--count);                
            }
        }


	return count ? RES_ERROR : RES_OK;	/* Return result */
}


/*-----------------------------------------------------------------------*/
/* Miscellaneous drive controls other than data read/write               */
/*-----------------------------------------------------------------------*/

static inline void enqueue_flash_erase(uint32_t from, uint32_t to){

    int16_t new_wp = fe_write_ptr+1;
    if(new_wp >= FLASH_ERASE_QUEUE_SIZE) { // move write pointer
        new_wp=0;                         // ring
    }
    while(new_wp == fe_read_ptr) hal_yield(0); // buffer overflow

    flash_erase_queue[fe_write_ptr].b = from;
    flash_erase_queue[fe_write_ptr].e = to;

    fe_write_ptr=new_wp; // move forward

    hal_set_task_active(_flash_erase_task);
}


DRESULT sd_ioctl (
	uint8_t ctl,		/* Control command code */
	void *buff		/* Pointer to the conrtol data */
)
{
    DRESULT res;

    uint8_t *ptr = (uint8_t *)buff;

    res = RES_ERROR;

    if (ctl== CTRL_POWER){

        switch (*ptr) {
        case 0: // Sub control code == 0 (POWER_OFF) 
             res = RES_OK;
             break;
         case 1: // Sub control code == 1 (POWER_ON) 
             res = RES_OK;
             break;
         case 2: // Sub control code == 2 (POWER_GET) 
             *(ptr + 1) = (uint8_t) MMC_CD;
             break;
         default:
             res = RES_PARERR;
             break;
        }

    } else {
    
        if (Stat & STA_NOINIT) return RES_NOTRDY;	/* Check if drive is ready */

	switch (ctl) {
	case CTRL_SYNC :		/* Wait for end of internal write process of the drive */
	    res = RES_OK;
	    break;

	case GET_SECTOR_COUNT :	/* Get drive capacity in unit of sector (uint32_t ) */    
            *(uint32_t *)buff = sd_max_sectors;
	    res = RES_OK;	        
	    break;

	case GET_BLOCK_SIZE :	/* Get erase block size in unit of FAT sector (uint32_t ) */
	    *(uint32_t *)buff = erase_size/FAT_SECTOR_SIZE;
	    res = RES_OK;	        
	    break;

        case GET_SECTOR_SIZE:
	    *(uint32_t *)buff = FAT_SECTOR_SIZE; // always
	    res = RES_OK;	        
	    break;

	case CTRL_TRIM : {	                                /* Erase a block of sectors (used when _USE_TRIM == 1) */
	    uint32_t  start_sector = ((uint32_t  *)buff)[0];
	    uint32_t  end_sector   = ((uint32_t  *)buff)[1];
	    uint32_t  block, last_block=-1;
	    
	    if(start_sector>=sd_max_sectors || end_sector>=sd_max_sectors) return RES_PARERR;
            if(chip_is_clear) { // just after full erase
    	        res = RES_OK;	    
    	        chip_is_clear=false;
                return res;
            }

            if(hal_is_armed()) return RES_OK;

#if 0 // in separate thread

            erase_page(start_sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE));    // clear 1st sector of DataFlash

            enqueue_flash_erase(start_sector+(erase_size/DF_PAGE_SIZE), end_sector); //and enqueue
#else
            uint32_t  sector;
	    for(sector=start_sector; sector <= end_sector;sector++){
                uint32_t  df_sect = sector * (FAT_SECTOR_SIZE/DF_PAGE_SIZE);    // sector in DataFlash
                block = df_sect / (erase_size/DF_PAGE_SIZE);    // number of EraseBlock
                if(last_block!=block){
                    last_block=block;
                    if(!erase_page(df_sect)) return RES_ERROR;                    
                    putch('.');
                    extern void digitalToggle(uint8_t pin);
                    
                    digitalToggle(HAL_GPIO_A_LED_PIN);
                }
            }
#endif
	    res = RES_OK;
	    
            } break;

        case CTRL_FORMAT:{
                ChipErase();
                res = RES_OK;	    
            } break;

	default:
		res = RES_PARERR;
	}

    }
    return res;
}


/*-----------------------------------------------------------------------*/
/* Device timer function                                                 */
/*-----------------------------------------------------------------------*/
/* This function must be called from timer interrupt routine in period
/  of 1 ms to generate card control timing.
*/

void sd_timerproc (void)
{
    uint16_t n;

    n = Timer1;						/* 1kHz decrement timer stopped at 0 */
    if (n) Timer1 = --n;
    n = Timer2;
    if (n) Timer2 = --n;
}



#endif //defined(BOARD_DATAFLASH_FATFS)