1 files changed, 106 insertions, 5 deletions

diff --git a/software/mpu/usb-cdc-shell.c b/software/mpu/usb-cdc-shell.c
index 00276aa..955cd0a 100644
--- a/software/mpu/usb-cdc-shell.c
+++ b/software/mpu/usb-cdc-shell.c
@@ -203,8 +203,8 @@ static char* blkstate_to_string(blkstate_t state)
 
 static void sdc_read_test(BaseSequentialStream *chp, const uint32_t sector)
 {
-  chprintf(chp, "Running SDCard Test\r\n");
-  chprintf(chp, "-------------------\r\n\r\n");
+  chprintf(chp, "Running SDCard Read Test\r\n");
+  chprintf(chp, "------------------------\r\n\r\n");
 
   unsigned int n;
   chprintf(chp, "waiting for card .");
@@ -289,11 +289,11 @@ static void sdc_read_test(BaseSequentialStream *chp, const uint32_t sector)
   chprintf(chp, " timeout\r\n");
 }
 
-static void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[])
+static void cmd_sdcr(BaseSequentialStream *chp, int argc, char *argv[])
 {
   (void)argv;
   if (argc != 1) {
-    chprintf(chp, "Usage: sdc <sector>\r\n");
+    chprintf(chp, "Usage: sdcr <sector>\r\n");
     return;
   }
 
@@ -302,6 +302,106 @@ static void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[])
   sdcStop(&SDCD1);
 }
 
+static void sdc_write_test(BaseSequentialStream *chp, const uint32_t sector)
+{
+  chprintf(chp, "Running SDCard Write Test\r\n");
+  chprintf(chp, "-------------------------\r\n\r\n");
+
+  unsigned int n;
+  chprintf(chp, "waiting for card .");
+  for(n = 0; n < 20; n++) {
+    if(blkIsInserted(&SDCD1)) {
+      chprintf(chp, " found\r\n");
+      chprintf(chp, "write protection: %s\r\n", blkIsWriteProtected(&SDCD1) ? "ON" : "OFF");
+      if(blkIsWriteProtected(&SDCD1)) {
+        chprintf(chp, "aborting due to write protection!\r\n");
+        return;
+      }
+
+            // Connect
+      if(blkConnect(&SDCD1) == CH_SUCCESS) {
+        chprintf(chp, "connecting ...");
+        while(blkGetDriverState(&SDCD1) == BLK_CONNECTING) {
+          chThdSleepMilliseconds(100);
+          chprintf(chp, ".");
+        }
+        if(blkGetDriverState(&SDCD1) != BLK_READY) {
+          chprintf(chp, "error.\r\nERROR driver not ready (state now: %s)\r\n", blkstate_to_string((blkGetDriverState(&SDCD1))));
+          return;
+        }
+        chprintf(chp, "done.\r\n");
+
+        BlockDeviceInfo bi;
+        if(blkGetInfo(&SDCD1, &bi) == CH_FAILED) {
+          chprintf(chp, "ERROR: reading device info\r\n");
+          return;
+        }
+        chprintf(chp, "Device: blocksize=%d, blocknum=%d\r\n", bi.blk_size, bi.blk_num);
+
+        if(bi.blk_num <= sector) {
+          chprintf(chp, "Requested sector (%d) is after end of device\r\n", sector);
+          return;
+        }
+
+            // Write
+        chprintf(chp, "\r\nWriting Block %ld: \r\n", sector);
+
+        uint8_t buf[1024];
+        if(sizeof(buf) < bi.blk_size) {
+          chprintf(chp, "Your write buffer ist too small - aborting...\r\n");
+          return;
+        }
+        uint32_t i;
+        for(i=0; i<sizeof(buf); ++i) {
+          buf[i] = (uint8_t)i;
+        }
+
+        if(blkWrite(&SDCD1, sector, buf, 1) == CH_FAILED) {
+          chprintf(chp, "ERROR: blkWrite returned with error\r\n");
+          return;
+        }
+
+            // Disconnect
+        if(blkDisconnect(&SDCD1) == CH_FAILED) {
+          chprintf(chp, "ERROR: blkDisconnect returned whith error\r\n");
+          return;
+        }
+        chprintf(chp, "disconnecting ...");
+        while(blkGetDriverState(&SDCD1) == BLK_DISCONNECTING) {
+          chThdSleepMilliseconds(100);
+          chprintf(chp, ".");
+        }
+        if(blkGetDriverState(&SDCD1) != BLK_ACTIVE) {
+          chprintf(chp, "error.\r\nERROR driver not ready (state now: %s)\r\n", blkstate_to_string((blkGetDriverState(&SDCD1))));
+          return;
+        }
+        chprintf(chp, "done.\r\n");
+      }
+      else {
+        chprintf(chp, "ERROR: blkConnect returned with error\r\n");
+      }
+      return;
+    }
+    chprintf(chp, ".");
+    chThdSleepMilliseconds(1000);
+  }
+
+  chprintf(chp, " timeout\r\n");
+}
+
+static void cmd_sdcw(BaseSequentialStream *chp, int argc, char *argv[])
+{
+  (void)argv;
+  if (argc != 1) {
+    chprintf(chp, "Usage: sdcw <sector>\r\n");
+    return;
+  }
+
+  sdcStart(&SDCD1, &sdccfg);
+  sdc_write_test(chp, strtoul(argv[0], NULL, 10));
+  sdcStop(&SDCD1);
+}
+
 static void cmd_mem(BaseSequentialStream *chp, int argc, char *argv[])
 {
   size_t n, size;
@@ -423,7 +523,8 @@ static void cmd_reboot(BaseSequentialStream *chp, int argc, char *argv[])
 /* } */
 
 static const ShellCommand commands[] = {
-  {"sdc", cmd_sdc},
+  {"sdcr", cmd_sdcr},
+  {"sdcw", cmd_sdcw},
   {"mem", cmd_mem},
   {"threads", cmd_threads},
   {"test", cmd_test},