static int destroy_ipsec_tunnel(const str remote_addr, ipsec_t* s)
{
struct mnl_socket* sock = init_mnl_socket();
if (sock == NULL) {
return -1;
}
LM_DBG("Destroying security associations: Local IP: %.*s client port: %d server port: %d; UE IP: %.*s; client port %d server port %d\n",
ipsec_listen_addr.len, ipsec_listen_addr.s, ipsec_client_port, ipsec_server_port,
remote_addr.len, remote_addr.s, s->port_uc, s->port_us);
// P-CSCF 'client' tunnel to UE 'server'
remove_sa (sock, ipsec_listen_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us);
remove_policy(sock, ipsec_listen_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us, IPSEC_POLICY_DIRECTION_OUT);
// UE 'client' to P-CSCF 'server' tunnel
remove_sa (sock, remote_addr, ipsec_listen_addr, s->port_uc, ipsec_server_port, s->spi_ps);
remove_policy(sock, remote_addr, ipsec_listen_addr, s->port_uc, ipsec_server_port, s->spi_ps, IPSEC_POLICY_DIRECTION_IN);
// Release SPIs
release_spi(s->spi_uc);
release_spi(s->spi_us);
close_mnl_socket(sock);
return 0;
}
int main(void){
/* Sample data */
char *data = "REDPITAYA SPI TEST";
/* Init the spi resources */
if(init_spi() < 0){
printf("Initialization of SPI failed. Error: %s\n", strerror(errno));
return -1;
}
/* Write some sample data */
if(write_spi(data, strlen(data)) < 0){
printf("Write to SPI failed. Error: %s\n", strerror(errno));
return -1;
}
/* Read flash ID and some sample loopback data */
if(read_flash_id(spi_fd) < 0){
printf("Error reading from SPI bus : %s\n", strerror(errno));
return -1;
}
/* Release resources */
if(release_spi() < 0){
printf("Relase of SPI resources failed, Error: %s\n", strerror(errno));
return -1;
}
return 0;
}
DRESULT mmc_readsector_halp(BYTE *buff, DWORD lba, BYTE top)
{
DRESULT res;
BYTE rc;
WORD bc;
GREENLEDON();
if (!(CardType & CT_BLOCK)) lba *= 512; /* Convert to byte address if needed */
res = RES_ERROR;
if (send_cmd(CMD17, lba) == 0) { /* READ_SINGLE_BLOCK */
bc = 30000;
do { /* Wait for data packet in timeout of 100ms */
rc = XFER_SPI(0xff);
} while (rc == 0xFF && --bc);
if (rc == 0xFE)
{
if (top)
{
for (bc = 0; bc < 256; ++bc)
{
XFER_SPI(0xff);
}
}
/* A data packet arrived */
for (bc = 0; bc < 256; ++bc)
{
buff[bc] = XFER_SPI(0xff);
}
if (!top)
{
for (bc = 0; bc < 256; ++bc)
{
XFER_SPI(0xff);
}
}
/* Skip CRC */
XFER_SPI(0xff);
XFER_SPI(0xff);
res = RES_OK;
}
}
release_spi();
GREENLEDOFF();
return res;
}
static void power_off (void)
{
SELECT(); /* Wait for card ready */
wait_ready();
release_spi();
SSP2CON1bits.SSPEN = 0; /* Disable SPI1 */
return;
}
DSTATUS disk_initialize (void)
{
BYTE n, cmd, ty, buf[4];
UINT tmr;
// INIT_PORT();
mmc_spi_InitSocket();
// CS_H();
mmc_spi_SetCardDeselect();
skip_mmc(10); /* Dummy clocks */
ty = 0;
if (send_cmd(CMD0, 0) == 1) { /* Enter Idle state */
if (send_cmd(CMD8, 0x1AA) == 1) { /* SDv2 */
for (n = 0; n < 4; n++) buf[n] = rcvr_mmc(); /* Get trailing return value of R7 resp */
if (buf[2] == 0x01 && buf[3] == 0xAA) { /* The card can work at vdd range of 2.7-3.6V */
for (tmr = 1000; tmr; tmr--) { /* Wait for leaving idle state (ACMD41 with HCS bit) */
if (send_cmd(ACMD41, 1UL << 30) == 0) break;
// DLY_US(1000);
mmc_spi_SetTimer(1);
while( mmc_spi_CheckTimer() ) {}
}
if (tmr && send_cmd(CMD58, 0) == 0) { /* Check CCS bit in the OCR */
for (n = 0; n < 4; n++) buf[n] = rcvr_mmc();
ty = (buf[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; /* SDv2 (HC or SC) */
}
}
} else { /* SDv1 or MMCv3 */
if (send_cmd(ACMD41, 0) <= 1) {
ty = CT_SD1; cmd = ACMD41; /* SDv1 */
} else {
ty = CT_MMC; cmd = CMD1; /* MMCv3 */
}
for (tmr = 1000; tmr; tmr--) { /* Wait for leaving idle state */
if (send_cmd(ACMD41, 0) == 0) break;
// DLY_US(1000);
mmc_spi_SetTimer(1);
while( mmc_spi_CheckTimer() ) {}
}
if (!tmr || send_cmd(CMD16, 512) != 0) /* Set R/W block length to 512 */
ty = 0;
}
}
CardType = ty;
release_spi();
// return ty ? 0 : STA_NOINIT;
if ( !ty ) return STA_NOINIT;
mmc_spi_SetTransClock();
return 0;
}
DRESULT disk_readp (
BYTE *buff, // Pointer to the read buffer (NULL:Read bytes are forwarded to the stream)
DWORD lba, // Sector number (LBA)
WORD ofs, // Byte offset to read from (0..511)
WORD cnt // Number of bytes to read (ofs + cnt mus be <= 512)
)
{
DRESULT res;
BYTE rc;
WORD bc;
if ((PINB&_BV(SD_INS))!=0x00) return RES_ERROR;
if (!(CardType & CT_BLOCK)) lba *= 512; // Convert to byte address if needed
res = RES_ERROR;
if (send_cmd(CMD17, lba) == 0) { // READ_SINGLE_BLOCK
bc = 30000;
do { // Wait for data packet in timeout of 100ms
rc = rcv_spi();
} while (rc == 0xFF && --bc);
if (rc == 0xFE) { // A data packet arrived
bc = 514 - ofs - cnt;
// Skip leading bytes
if (ofs) {
do rcv_spi(); while (--ofs);
}
// Receive a part of the sector
if (buff) { // Store data to the memory
do
*buff++ = rcv_spi();
while (--cnt);
} else { // Forward data to the outgoing stream (depends on the project)
do
;//uart_transmit(rcv_spi()); // (Console output)
while (--cnt);
}
// Skip trailing bytes and CRC
do rcv_spi(); while (--bc);
res = RES_OK;
}
}
release_spi();
return res;
}
DRESULT disk_readp (
BYTE *buff, /* Pointer to the read buffer (NULL:Read bytes are forwarded to the stream) */
DWORD lba, /* Sector number (LBA) */
WORD ofs, /* Byte offset to read from (0..511) */
WORD cnt /* Number of bytes to read (ofs + cnt mus be <= 512) */
)
{
DRESULT res;
BYTE d;
WORD bc, tmr;
if (!(CardType & CT_BLOCK)) lba *= 512; /* Convert to byte address if needed */
res = RES_ERROR;
if (send_cmd(CMD17, lba) == 0) { /* READ_SINGLE_BLOCK */
tmr = 1000;
do { /* Wait for data packet in timeout of 100ms */
// DLY_US(100);
mmc_spi_Wait100us();
d = rcvr_mmc();
} while (d == 0xFF && --tmr);
if (d == 0xFE) { /* A data packet arrived */
bc = 514 - ofs - cnt;
/* Skip leading bytes */
if (ofs) skip_mmc(ofs);
/* Receive a part of the sector */
// if (buff) { /* Store data to the memory */
do
*buff++ = rcvr_mmc();
while (--cnt);
// } else { /* Forward data to the outgoing stream */
// do {
// d = rcvr_mmc();
// FORWARD(d);
// } while (--cnt);
// }
/* Skip trailing bytes and CRC */
skip_mmc(bc);
res = RES_OK;
}
}
release_spi();
return res;
}
DRESULT disk_readp (
u8 *buff, /* Pointer to the read buffer (NULL:Read bytes are forwarded to the stream) */
u32 lba, /* Sector number (LBA) */
u16 ofs, /* Byte offset to read from (0..511) */
u16 cnt /* Number of bytes to read (ofs + cnt mus be <= 512) */
)
{
DRESULT res;
u8 rc;
u16 bc;
if (!(CardType & CT_BLOCK)) lba *= 512; /* Convert to byte address if needed */
res = RES_ERROR;
if (send_cmd(CMD17, lba) == 0) { /* READ_SINGLE_BLOCK */
Tim1 = 200;
do { /* Wait for data packet in timeout of 200ms */
rc = rcvr_spi();
Tim1 = decreasetim(Tim1);
} while (rc == 0xFF && Tim1);
}
if(Tim1) {
if (rc == 0xFE) { /* A data packet arrived */
bc = 514 - ofs - cnt;
/* Skip leading bytes */
if (ofs) {
do rcvr_spi(); while (--ofs);
}
/* Receive a part of the sector */
do
*buff++ = rcvr_spi();
while (--cnt);
/* Skip trailing bytes and CRC */
do rcvr_spi(); while (--bc);
res = cnt ? 1 : RES_OK;
}
}
release_spi();
return res;
}
DRESULT disk_writep (
const BYTE *buff, // Pointer to the bytes to be written (NULL:Initiate/Finalize sector write)
DWORD sa // Number of bytes to send, Sector number (LBA) or zero
)
{
DRESULT res;
WORD bc;
static WORD wc;
if ((PINB&_BV(SD_INS))!=0x00) return RES_ERROR;
if ((PINB&_BV(SD_WP))!=0x00) return RES_ERROR;
res = RES_ERROR;
if (buff) { // Send data bytes
bc = (WORD)sa;
while (bc && wc) { // Send data bytes to the card
xmit_spi(*buff++);
wc--; bc--;
}
res = RES_OK;
} else {
if (sa) { // Initiate sector write process
if (!(CardType & CT_BLOCK)) sa *= 512; // Convert to byte address if needed
if (send_cmd(CMD24, sa) == 0) { // WRITE_SINGLE_BLOCK
xmit_spi(0xFF); xmit_spi(0xFE); // Data block header
wc = 512; // Set byte counter
res = RES_OK;
}
} else { // Finalize sector write process
bc = wc + 2;
while (bc--) xmit_spi(0); // Fill left bytes and CRC with zeros
if ((rcv_spi() & 0x1F) == 0x05) { // Receive data resp and wait for end of write process in timeout of 300ms
for (bc = 65000; rcv_spi() != 0xFF && bc; bc--) ; // Wait ready
if (bc) res = RES_OK;
}
release_spi();
}
}
return res;
}
DRESULT disk_writep (
const BYTE *buff, /* Pointer to the bytes to be written (NULL:Initiate/Finalize sector write) */
DWORD sa /* Number of bytes to send, Sector number (LBA) or zero */
)
{
DRESULT res;
WORD bc, tmr;
static WORD wc;
res = RES_ERROR;
if (buff) { /* Send data bytes */
bc = (WORD)sa;
while (bc && wc) { /* Send data bytes to the card */
xmit_mmc(*buff++);
wc--; bc--;
}
res = RES_OK;
} else {
if (sa) { /* Initiate sector write process */
if (!(CardType & CT_BLOCK)) sa *= 512; /* Convert to byte address if needed */
if (send_cmd(CMD24, sa) == 0) { /* WRITE_SINGLE_BLOCK */
xmit_mmc(0xFF); xmit_mmc(0xFE); /* Data block header */
wc = 512; /* Set byte counter */
res = RES_OK;
}
} else { /* Finalize sector write process */
bc = wc + 2;
while (bc--) xmit_mmc(0); /* Fill left bytes and CRC with zeros */
if ((rcvr_mmc() & 0x1F) == 0x05) { /* Receive data resp and wait for end of write process in timeout of 300ms */
for (tmr = 10000; rcvr_mmc() != 0xFF && tmr; tmr--) /* Wait for ready (max 1000ms) */
// DLY_US(100);
mmc_spi_Wait100us();
if (tmr) res = RES_OK;
}
release_spi();
}
}
return res;
}
//--------------------------------------------------------------------------
// Initialize Disk Drive
//--------------------------------------------------------------------------
DSTATUS disk_initialize (void)
{
BYTE n, cmd, ty, ocr[4];
WORD tmr;
INIT_SPI();
if ((PINB&_BV(SD_INS))!=0x00) return STA_NOINIT;
#if _WRITE_FUNC
if (MMC_SEL) disk_writep(0, 0); // Finalize write process if it is in progress
#endif
for (n = 100; n; n--) rcv_spi(); // Dummy clocks
ty = 0;
if (send_cmd(CMD0, 0) == 1) { // Enter Idle state
if (send_cmd(CMD8, 0x1AA) == 1) { // SDv2
for (n = 0; n < 4; n++) ocr[n] = rcv_spi(); // Get trailing return value of R7 resp
if (ocr[2] == 0x01 && ocr[3] == 0xAA) { // The card can work at vdd range of 2.7-3.6V
for (tmr = 12000; tmr && send_cmd(ACMD41, 1UL << 30); tmr--) ; // Wait for leaving idle state (ACMD41 with HCS bit)
if (tmr && send_cmd(CMD58, 0) == 0) { // Check CCS bit in the OCR
for (n = 0; n < 4; n++) ocr[n] = rcv_spi();
ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; // SDv2 (HC or SC)
}
}
} else { // SDv1 or MMCv3
if (send_cmd(ACMD41, 0) <= 1) {
ty = CT_SD1; cmd = ACMD41; // SDv1
} else {
ty = CT_MMC; cmd = CMD1; // MMCv3
}
for (tmr = 25000; tmr && send_cmd(cmd, 0); tmr--) ; // Wait for leaving idle state
if (!tmr || send_cmd(CMD16, 512) != 0) // Set R/W block length to 512
ty = 0;
}
}
CardType = ty;
release_spi();
return ty ? 0 : STA_NOINIT;
}
static
BYTE send_cmd (
BYTE cmd, /* Command byte */
DWORD arg /* Argument */
)
{
BYTE n, res;
if (cmd & 0x80) { /* ACMD<n> is the command sequense of CMD55-CMD<n> */
cmd &= 0x7F;
res = send_cmd(CMD55, 0);
if (res > 1) return res;
}
/* Select the card */
// CS_H(); rcvr_mmc();
release_spi();
// CS_L(); rcvr_mmc();
mmc_spi_SetCardSelect();
/* Send a command packet */
xmit_mmc(cmd); /* Start + Command index */
xmit_mmc((BYTE)(arg >> 24)); /* Argument[31..24] */
xmit_mmc((BYTE)(arg >> 16)); /* Argument[23..16] */
xmit_mmc((BYTE)(arg >> 8)); /* Argument[15..8] */
xmit_mmc((BYTE)arg); /* Argument[7..0] */
n = 0x01; /* Dummy CRC + Stop */
if (cmd == CMD0) n = 0x95; /* Valid CRC for CMD0(0) */
if (cmd == CMD8) n = 0x87; /* Valid CRC for CMD8(0x1AA) */
xmit_mmc(n);
/* Receive a command response */
n = 10; /* Wait for a valid response in timeout of 10 attempts */
do {
res = rcvr_mmc();
} while ((res & 0x80) && --n);
return res; /* Return with the response value */
}
DRESULT disk_writep (
const u8 *buff, /* Pointer to the bytes to be written (NULL:Initiate/Finalize sector write) */
u32 sa /* Number of bytes to send, Sector number (LBA) or zero */
)
{
DRESULT res;
u16 bc;
static u16 wc;
res = RES_ERROR;
if (buff) { /* Send data bytes */
bc = (u16)sa;
while (bc && wc) { /* Send data bytes to the card */
xmit_spi(*buff++);
wc--; bc--;
}
res = RES_OK;
} else {
if (sa) { /* Initiate sector write process */
if (!(CardType & CT_BLOCK)) sa *= 512; /* Convert to byte address if needed */
if (send_cmd(CMD24, sa) == 0) { /* WRITE_SINGLE_BLOCK */
xmit_spi(0xFF); xmit_spi(0xFE); /* Data block header */
wc = 512; /* Set byte counter */
res = RES_OK;
}
} else { /* Finalize sector write process */
bc = wc + 2;
while (bc--) xmit_spi(0); /* Fill left bytes and CRC with zeros */
if ((rcvr_spi() & 0x1F) == 0x05) { /* Receive data resp and wait for end of write process in timeout of 300ms */
for (bc = 65000; rcvr_spi() != 0xFF && bc; bc--) ; /* Wait ready */
if (bc) res = RES_OK;
}
release_spi();
}
}
return res;
}
DRESULT mmc_writesector(BYTE *buff, DWORD sa)
{
DRESULT res;
WORD bc;
GREENLEDON();
res = RES_ERROR;
if (!(CardType & CT_BLOCK)) sa *= 512; /* Convert to byte address if needed */
if (send_cmd(CMD24, sa) == 0)
{
XFER_SPI(0xFF);
XFER_SPI(0xFE);
for (bc = 0; bc < 512; ++bc)
{
XFER_SPI(buff[bc]);
}
XFER_SPI(0);
XFER_SPI(0);
if ((XFER_SPI(0xff) & 0x1F) == 0x05)
{
/* Receive data resp and wait for end of write process in timeout of 300ms */
for (bc = 65000; XFER_SPI(0xff) != 0xFF && bc; bc--) ; /* Wait ready */
if (bc) res = RES_OK;
release_spi();
}
}
GREENLEDOFF();
return res;
}
DRESULT disk_readp (
BYTE *buff, /* Pointer to the read buffer (NULL:Read bytes are forwarded to the stream) */
DWORD sector, /* Sector number (LBA) */
UINT offset, /* Byte offset to read from (0..511) */
UINT count /* Number of bytes to read (ofs + cnt mus be <= 512) */
)
{
BYTE *buff_sec = buffer;
DRESULT res;
BYTE d;
UINT bc, tmr;
if (!(CardType & CT_BLOCK)) sector *= 512; /* Convert to byte address if needed */
// check if sector is already in sector buffer
if(buffer_sector == sector) {
buff_sec += offset; // skip to requested bytes
while(count--)
*buff++ = *buff_sec++;
return RES_OK;
}
res = RES_ERROR;
if (send_cmd(CMD17, sector) == 0) { /* READ_SINGLE_BLOCK */
tmr = 1000;
do { /* Wait for data packet in timeout of 100ms */
DLY_US(100);
d = rcvr_mmc();
} while (d == 0xFF && --tmr);
if (d == 0xFE) { /* A data packet arrived */
bc = 512 - offset - count;
/* Skip leading bytes (store in buffer only) */
while(offset--)
*buff_sec++ = rcvr_mmc();
/* Receive a part of the sector */
if (buff) { /* Store data to the memory */
do
*buff_sec++ = *buff++ = rcvr_mmc();
while (--count);
} else { /* Forward data to the outgoing stream */
do {
*buff_sec++ = d = rcvr_mmc();
FORWARD(d);
} while (--count);
}
/* Skip trailing bytes (store in buffer only) */
while(bc--)
*buff_sec++ = rcvr_mmc();
/* and skip crc */
skip_mmc(2);
buffer_sector = sector;
res = RES_OK;
}
}
release_spi();
return res;
}
DSTATUS disk_initialize (void)
{
BYTE n, cmd, ty, buf[4];
UINT tmr;
#if AXI_SPI
/*
* Initialize the SPI driver so that it's ready to use,
* specify the device ID that is generated in xparameters.h.
*/
XStatus Status = XSpi_Initialize(&Spi, XPAR_SDCARD_SPI_DEVICE_ID);
if(Status != XST_SUCCESS) {\
xil_printf("Failure INIT\r\n");
return XST_FAILURE;
}
/*
* Set the SPI device as a master and in manual slave select mode such
* that the slave select signal does not toggle for every byte of a
* transfer, this must be done before the slave select is set.
*/
Status = XSpi_SetOptions(&Spi, XSP_MASTER_OPTION |
XSP_MANUAL_SSELECT_OPTION);
if(Status != XST_SUCCESS) {
xil_printf("Failure Options\r\n");
return XST_FAILURE;
}
Status = XSpi_SetSlaveSelect(&Spi, 1);
if(Status != XST_SUCCESS) {
xil_printf("Failure Slave Select\r\n");
return XST_FAILURE;
}
XSpi_Start(&Spi);
XSpi_IntrGlobalDisable(&Spi);
DLY_US(1);
#else
INIT_PORT();
CS_H();
#endif
skip_mmc(10); /* Dummy clocks */
ty = 0;
if (send_cmd(CMD0, 0) == 1) { /* Enter Idle state */
if (send_cmd(CMD8, 0x1AA) == 1) { /* SDv2 */
for (n = 0; n < 4; n++) buf[n] = rcvr_mmc(); /* Get trailing return value of R7 resp */
if (buf[2] == 0x01 && buf[3] == 0xAA) { /* The card can work at vdd range of 2.7-3.6V */
for (tmr = 1000; tmr; tmr--) { /* Wait for leaving idle state (ACMD41 with HCS bit) */
if (send_cmd(ACMD41, 1UL << 30) == 0) break;
DLY_US(1000);
}
if (tmr && send_cmd(CMD58, 0) == 0) { /* Check CCS bit in the OCR */
for (n = 0; n < 4; n++) buf[n] = rcvr_mmc();
ty = (buf[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; /* SDv2 (HC or SC) */
}
}
} else { /* SDv1 or MMCv3 */
if (send_cmd(ACMD41, 0) <= 1) {
ty = CT_SD1; cmd = ACMD41; /* SDv1 */
} else {
ty = CT_MMC; cmd = CMD1; /* MMCv3 */
}
for (tmr = 1000; tmr; tmr--) { /* Wait for leaving idle state */
if (send_cmd(cmd, 0) == 0) break;
DLY_US(1000);
}
if (!tmr || send_cmd(CMD16, 512) != 0) /* Set R/W block length to 512 */
ty = 0;
}
}
CardType = ty;
release_spi();
return ty ? 0 : STA_NOINIT;
}
DSTATUS disk_initialize(void)
{
u8 n, cmd, ty, ocr[4], rep, timeout;
u16 tmr;
power_on(); /* Force socket power on */
for (n = 10; n; n--) xmit_spi(0xFF); /* Dummy clocks */
ty = 0;
timeout=100;
// Trying to enter Idle state
do {
DESELECT();
xmit_spi(0xFF);
SELECT();
rep = send_cmd(CMD0,0);
} while ((rep != 1) && (--timeout));
if(timeout == 0)
{
DESELECT();
xmit_spi(0xFF);
SELECT();
rep = send_cmd(CMD12,0);
rep = send_cmd(CMD0,0);
if (rep != 1)
{
return STA_NOINIT;
}
}
rep = send_cmd(CMD8, 0x1AA);
// SDHC
if ( rep == 1)
{
for (n = 0; n < 4; n++) ocr[n] = rcvr_spi(); /* Get trailing return value of R7 resp */
if (ocr[2] == 0x01 && ocr[3] == 0xAA) { /* The card can work at vdd range of 2.7-3.6V */
for (tmr = 25000; tmr && send_cmd(ACMD41, 1UL << 30); tmr--) ; /* Wait for leaving idle state (ACMD41 with HCS bit) */
if (tmr && send_cmd(CMD58, 0) == 0) { /* Check CCS bit in the OCR */
for (n = 0; n < 4; n++) ocr[n] = rcvr_spi();
ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; /* SDv2 */
}
}
}
// SDSC or MMC
else
{
if (send_cmd(ACMD41, 0) <= 1) {
ty = CT_SD1; cmd = ACMD41; /* SDv1 */
} else {
ty = CT_MMC; cmd = CMD1; /* MMCv3 */
}
for (tmr = 25000; tmr && send_cmd(cmd, 0); tmr--) ; /* Wait for leaving idle state */
if (!tmr || send_cmd(CMD16, 512) != 0) /* Set R/W block length to 512 */
ty = 0;
}
CardType = ty;
release_spi();
// Initialization succeded
if (ty)
{
FCLK_FAST();
return RES_OK;
}
// Initialization failed
else
{
power_off();
return STA_NOINIT;
}
}
DRESULT disk_readp (
BYTE *buff, /* Pointer to the read buffer (NULL:Read bytes are forwarded to the stream) */
DWORD lba, /* Sector number (LBA) */
WORD ofs, /* Byte offset to read from (0..511) */
WORD cnt /* Number of bytes to read (ofs + cnt mus be <= 512) */
)
{
#if 0
xil_printf("\n\n");
xil_printf("lba = %d\n", lba);
xil_printf("ofs = %d\n", ofs);
xil_printf("cnt = %d\n", cnt);
#endif
typedef struct LRUEntry {
bool valid;
DWORD lba_key;
BYTE buffer[512];
struct LRUEntry* more_recently_used;
struct LRUEntry* less_recently_used;
} LRUEntry;
#define NUM_LRU_ENTRIES 2 // At least 2
static LRUEntry* least_recently_used_entry = 0;
static LRUEntry* most_recently_used_entry = 0;
static LRUEntry read_buff_lru[NUM_LRU_ENTRIES];
static BYTE write_buff[512];
// Init once.
if(!write_buff[0]){
memset(write_buff, 0xff, 512);
// 0 is most recent and NUM_LRU_ENTRIES-1 is least recent.
read_buff_lru[0].valid = false;
read_buff_lru[0].more_recently_used = NULL;
read_buff_lru[0].less_recently_used = &read_buff_lru[1];
most_recently_used_entry = &read_buff_lru[0];
for(int i = 1; i < NUM_LRU_ENTRIES-1; i++){
read_buff_lru[i].valid = false;
read_buff_lru[i].more_recently_used = &read_buff_lru[i-1];
read_buff_lru[i].less_recently_used = &read_buff_lru[i+1];
}
read_buff_lru[NUM_LRU_ENTRIES-1].valid = false;
read_buff_lru[NUM_LRU_ENTRIES-1].more_recently_used = &read_buff_lru[NUM_LRU_ENTRIES-2];
read_buff_lru[NUM_LRU_ENTRIES-1].less_recently_used = NULL;
least_recently_used_entry = &read_buff_lru[NUM_LRU_ENTRIES-1];
}
// Search for entry...
for(int i = 0; i < NUM_LRU_ENTRIES; i++){
if(read_buff_lru[i].valid && read_buff_lru[i].lba_key == lba){
if(buff){
memcpy(buff, read_buff_lru[i].buffer + ofs, cnt);
}
LRUEntry* curr_entry = &read_buff_lru[i];
// Update LRU list.
if(curr_entry != most_recently_used_entry){
if(curr_entry == least_recently_used_entry){
least_recently_used_entry = curr_entry->more_recently_used;
least_recently_used_entry->less_recently_used = NULL;
}
most_recently_used_entry->more_recently_used = curr_entry;
curr_entry->less_recently_used = most_recently_used_entry;
curr_entry->more_recently_used = NULL;
most_recently_used_entry = curr_entry;
}
return RES_OK;
}
}
// Took least recently used entry to read to it.
// Update LRU list.
LRUEntry* curr_entry = least_recently_used_entry;
// Setting new least recently used.
least_recently_used_entry = curr_entry->more_recently_used;
least_recently_used_entry->less_recently_used = NULL;
// Prepending new most recently used.
most_recently_used_entry->more_recently_used = curr_entry;
curr_entry->less_recently_used = most_recently_used_entry;
curr_entry->more_recently_used = NULL;
most_recently_used_entry = curr_entry;
curr_entry->valid = true;
curr_entry->lba_key = lba;
BYTE* read_buff = curr_entry->buffer;
if (!(CardType & CT_BLOCK)) lba *= 512; /* Convert to byte address if needed */
DRESULT res = RES_ERROR;
if (send_cmd(CMD17, lba) == 0) { /* READ_SINGLE_BLOCK */
BYTE d;
int tmr = 100000;
do { /* Wait for data packet in timeout of 100ms */
DLY_US(1);
d = rcvr_mmc();
} while (d == 0xFF && --tmr);
if (d == 0xFE) { /* A data packet arrived */
XStatus Status = XSpi_Transfer(&Spi, write_buff, read_buff, 512);
if(Status != XST_SUCCESS) {
xil_printf("Error in read transfer\r\n");
return res;
}
if(buff){
memcpy(buff, read_buff + ofs, cnt);
}
res = RES_OK;
}
}
release_spi();
return res;
}
DSTATUS mmc_initialize (void)
{
BYTE n, cmd, ty, ocr[4];
WORD tmr;
GREENLEDON();
INIT_SPI();
#if _WRITE_FUNC
if (MMC_SEL) disk_writep(0, 0); /* Finalize write process if it is in progress */
#endif
for (n = 11; n; --n) XFER_SPI(0xff); /* Dummy clocks */
ty = 0;
if (send_cmd(CMD0, 0) == 1)
{
/* Enter Idle state */
if (send_cmd(CMD8, 0x1AA) == 1)
{ /* SDv2 */
for (n = 0; n < 4; n++)
{
ocr[n] = XFER_SPI(0xff); /* Get trailing return value of R7 resp */
}
if (ocr[2] == 0x01 && ocr[3] == 0xAA)
{
/* The card can work at vdd range of 2.7-3.6V */
for (tmr = 12000; tmr && send_cmd(ACMD41, 1UL << 30); tmr--) ; /* Wait for leaving idle state (ACMD41 with HCS bit) */
if (tmr && send_cmd(CMD58, 0) == 0)
{
/* Check CCS bit in the OCR */
for (n = 0; n < 4; n++) ocr[n] = XFER_SPI(0xff);
ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; /* SDv2 (HC or SC) */
}
}
}
else
{ /* SDv1 or MMCv3 */
if (send_cmd(ACMD41, 0) <= 1)
{
ty = CT_SD1; cmd = ACMD41; /* SDv1 */
}
else
{
ty = CT_MMC; cmd = CMD1; /* MMCv3 */
}
for (tmr = 25000; tmr && send_cmd(cmd, 0); tmr--) ; /* Wait for leaving idle state */
if (!tmr || send_cmd(CMD16, 512) != 0) /* Set R/W block length to 512 */
{
ty = 0;
}
}
}
CardType = ty;
release_spi();
GREENLEDOFF();
return ty ? 0 : STA_NOINIT;
}