/* ---------------------------------------------------------------------------
 * mmc.c                                                (c) 2008 Micro-key bv
 * ---------------------------------------------------------------------------
 * Micro-key bv
 * Industrieweg 28, 9804 TG Noordhorn
 * Postbus 92, 9800 AB Zuidhorn
 * The Netherlands
 * Tel: +31 594 503020
 * Fax: +31 594 505825
 * Email: support@microkey.nl  
 * Web: www.microkey.nl
 * ---------------------------------------------------------------------------
 * Description: 
 * ---------------------------------------------------------------------------
 * Version(s):  0.1, Apr 22, 2008, MMi
 *                   Creation.
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * System include files
 * ---------------------------------------------------------------------------
 */
/* Compiler Includes */
#include <string.h>
#include <assert.h>

#include "LPC23xx.h"
#include "types.h"
/* ---------------------------------------------------------------------------
 * Application include files
 * ---------------------------------------------------------------------------
 */
#include "mmc.h"
#include "mmc_transfer.h"
#include "dio.h"
/* ---------------------------------------------------------------------------
 * Local constant and macro definitions
 * ---------------------------------------------------------------------------
 */
#define MMC_GLOBAL

#define __no_init

#define CSD_GET_TRAN_SPEED_EXP()      (MmcSdCsd[ 0]&0x07)
#define CSD_GET_TRAN_SPEED_MANT()    ((MmcSdCsd[ 0]&0xF8)>>3 )
#define CSD_GET_NSAC()                (MmcSdCsd[ 1]          )
#define CSD_GET_TAAC_EXP()            (MmcSdCsd[ 2]&0x7)
#define CSD_GET_TAAC_MANT()          ((MmcSdCsd[ 2]&0xF8)>>3 )
#define CSD_GET_R2W_FACTOR()         ((MmcSdCsd[15]&0x1C)>>2 )
#define CSD_GET_READ_BL_LEN()         (MmcSdCsd[ 6]&0x0F)
#define CSD_GET_C_SIZE()            (((MmcSdCsd[ 5]&0x03)<<10) + (MmcSdCsd[4]<<2) + ((MmcSdCsd[11]&0xc0)>>6))
#define CSD_GET_C_SIZE_MULT()       (((MmcSdCsd[10]&0x03)<<1 ) +((MmcSdCsd[9]&0x80)>>7))
#define CSD_GET_PERM_WRITE_PROTECT() ((MmcSdCsd[13]&0x20)>>5 )
#define CSD_GET_TMP_WRITE_PROTECT()  ((MmcSdCsd[13]&0x10)>>4 )

volatile UINT32 i;


#define SpcInquiryRemovableMedium        0x80
#define SpcInquiryStandartVersion        5
#define SizeOfInquiryDescMmcDsk          36

const UINT32 MmcTransfExp[] =
    { 10000UL, 100000UL, 1000000UL, 10000000UL, 0UL, 0UL, 0UL, 0UL, 
};

const UINT32 MmmcAccessTime [] =
    { 1UL, 10UL, 100UL, 1000UL, 10000UL, 100000UL, 1000000UL, 10000000UL, 
};

const UINT32 MmcCsdMant[] =
    { 0UL, 10UL, 12UL, 13UL, 15UL, 20UL, 25UL, 30UL, 35UL, 40UL, 45UL, 50UL,
            55UL, 60UL, 70UL, 80UL, 
};

