Diff: slcan.cpp

Revision:

2:1327e61cc56b

Parent:

1:3644b10bce2f

Child:

3:bc163d555ddc

--- a/slcan.cpp	Thu Jun 09 05:30:21 2016 +0000
+++ b/slcan.cpp	Thu Jun 09 06:29:08 2016 +0000
@@ -70,6 +70,21 @@
 
 }
 
+uint8_t SLCANBase::getFirmwareVersion() {
+    // firmware version in BCD
+    return 0x10;
+}
+
+uint8_t SLCANBase::getHardwareVersion() {
+    // hardware version in BCD
+    return 0x10;
+}
+
+const char* SLCANBase::getSerialString() {
+    // 4 character serial number
+    return "C254";
+}
+
 bool SLCANBase::update() {
     bool active = false;
     if (processCommands()) {
@@ -132,8 +147,23 @@
     return len;
 }
 
-bool SLCANBase::execCommand(const char* command) {
+size_t SLCANBase::commandResponseLength(const char* command) {
+    if (command[0] == 'N') {
+        return 6;
+    } else if (command[0] == 'V') {
+        return 6;
+    } else if (command[0] == 'v') {
+        return 4;
+    } else {
+        return 1;
+    }
+}
+
+bool SLCANBase::execCommand(const char* command, char* response) {
     bool success = false;
+    if (response) {
+        response[0] = '\0';
+    }
 
     switch (command[0]) {
         // Configuration commands
@@ -153,6 +183,13 @@
             success = execTransmitCommand(command);
             break;
         }
+        // Diagnostic commands
+        case 'V':
+        case 'v':
+        case 'N':
+        case 'W':
+            success = execDiagnosticCommand(command, response);
+            break;
         default: {
             success = false;
             break;
@@ -261,6 +298,76 @@
     return success;
 }
 
+bool SLCANBase::execDiagnosticCommand(const char* command, char* response) {
+    bool success = false;
+    size_t len = strlen(command);
+    
+    // Validate command length
+    if (command[0] == 'W') {
+        if (len != 5) {
+            return false;
+        }
+    } else if (len != 1) {
+        return false;
+    }
+    
+    if (!response) {
+        return false;
+    }
+
+    switch (command[0]) {
+        case 'V': {
+            success = true;
+            uint8_t hwVersion = getHardwareVersion();
+            uint8_t fwVersion = getFirmwareVersion();
+            
+            response[0] = 'V';
+            response[1] = format_nibble(hwVersion >> 4);
+            response[2] = format_nibble(hwVersion >> 0);
+            response[3] = format_nibble(fwVersion >> 4);
+            response[4] = format_nibble(fwVersion >> 0);
+            response[5] = '\0';
+            break;
+        }
+        case 'v': {
+            success = true;
+            uint8_t fwVersion = getFirmwareVersion();
+            response[0] = 'v';
+            response[1] = format_nibble(fwVersion >> 4);
+            response[2] = format_nibble(fwVersion >> 0);
+            response[3] = '\0';
+            break;
+        }
+        case 'N': {
+            success = true;
+            const char* serial = getSerialString();
+            size_t index = 0;
+            response[index++] = 'N';
+            for (int i=0; i < 4; i++) {
+                char c = serial[i];
+                if (c == '\0') {
+                    break;
+                } else {
+                    response[index++] = c;
+                }
+            }
+            response[index] = '\0';
+            break;
+        }
+        case 'W': {
+            // Just swallow the MCP2515 register write command
+            success = true;
+            break;
+        }
+        default: {
+            success = false;
+            break;
+        }
+    }
+    
+    return success;
+}
+
 USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
     : stream(stream),
       can(can),
@@ -325,16 +432,24 @@
     }
     
     // Process the current command if there's space to send the response
-    if (commandQueued && (outputPacketLen < sizeof(outputPacketBuffer))) {
-        if (execCommand(inputCommandBuffer)) {
-            // Success
-            outputPacketBuffer[outputPacketLen++] = '\r';
-        } else {
-            // Failure
-            outputPacketBuffer[outputPacketLen++] = '\a';
+    if (commandQueued) {
+        size_t responseLength = commandResponseLength(inputCommandBuffer);
+        if ((outputPacketLen + responseLength) <= sizeof(outputPacketBuffer)) {
+            char outputResponseBuffer[32];
+            outputResponseBuffer[0] = '\0';
+            if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
+                // Success
+                for (char* s = outputResponseBuffer; *s != '\0'; s++) {
+                    outputPacketBuffer[outputPacketLen++] = *s;
+                }
+                outputPacketBuffer[outputPacketLen++] = '\r';
+            } else {
+                // Failure
+                outputPacketBuffer[outputPacketLen++] = '\a';
+            }
+            commandQueued = false;
+            active = true;
         }
-        commandQueued = false;
-        active = true;
     }
     
     return active;
@@ -450,10 +565,13 @@
         }
     }
     
-    // Process the current command if there's space to send the response
+    // Process the current command
     if (commandQueued) {
-        if (execCommand(inputCommandBuffer)) {
+        char outputResponseBuffer[32];
+        outputResponseBuffer[0] = '\0';
+        if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
             // Success
+            stream.puts(outputResponseBuffer);
             stream.putc('\r');
         } else {
             // Failure