static msg_t thdUsbStorage(void *arg)
{
(void) arg; // unused
chRegSetThreadName("UsbStorage:polling");
uint antiBounce=5;
EventListener connected;
// Should use EXTI interrupt instead of active polling,
// but in the chibios_opencm3 implementation, since EXTI is
// used via libopencm3, ISR are routed on pprz/opencm3 and cannot
// be used concurrently by chibios api
// Should be fixed when using chibios-rt branch
while (!chThdShouldTerminate() && antiBounce) {
const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS);
if (usbConnected)
antiBounce--;
else
antiBounce=5;
chThdSleepMilliseconds(20);
}
isRunning = true;
chRegSetThreadName("UsbStorage:connected");
/* Stop the logs*/
chibios_logFinish (true);
/* connect sdcard sdc interface sdio */
if (sdioConnect () == false)
chThdExit (RDY_TIMEOUT);
/* initialize the USB mass storage driver */
msdInit(&UMSD1);
/* start the USB mass storage service */
msdStart(&UMSD1, &msdConfig);
/* start the USB driver */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
usbStart(&USBD1, &usbConfig);
usbConnectBus(&USBD1);
/* wait for a real usb storage connexion before shutting down autopilot */
chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1));
chEvtWaitOne(EVENT_MASK(1));
/* stop autopilot */
if (pprzThdPtr != NULL) {
chThdTerminate (pprzThdPtr);
chThdWait (pprzThdPtr);
pprzThdPtr = NULL;
}
/* wait until usb-storage is unmount and usb cable is unplugged*/
while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) {
chThdSleepMilliseconds(10);
}
/* then close open descriptors and reboot autopilot */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(500);
msdStop(&UMSD1);
sdioDisconnect ();
MCU_RESTART();
return RDY_OK;
}
void cmd_sdiotest(void) {
uint32_t i = 0;
FRESULT err = 0;
bool format = false;
chThdSleepMilliseconds(100);
if (!sdioConnect ()) {
sdioDebug (" FAIL\r\n ");
return;
}
if (TRUE) {
sdioDebug ("OK\r\n");
sdioDebug ("*** Card CSD content is: ");
sdioDebug ("%X %X %X %X \r\n", (&SDCD1)->csd[3], (&SDCD1)->csd[2],
(&SDCD1)->csd[1], (&SDCD1)->csd[0]);
sdioDebug ("capacity = %d sectors of 512 bytes = %d Mo\r\n",
SDCD1.capacity, SDCD1.capacity / 2048);
sdioDebug ("Single aligned read...");
chThdSleepMilliseconds(100);
if (sdcRead(&SDCD1, 0, inbuf, 1))
goto error;
sdioDebug (" OK\r\n");
chThdSleepMilliseconds(100);
sdioDebug ("Single unaligned read...");
chThdSleepMilliseconds(100);
if (sdcRead(&SDCD1, 0, inbuf + 1, 1))
goto error;
if (sdcRead(&SDCD1, 0, inbuf + 2, 1))
goto error;
if (sdcRead(&SDCD1, 0, inbuf + 3, 1))
goto error;
sdioDebug (" OK\r\n");
chThdSleepMilliseconds(100);
sdioDebug ("Multiple aligned reads...");
chThdSleepMilliseconds(100);
fillbuffers(0x55);
/* fill reference buffer from SD card */
if (sdcRead(&SDCD1, 0, inbuf, SDC_BURST_SIZE))
goto error;
for (i=0; i<1000; i++){
if (sdcRead(&SDCD1, 0, outbuf, SDC_BURST_SIZE))
goto error;
if (memcmp(inbuf, outbuf, SDC_BURST_SIZE * MMCSD_BLOCK_SIZE) != 0)
goto error;
}
sdioDebug (" OK\r\n");
chThdSleepMilliseconds(100);
sdioDebug ("Multiple unaligned reads...");
chThdSleepMilliseconds(100);
fillbuffers(0x55);
/* fill reference buffer from SD card */
if (sdcRead(&SDCD1, 0, inbuf + 1, SDC_BURST_SIZE))
goto error;
for (i=0; i<1000; i++){
if (sdcRead(&SDCD1, 0, outbuf + 1, SDC_BURST_SIZE))
goto error;
if (memcmp(inbuf, outbuf, SDC_BURST_SIZE * MMCSD_BLOCK_SIZE) != 0)
goto error;
}
sdioDebug (" OK\r\n");
chThdSleepMilliseconds(100);
#if SDC_DATA_DESTRUCTIVE_TEST
if (format) {
sdioDebug ("Single aligned write...");
chThdSleepMilliseconds(100);
fillbuffer(0xAA, inbuf);
if (sdcWrite(&SDCD1, 0, inbuf, 1))
goto error;
fillbuffer(0, outbuf);
if (sdcRead(&SDCD1, 0, outbuf, 1))
goto error;
if (memcmp(inbuf, outbuf, MMCSD_BLOCK_SIZE) != 0)
goto error;
sdioDebug (" OK\r\n");
sdioDebug ("Running badblocks at 0x10000 offset...");
chThdSleepMilliseconds(100);
if(badblocks(0x10000, 0x11000, SDC_BURST_SIZE, 0xAA))
goto error;
sdioDebug (" OK\r\n");
} else {
}
#endif /* !SDC_DATA_DESTRUCTIVE_TEST */
/**
* Now perform some FS tests.
*/
DWORD clusters=0;
FATFS *fsp=NULL;
FIL FileObject;
uint32_t bytes_written=0;
uint32_t bytes_read=0;
FILINFO fno ;
#if _USE_LFN
char lfn[_MAX_LFN + 1];
fno.lfname = lfn;
fno.lfsize = sizeof lfn;
#endif
const uint8_t teststring[] = {"This is test file\r\n"} ;
/* FS object.*/
static FATFS SDC_FS;
#if SDC_DATA_DESTRUCTIVE_TEST
if (format) {
sdioDebug ("Formatting... ");
chThdSleepMilliseconds(100);
sdioDebug ("Register working area for filesystem... ");
chThdSleepMilliseconds(100);
err = f_mount(0, &SDC_FS);
if (err != FR_OK){
goto error;
}
else{
sdioDebug ("OK\r\n");
}
}
if (format) {
sdioDebug ("f_mkfs starting ... ");
chThdSleepMilliseconds(100);
err = f_mkfs (0,0,0);
if (err != FR_OK){
goto error;
} else {
sdioDebug ("OK\r\n");
}
}
#endif /* SDC_DATA_DESTRUCTIVE_TEST */
sdioDebug ("get free space on filesystem... ");
chThdSleepMilliseconds(100);
err = f_getfree(NULL, &clusters, &fsp);
if (err != FR_OK)
goto error;
sdioDebug ("OK\r\n");
sdioDebug ("FS: %lu free clusters, %lu sectors per cluster, %lu bytes free\r\n",
clusters, (uint32_t)SDC_FS.csize,
clusters * (uint32_t)SDC_FS.csize * (uint32_t)MMCSD_BLOCK_SIZE);
sdioDebug ("Create file \"chtest.txt\"... ");
chThdSleepMilliseconds(100);
err = f_open(&FileObject, "chtest.txt", FA_WRITE | FA_OPEN_ALWAYS);
if (err != FR_OK) {
goto error;
}
sdioDebug ("OK\r\n");
sdioDebug ("Write some data in it... ");
chThdSleepMilliseconds(100);
err = f_write(&FileObject, teststring, sizeof(teststring), (void *)&bytes_written);
if (err != FR_OK) {
goto error;
}
else
sdioDebug ("OK\r\n");
sdioDebug ("Close file \"chtest.txt\"... ");
err = f_close(&FileObject);
if (err != FR_OK) {
goto error;
}
else
sdioDebug ("OK\r\n");
sdioDebug ("Check file size \"chtest.txt\"... ");
chThdSleepMilliseconds(10);
err = f_stat("chtest.txt", &fno);
chThdSleepMilliseconds(100);
if (err != FR_OK) {
goto error;
} else {
if (fno.fsize == sizeof(teststring)) {
sdioDebug ("OK\r\n");
}
else
goto error;
}
sdioDebug ("Check file content \"chtest.txt\"... ");
err = f_open(&FileObject, "chtest.txt", FA_READ | FA_OPEN_EXISTING);
chThdSleepMilliseconds(100);
if (err != FR_OK) {
goto error;
}
uint8_t buf[sizeof(teststring)];
err = f_read(&FileObject, buf, sizeof(teststring), (void *)&bytes_read);
if (err != FR_OK) {
goto error;
} else {
if (memcmp(teststring, buf, sizeof(teststring)) != 0){
goto error;
} else {
sdioDebug ("OK\r\n");
}
}
{
FILINFO fno;
DIR dir;
// char *fn; /* This function is assuming non-Unicode cfg. */
#if _USE_LFN
char lfn[_MAX_LFN + 1];
fno.lfname = lfn;
fno.lfsize = sizeof lfn;
#endif
const char *path = "";
FRESULT res =0;
res = f_opendir(&dir, path); /* Open the directory */
if (res == FR_OK) {
for (;;) {
res = f_readdir(&dir, &fno); /* Read a directory item */
if (res != FR_OK || fno.fname[0] == 0) break; /* Break on error or end of dir */
if (fno.fname[0] == '.') continue; /* Ignore dot entry */
#if _USE_LFN
// fn = fno.lfname;
#else
// fn = fno.fname;
#endif
/* It is a file. */
//chprintf(chp, "readdir %s/%s\r\n", path, fn);
}
}
}
sdioDebug ("Umount filesystem... ");
f_mount(0, NULL);
sdioDebug ("OK\r\n");
sdioDebug ("Disconnecting from SDIO...");
chThdSleepMilliseconds(100);
if (!sdioDisconnect())
goto error;
sdioDebug (" OK\r\n");
sdioDebug ("------------------------------------------------------\r\n");
sdioDebug ("All tests passed successfully.\r\n");
chThdSleepMilliseconds(100);
sdioDisconnect();
return;
}
error:
sdioDebug ("SDC error [%d] occurs\r\n", err);
sdioDisconnect();
}