const MmcCommads_t MmcCmd[CMD_END] =
    {                                   /* {TxData, Arg, Resp}              */
        { 0x00, MmcNoArg, MmcNoResp },  /* CMD0                             */
        { 0x01, MmcOcr, MmcR3 },        /* CMD1                             */
        { 0x02, MmcNoArg, MmcR2 },      /* CMD2                             */
        { 0x03, MmcRelAddr, MmcR1 },    /* CMD3                             */
        { 0x07, MmcRelAddr, MmcR1 },    /* CMD7                             */
        { 0x09, MmcRelAddr, MmcR2 },    /* CMD9                             */
        { 0x0A, MmcRelAddr, MmcR2 },    /* CMD10                            */
        { 0x0C, MmcNoArg, MmcR1b },     /* CMD12                            */
        { 0x0D, MmcRelAddr, MmcR1 },    /* CMD13                            */
        { 0x10, MmcBlockLen, MmcR1 },   /* CMD16                            */
        { 0x11, MmcDataAdd, MmcR1 },    /* CMD17                            */
        { 0x12, MmcDataAdd, MmcR1 },    /* CMD18                            */
        { 0x18, MmcDataAdd, MmcR1 },    /* CMD24                            */
        { 0x19, MmcDataAdd, MmcR1 },    /* CMD25                            */
        { 0x1B, MmcOcr, MmcR1 },        /* CMD27                            */
        { 0x1C, MmcDataAdd, MmcR1b },   /* CMD28                            */
        { 0x1D, MmcDataAdd, MmcR1b },   /* CMD29                            */
        { 0x1E, MmcDataAdd, MmcR1 },    /* CMD30                            */
        { 0x20, MmcDataAdd, MmcR1 },    /* CMD32                            */
        { 0x21, MmcDataAdd, MmcR1 },    /* CMD33                            */
        { 0x22, MmcDataAdd, MmcR1 },    /* CMD34                            */
        { 0x23, MmcDataAdd, MmcR1 },    /* CMD35                            */
        { 0x24, MmcDataAdd, MmcR1 },    /* CMD36                            */
        { 0x25, MmcDataAdd, MmcR1 },    /* CMD37                            */
        { 0x26, MmcNoArg, MmcR1b },     /* CMD38                            */
        { 0x2A, MmcNoArg, MmcR1b },     /* CMD42                            */
        { 0x37, MmcRelAddr, MmcR1 },    /* CMD55                            */
        { 0x38, MmcNoArg, MmcR1 },      /* CMD56                            */
        { 0x06, MmcDataAdd, MmcR1 },    /* ACMD46                           */
        { 0x29, MmcDataAdd, MmcR3 },    /* ACMD41                           */
};

const UINT8 MmcDskInquiry[]__attribute__((aligned (4)))=
    {
    SbcDirectAccess,                    /* 0 PERIPHERAL QUALIFIER AND DEVICE*/
    SpcInquiryRemovableMedium,          /* 1 RMB                            */
    SpcInquiryStandartVersion,          /* 2 VERSION                        */
    0x02,                               /* 3 NORMACA HISUP RESPONSE FORMAT  */
    36-4,                               /* 4 ADDITIONAL LENGTH (n-4)        */
    0x00,                               /* 5 SCCS                           */
    /* for parallel SCSI only                                               */
    0x00,       /* 6 BQUE ENCSERV VS MULTIP MCHNGR Obsolete Obsolete ADDR16 */      
    0x00,       /* 7 RELADR Obsolete WBUS16† SYNC† LINKED Obsolete CMDQUE VS*/
    'I','A','R',' ','S','y','s','.',    /* 8 - 15 VENDOR IDENTIFICATION     */
    /* 16 - 31 PRODUCT IDENTIFICATION                                       */
    'L','P','C','2','3','7','8',' ','S','t','o','r','a','g','e',' ',
    '1','.','0','0',                    /* 32 - 35 PRODUCT REVISION LEVEL   */
    };

/* ---------------------------------------------------------------------------
 * Global variable definitions
 * ---------------------------------------------------------------------------
 */
static UINT32 CardRCA;
UINT32 MmcLastError;
UINT32 dlycnt;
UINT32 Tnac;
UINT32 Twr;

UINT8 MmcSdCsd[16]__attribute__ ((aligned (4)));
static BOOLEAN bMmcPermWriteProtect;
BOOLEAN bMmcChanged;

DiskCtrlBlk_t MmcDskCtrlBlk;
/* ---------------------------------------------------------------------------
 * Local variable definitions
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * Local function definitions
 * ---------------------------------------------------------------------------
 */

/* Function returns TRUE if Card is inserted, FALSE if Card is removed      */
inline BOOLEAN MmcPresent(void)
{
    return (!(MMC_CP_FIO & MMC_CP_MASK));
}


/* Function returns TRUE is WriteProtection is activated, FALSE if
 * Protection is off. If no Card is inserted, Function will also return
 * TRUE. This results from the physical Switch Connection (See Datasheet).
 * Usage of this Function only makes Sense with previous TRUE-Return 
 * from MmcPresent.
 */ 
inline BOOLEAN MmcWriteProtect(void)
{
    if (!(MMC_WP_FIO & MMC_WP_MASK))
        {
        return (FALSE);
        }
    else
        {
        return (TRUE);
        }
}

UINT32 MmcSetClockFreq (UINT32 Frequency)
    {
    UINT32 Pclk = SYS_GetFPclk(MCI_PCLK_OFFSET);
    UINT32 Div;
    Frequency <<= 1;

    for(Div = 1; Div <= 256; ++Div)
        {
        if((Frequency * Div)> Pclk)
            {
            break;
            }
        }
    MCI_CLOCK = ((Div - 1) & 0xFF);     /* Set Clock Devider (Bit 0-7)      */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    MCI_CLOCK |= (1 << 8);              /* enable Clock (Bit 8)             */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    MCI_CLOCK |= (1 << 9);              /* Set PowerSave (Bit 9)            */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);

    return(Pclk/(Div<<1));              /* Return real frequency            */
    }

void MmcInit(void)
{
    UINT32 i;

    MMC_CP_DIR  &= ~MMC_CP_MASK;        /* Set CP to Input                  */
    MMC_CP_MODE &=~(1 << 24);           /* Neither PullUp nor PullDown      */
    MMC_CP_MODE |= (1 << 25);
    
    MMC_WP_DIR  &= ~MMC_WP_MASK;        /* Set WP to Input                  */
    MMC_WP_MODE &=~(1 << 22);           /* Neither PullUp nor PullDown      */
    MMC_WP_MODE |= (1 << 23);
    

    PCLKSEL1 |= (1 << 24);              /* set divider to /1 (Bit 24&25)    */
    PCLKSEL1 &=~(1 << 25);

    PCONP |= 0x30000000;                /* Set Power up                     */

    PINSEL1 &=~(1 << 6);                /* Assign Pin 0.19 (Bit 6&7)        */
    PINSEL1 |= (1 << 7);
    PINSEL1 &=~(1 << 8);                /* Assign Pin 0.20 (Bit 8&9)        */
    PINSEL1 |= (1 << 9);
    PINSEL1 &=~(1 << 10);               /* Assign Pin 0.21 (Bit 10&11)      */
    PINSEL1 |= (1 << 11);
    PINSEL1 &=~(1 << 12);               /* Assign Pin 0.22 (Bit 12&13)      */
    PINSEL1 |= (1 << 13);
    PINSEL4 &=~(1 << 22);               /* Assign Pin 2.11 (Bit 22&23)      */
    PINSEL4 |= (1 << 23);
    PINSEL4 &=~(1 << 24);               /* Assign Pin 2.12 (Bit 24&25)      */
    PINSEL4 |= (1 << 25);
    PINSEL4 &=~(1 << 26);               /* Assign Pin 2.13 (Bit 26&27)      */
    PINSEL4 |= (1 << 27);
    

    PINMODE1 &=~(1 << 6) & ~(1 << 7);   /* Set PullUp Pin 0.19 (Bit 6&7)    */
    PINMODE1 &=~(1 << 8) & ~(1 << 9);   /* Set PullUp Pin 0.20 (Bit 8&9)    */
    PINMODE1 &=~(1 << 10) & ~(1 << 11); /* Set PullUp Pin 0.21 (Bit 10&11)  */
    PINMODE1 &=~(1 << 12) & ~(1 << 13); /* Set PullUp Pin 0.22 (Bit 12&13)  */

   
    MCI_COMMAND = 0;                    /* Clear MCI Command Register       */
    MCI_DATA_CTRL = 0;                  /* Clear MCI Data Control Register  */
    MCI_CLEAR = 0x7FF;                  /* Clear all pending interrupts.    */
    
    /* Power up, switch on VCC for the Flash Card - Wait shortly            */
    MCI_POWER = 0x02;
    for (i = 0; i < 50000; i++);

   
    MCI_POWER |= 0x01;                  /* Power on the Flash Card.         */
    for (i = 0; i < 50000; i++);        /* Wait shortly                     */
        
    MmcPowerDown();
    
    GPDMA_SYNC = 0;                     /* DMA sync enable                  */
    GPDMA_INT_ERR_CLR = 3;              /* Clear DMA Error Interrupts       */
    GPDMA_INT_TCCLR = 3;                /* Clear DMA Interrupts             */
   
}

void MmcPowerDown(void)
{
    SCS |= (1 << 3);                    /* Init Power State (Bit 3)         */
    MCI_POWER = 0;
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    MCI_MASK0 = 0;                      /* Disable all interrupts for now   */
    MCI_MASK1 = 0;
    MCI_COMMAND = 0;
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    MCI_DATA_CTRL = 0;
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    MCI_CLEAR = 0x7FF;
    
    MCI_CLOCK &=~(1 << 11);             /* clear all pending interrupts     */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    
    MmcSetClockFreq(IdentificationModeClock); /* ClkFreq Ident Mode< 400kHz */
}


MmcState_t MmcSendCmd(MmcSpiCmdInd_t ComdInd, pUINT32 pArg)
{
    UINT32 Status;
    union
        {
            UINT32 Data;
            struct
                {
                    UINT32 CMDINDEX : 6;
                    UINT32 RESPONSE : 1;
                    UINT32 LONGRSP : 1;
                    UINT32 INTERRUPT : 1;
                    UINT32 PENDING : 1;
                    UINT32 ENABLE : 1;
                    UINT32 :21;
                };
        } Command;
    
    /* The Command engine has to be Disabled when modifying the Argument
     * of the Peripheral
     */
    while ((MCI_STATUS << 11) == TRUE)  /* While a Command is in Progress   */
        {
        MCI_COMMAND = 0;                /* Clear MCI Command Register       */
        for(dlycnt = 10; dlycnt > 0;dlycnt--);
        }
    Command.Data = 0;
    
    /* Copy Command-specific Index-Argument from MmcCmd LookUp Table to
     * Command struct
     */ 
    Command.CMDINDEX = MmcCmd[ComdInd].TxData;  
    if (pArg != NULL)                   /* Set Command Response depending on*/
        {                               /* asked Argument                   */
        switch (MmcCmd[ComdInd].Resp)
            {
            case MmcR2:
                Command.LONGRSP = 1;
            case MmcR1:
            case MmcR1b:
            case MmcR3:
                Command.RESPONSE = 1;
            case MmcNoResp:;
            }
        }
    Command.ENABLE = 1;
    
    if (MmcCmd[ComdInd].Arg != MmcNoArg) /* If Argument exists              */
        {
        MCI_ARGUMENT = *pArg;           /* Copy Argument to MCI Argument Reg*/
        } 
    else
        {
        MCI_ARGUMENT = 0;
        }
    
    MCI_COMMAND = Command.Data;         /* Send Command to Card             */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    while ((Status = MCI_STATUS & 0x000000C5) == 0);/* Wait command respond */
        
    MCI_CLEAR = Status;                 /* Clear actual Status              */

    if (Status & (1UL << 2))            /* If Command TimeOut occourses     */
        {
        MCI_COMMAND = 0;                /* reset Command Register           */
        for(dlycnt = 10; dlycnt > 0;dlycnt--);
        return (MmcNoResponse);         /* Return with Error Message        */
        }
    if (Status & (1UL << 0))            /* Command CRC Error                */
        {
        switch (MCI_COMMAND & 0x3F)     /* Ignore CRC Error for following   */
            {                           /* Commands                         */
            case 1:                     /* CMD1                             */
            case 41:                    /* ACMD42                           */
            case 12:                    /* CMD12                            */
                MCI_COMMAND = 0;
                for(dlycnt = 10; dlycnt > 0;dlycnt--);
                MCI_ARGUMENT = 0xFFFFFFFF;
                break;
            default:                    /* No CRC Error Ignoring            */
                MCI_COMMAND = 0;
                for(dlycnt = 10; dlycnt > 0;dlycnt--);
                return (MmcCardError);
            }
        }
    
    if (pArg != NULL)
        {
        switch (MmcCmd[ComdInd].Resp)   /* Handle Command Response          */
            {
            case MmcNoResp:
                break;
            case MmcR3:
                *pArg = MCI_RESP0;
                break;
            case MmcR2:
                *pArg++ = MCI_RESP0;
                *pArg++ = MCI_RESP1;
                *pArg++ = MCI_RESP2;
                *pArg++ = MCI_RESP3;
                break;
            default:
                if (MmcCmd[ComdInd].TxData != (MCI_RESP_CMD & 0x3F)) /* Bit 0-5*/
                    {
                    return (MmcCardError);
                    }
                *pArg = MCI_RESP0;
            }
        }
    MCI_COMMAND = 0;
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    return (MmcOk);
}

UINT32 Status;
MmcState_t MmcInitMedia(void)
{
    UINT32 i;
    UINT32 res;
    UINT32 loopcnt;
    volatile UINT32 Dly;
    UINT8 MmcSdCid[16];
    BOOLEAN CardDetected = FALSE;
    
    Tnac = 1;
    if (MmcPresent() == FALSE)          /* If no Card inserted              */
        {
        if (MCI_POWER & 0x3)            /* Is Power up?                     */
            {
            MmcPowerDown();             /* Set to power down state          */
            }
        return (MmcNoPresent);          /* Return with Error Message        */
        }
    
    MmcPowerDown();                     /* Power Down                       */
    MmcDly_1ms(100);
    
    MCI_POWER &=~(1 << 0);              /* power up (Bit 1)                 */
    MCI_POWER |= (1 << 1);
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    while ((MCI_POWER & 0x3) != 0x2);
    
    MCI_POWER |= (1 << 0);              /* power reserved (Bit 0)           */
    MCI_POWER &=~(1 << 1);
    MmcDly_1ms(100);
    MCI_POWER &=~(1 << 6);              /* Disable OpenDrain Mode           */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);

    MCI_POWER = 0x02;                   /* Power up (Bit 1)                 */
    for (loopcnt = 0; loopcnt < 50000; loopcnt++);

    MCI_POWER |= 0x01;                  /* Power on the Flash Card          */
    for (loopcnt = 0; loopcnt < 50000; loopcnt++);
    
    if (MmcSendCmd(CMD0, NULL) != MmcOk) /* Send Cmd0, Reset Card           */
        {
        return (MmcNoResponse);
        }

    MCI_POWER |= 0x40;                  /* Set OpenDrain output control     */
    
    MmcDskCtrlBlk.DiskType = DiskMMC;   /* Determinate Card type SD or MMC  */
    for (i=100; i; --i)                 /* Try detecting several Times      */
        {
        MCI_POWER &=~(1 << 6);          /* Disable OpenDrain Mode           */
        for(dlycnt = 10; dlycnt > 0;dlycnt--);
        res = 0;
        if (((Status = MmcSendCmd(CMD55, &res)) == MmcOk) && (res & 0x100))
            {                           /* Send Notification for ACMD41     */
            res = OcrReg;
            if ((MmcSendCmd(ACMD41, &res) == MmcOk) && (res & 0x80000000))
                                        /* Activates Init Process (only SD) */
                {                       /* Gets here when SD Card detected  */
                CardDetected = TRUE;
                MmcDskCtrlBlk.DiskType = DiskSD;
                break;
                }
            } 
        else                            /* If Cmd55 fails                   */
            {
            MCI_POWER |= (1 << 6);      /* enable OpenDrain Mode            */
            for(dlycnt = 10; dlycnt > 0;dlycnt--);
            res = OcrReg;
            if (MmcSendCmd(CMD1, &res) == MmcOk && (res & 0x80000000))
                                        /* CMD1 for MMC Init sequence       */
                {                       /* Gets here when MMC Card detected */
                CardDetected = TRUE;
                break;
                }
            }
        MmcDly_1ms(50);
        }                               /* end Card Detection               */

    if (CardDetected == FALSE)          /* If no Card recognized            */
        {
        return (MmcNoResponse);         /* Return with Error Message        */
        }
    if (MmcSendCmd(CMD2, (pUINT32)MmcSdCid) != MmcOk) /* Read CID from Card */
        {
        return (MmcNoResponse);         /* Return with Error Message        */
        }
    
    /* Set address                                                          */
    CardRCA = (MmcDskCtrlBlk.DiskType == DiskMMC) ? 0x00010000 : 0x00000000;
    if (MmcSendCmd(CMD3, &CardRCA) != MmcOk)
        {
        return (MmcNoResponse);
        }
    if (MmcDskCtrlBlk.DiskType == DiskSD)
        {
        CardRCA &= 0xFFFF0000;
        } 
    else
        {
        CardRCA = 0x00010000;
        }

    MCI_POWER &=~(1 << 6);              /* Disable OpenDrain Mode           */
    for(dlycnt = 10; dlycnt > 0;dlycnt--);
    

    /* Read Card specific Data from Card                                    */                 
    MmcSdCsd[0] = 0;
    MmcSdCsd[1] = 0;
    MmcSdCsd[2] = CardRCA >> 16;
    MmcSdCsd[3] = CardRCA >> 24;
    if (MmcSendCmd(CMD9, (pUINT32)MmcSdCsd) != MmcOk)
        {
        return (MmcNoResponse);
        }
    
    MmcCsdImplemet();                   /* Implement CSD data               */
    
    res = CardRCA;
    if (MmcSendCmd(CMD7, &res) != MmcOk) /* Select Card                     */
        {
        return (MmcNoResponse);
        }
    res = CardRCA;
    if (MmcOk != MmcSendCmd(CMD13, &res)) /* Send Card Status Register      */
        {
        return (MmcNoResponse);
        } else if (!(res & READY_FOR_DATA) || ((res & CURRENT_STATE)
            != CARD_TRAN))
        {
        return (MmcCardError);
        }
    
    if (MmcDskCtrlBlk.DiskType == DiskSD)
        {
        MCI_CLOCK |= (1 << 11);         /* Use wide bus for SD              */
        for (loopcnt = 0; loopcnt < 100; loopcnt++);

        res = CardRCA;
        if (((Status = MmcSendCmd(CMD55, &res)) != MmcOk) || !(res & 0x100))
                                        /* Send Notification for ACMD6      */
            {
            return (MmcCardError);
            }

        res = 2;
        if (MmcSendCmd(ACMD6, &res) != MmcOk) /* Set bus width 4bits        */
            {
            return (MmcCardError);
            }
        }
      
    res = MmcDskCtrlBlk.BlockSize;
    if (MmcSendCmd(CMD16, &res))        /* Set Block size                   */
        {
        return (MmcNoResponse);
        }
    
    return (MmcOk);
}

UINT32 Tets;
void MmcCsdImplemet(void)
{
    UINT32 Freq;
    UINT64 Tmp;
    // Calculate SPI max clock
    Freq = MmcTransfExp[CSD_GET_TRAN_SPEED_EXP()] * MmcCsdMant[CSD_GET_TRAN_SPEED_MANT()];
    Freq = MmcSetClockFreq(Freq);
    if (MmcDskCtrlBlk.DiskType == DiskMMC)
        {
        // Calculate Time outs for MMC cards
        Tmp = MmmcAccessTime[CSD_GET_TAAC_EXP()] * MmcCsdMant[CSD_GET_TAAC_MANT()];
        Tmp /= 10000; // us
        // Freq [Hz], Tmp[1 us], *10
        Tmp = (Freq*Tmp)/100000LL;
        // NSAC*100*10
        Tmp += 1000*CSD_GET_NSAC();
        // Max time out
        Tnac = Tmp;
        Twr = Tmp * (1<<CSD_GET_R2W_FACTOR());
        }
    else
        {
        // Calculate Time outs for SD cards
        // Freq [Hz], RD_TIME_OUT[ms]
        Tmp = Freq/1000;
        Tnac = Tmp * RD_TIME_OUT;
        Twr = Tmp * WR_TIME_OUT;
        }
    // Calculate Block size and Block Number
    MmcDskCtrlBlk.BlockSize = 1<<CSD_GET_READ_BL_LEN();
    MmcDskCtrlBlk.BlockNumb = (CSD_GET_C_SIZE()+1)*(4<<CSD_GET_C_SIZE_MULT());
    // Set Write Protect
    bMmcPermWriteProtect = CSD_GET_PERM_WRITE_PROTECT() |\
 CSD_GET_TMP_WRITE_PROTECT();
    MmcDskCtrlBlk.WriteProtect = MmcWriteProtect() |\
 bMmcPermWriteProtect;
    }


inline MmcState_t MmcVerify(const UINT8 * pData, UINT32 Add, UINT32 Length)
{

__no_init static UINT8 TempBuffer[2048]__attribute__ ((section (".dmaram")));
                                        /* maximum block length is 2048     */
    MmcState_t Status = MmcOk;
    UINT32 i = 0;
    
    while (Length)
        {
        Status = MmcReadBlock(TempBuffer, Add); 
        if (Status != MmcOk)
            {
            break;
            }
        for (i = 0; i < MmcDskCtrlBlk.BlockSize; ++i)
            {
            if (TempBuffer[i] != *pData++)
                {
                return (MmcMiscompare);
                }
            }
        Length -= MmcDskCtrlBlk.BlockSize;
        Add += MmcDskCtrlBlk.BlockSize;
        }
    return (Status);
}


UINT32 MmcGetLastError(void)
{
    return(MmcLastError);
}

void MmcStatusUpdate(void)
{
    UINT32 Data = CardRCA;
    // Update WP state
    MmcDskCtrlBlk.WriteProtect = MmcWriteProtect() |\
                                 bMmcPermWriteProtect;
    if (!MmcPresent())
        {
        MmcDskCtrlBlk.DiskStatus = DiskNotPresent;
        MmcPowerDown();
        return;
        }
    
    if ((MmcDskCtrlBlk.DiskStatus != DiskCommandPass))
        {
        switch (MmcInitMedia())
            {
            case MmcOk:
                MmcDskCtrlBlk.DiskStatus = DiskCommandPass;
                MmcDskCtrlBlk.MediaChanged = TRUE;
                break;
            case MmcCardError:
                MmcDskCtrlBlk.DiskStatus = DiskNotReady;
                break;
            default:
                MmcDskCtrlBlk.DiskStatus = DiskNotPresent;
                break;
            }
        } 
    else if (MmcSendCmd(CMD13, &Data) != MmcOk)
        {
        MmcDskCtrlBlk.DiskStatus = DiskNotReady;
        } 
    else if (!(Data & READY_FOR_DATA))
        {
        MmcDskCtrlBlk.DiskStatus = DiskNotReady;
        } 
    else if ((Data & CURRENT_STATE) == CARD_TRAN)
        {
        MmcDskCtrlBlk.DiskStatus = DiskCommandPass;
        } 
    else
        {
        MmcDskCtrlBlk.DiskStatus = DiskNotReady;
        }
}

void MmcDiskInit(void)
{
    MmcDskCtrlBlk.BlockNumb =0;
    MmcDskCtrlBlk.BlockSize =0;
    MmcLastError = 0;
    // Init SPI
    MmcInit();
    // Media Init
    switch (MmcInitMedia())
        {
        case MmcOk:
            MmcCsdImplemet();
            MmcDskCtrlBlk.DiskStatus = DiskCommandPass;
            MmcDskCtrlBlk.MediaChanged = TRUE;
            break;
        case MmcCardError:
            MmcDskCtrlBlk.DiskStatus = DiskNotReady;
            break;
        default:
            MmcDskCtrlBlk.DiskStatus = DiskNotPresent;
            break;
        }
}

#ifdef MMC_DISK_INFO
UINT32 MmcDiskInfo (pUINT8 pData, DiskInfoType_t DiskInfoType)
    {
    pMmc3FormatCapResponse_t pFormatCapacity;
    switch (DiskInfoType)
        {
        case DiskInquiry:
        memcpy(pData,MmcDskInquiry,SizeOfInquiryDescMmcDsk);
        return(SizeOfInquiryDescMmcDsk);
        case DiskFormatCapacity:
        pFormatCapacity = (pMmc3FormatCapResponse_t)pData;
        memset(pFormatCapacity,0,sizeof(Mmc3FormatCapResponse_t));
        pFormatCapacity->CapacityListLength = sizeof(Mmc3FormatCapDescriptor_t);
        if (MmcDskCtrlBlk.DiskStatus != DiskCommandPass)
            {
            pFormatCapacity->MaximumDescriptor.DescriptorType = FormattedMedia;
            pFormatCapacity->MaximumDescriptor.BlockLength[0] = (MmcDskCtrlBlk.BlockSize >> 16) & 0xFF;
            pFormatCapacity->MaximumDescriptor.BlockLength[1] = (MmcDskCtrlBlk.BlockSize >> 8) & 0xFF;
            pFormatCapacity->MaximumDescriptor.BlockLength[2] = (MmcDskCtrlBlk.BlockSize ) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[0] = (MmcDskCtrlBlk.BlockNumb >> 24) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[1] = (MmcDskCtrlBlk.BlockNumb >> 16) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[2] = (MmcDskCtrlBlk.BlockNumb >> 8) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[3] = (MmcDskCtrlBlk.BlockNumb ) & 0xFF;
            }
        else
            {
            pFormatCapacity->MaximumDescriptor.DescriptorType = NoMediaPresent;
            pFormatCapacity->MaximumDescriptor.BlockLength[0] = (2048 >> 16) & 0xFF;
            pFormatCapacity->MaximumDescriptor.BlockLength[1] = (2048 >> 8) & 0xFF;
            pFormatCapacity->MaximumDescriptor.BlockLength[2] = (2048 ) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[0] = (0xFFFFFFFF >> 24) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[1] = (0xFFFFFFFF >> 16) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[2] = (0xFFFFFFFF >> 8) & 0xFF;
            pFormatCapacity->MaximumDescriptor.NumberofBlocks[3] = (0xFFFFFFFF ) & 0xFF;
            }
        return(sizeof(Mmc3FormatCapResponse_t));
        }
    return(0);
    }
#endif 

pDiskCtrlBlk_t MmcGetDiskCtrlBkl(void)
{
    return (&MmcDskCtrlBlk);
}

UINT32 SYS_GetFSclk(void)
    {
    UINT32 Mul = 1, Div = 1, Osc, Fsclk;
    if (PLLSTAT & (1 << 25))
        {
        // when PLL is connected
        Mul = (PLLSTAT & 0x7FFF) +1;
        Div = ((PLLSTAT >> 16) & 0xFF) +1;
        }

    // Find clk source
    switch (CLKSRCSEL & 0x3)
        {
        case 0:
        Osc = I_RC_OSC_FREQ;
        break;
        case 1:
        Osc = MAIN_OSC_FREQ;
        break;
        case 2:
        Osc = RTC_OSC_FREQ;
        break;
        default:
        Osc = 0;
        }
    // Calculate system frequency
    Fsclk = Osc*Mul*2;
    Fsclk /= Div*(CCLKCFG+1);
    return (Fsclk);
    }

UINT32 SYS_GetFPclk(UINT32 Periphery)
    {
    UINT32 Fpclk;
    pUINT32 pReg = (pUINT32)((Periphery < 32) ? &PCLKSEL0 : &PCLKSEL1);

    Periphery &= 0x1F; // %32
    Fpclk = SYS_GetFSclk();
    // find peripheral appropriate periphery divider
    switch ((*pReg >> Periphery) & 3)
        {
        case 0:
        Fpclk /= 4;
        break;
        case 1:
        break;
        case 2:
        Fpclk /= 2;
        break;
        default:
        Fpclk /= 8;
        }
    return (Fpclk);
    }

void Dly100us(void *arg)
{
    volatile UINT32 Dly = (UINT32)arg, Dly100;
    for (; Dly; Dly--)
        for (Dly100 = 500; Dly100; Dly100--)
            ;
}