Diff: slcan.cpp

Revision:

0:f2565808eea5

Child:

1:3644b10bce2f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/slcan.cpp	Sat Jun 04 04:40:58 2016 +0000
@@ -0,0 +1,488 @@
+#include "slcan.h"
+
+// Helper methods for parsing commands
+static bool parse_hex_digits(const char* input, uint8_t num_digits, uint32_t* value_out) {
+    bool success = true;
+    uint32_t value = 0;
+
+    uint8_t i;
+    for (i=0; i < num_digits; i++) {
+        uint32_t nibble = 0;
+        if (input[i] >= '0' && input[i] <= '9') {
+            nibble = 0x0 + (input[i] - '0');
+        } else if (input[i] >= 'a' && input[i] <= 'f') {
+            nibble = 0xA + (input[i] - 'a');
+        } else if (input[i] >= 'A' && input[i] <= 'F') {
+            nibble = 0xA + (input[i] - 'A');
+        } else {
+            success = false;
+            break;
+        }
+        uint8_t offset = 4*(num_digits-i-1);
+        value |= (nibble << offset);
+    }
+
+    if (success) {
+        *value_out = value;
+    }
+
+    return success;
+}
+
+static bool parse_hex_values(const char* input, uint8_t num_values, uint8_t* values_out) {
+    uint8_t i;
+    for (i=0; i < num_values; i++) {
+        uint32_t value;
+        if (parse_hex_digits(input, 2, &value)) {
+            values_out[i] = (uint8_t)value;
+        } else {
+            return false;
+        }
+        input += 2;
+    }
+
+    return true;
+}
+
+static bool parse_dec_digit(const char* input, uint8_t* value_out) {
+    if (input[0] >= '0' && input[0] <= '9') {
+        *value_out = 0 + (input[0] - '0');
+        return true;
+    } else {
+        return false;
+    }
+}
+
+static inline char format_nibble(uint8_t x) {
+    uint8_t nibble = x & 0x0F;
+    return (nibble < 10) ? ('0' + nibble) : ('A' + (nibble - 10));
+}
+
+static inline char format_digit(uint8_t d) {
+    return '0' + d;
+}
+
+SLCANBase::SLCANBase() {
+    
+}
+
+SLCANBase::~SLCANBase() {
+
+}
+
+bool SLCANBase::update() {
+    bool active = false;
+    if (processCommands()) {
+        active = true;
+    }
+    if (processCANMessages()) {
+        active = true;
+    }
+    
+    if (active) {
+        flush();
+    }
+    
+    return active;
+}
+
+size_t SLCANBase::formattedCANMessageLength(const CANMessage& msg) {
+    size_t len;
+    if (msg.format == CANStandard) {
+        len = 1 + 3 + 1 + (2 * msg.len) + 1;
+    } else {
+        len = 1 + 8 + 1 + (2 * msg.len) + 1;
+    }
+    
+    return len;
+}
+
+size_t SLCANBase::formatCANMessage(const CANMessage& msg, char* buf, size_t max_len) {
+    size_t len = formattedCANMessageLength(msg);
+    if (len > max_len) {
+        return 0;
+    }
+    
+    if (msg.format == CANStandard) {
+        *buf++ = (msg.type == CANData) ? 't' : 'r';
+        *buf++ = format_nibble((uint8_t)(msg.id >> 8));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 4));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 0));
+        *buf++ = format_digit(msg.len);
+    } else {
+        *buf++ = (msg.type == CANData) ? 'T' : 'R';
+        *buf++ = format_nibble((uint8_t)(msg.id >> 28));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 24));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 20));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 16));
+        *buf++ = format_nibble((uint8_t)(msg.id >> 12));
+        *buf++ = format_nibble((uint8_t)(msg.id >>  8));
+        *buf++ = format_nibble((uint8_t)(msg.id >>  4));
+        *buf++ = format_nibble((uint8_t)(msg.id >>  0));
+        *buf++ = format_digit(msg.len);
+    }
+    
+    for (unsigned char i=0; i < msg.len; i++) {
+        *buf++ = format_nibble((uint8_t)(msg.data[i] >> 4));
+        *buf++ = format_nibble((uint8_t)(msg.data[i] >> 0));
+    }
+    
+    *buf++ = '\r';
+    
+    return len;
+}
+
+bool SLCANBase::execCommand(const char* command) {
+    bool success = false;
+
+    switch (command[0]) {
+        // Configuration commands
+        case 'S':
+        case 'O':
+        case 'L':
+        case 'l':
+        case 'C': {
+            success = execConfigCommand(command);
+            break;
+        }
+        // Transmission commands
+        case 't':
+        case 'T':
+        case 'r':
+        case 'R': {
+            success = execTransmitCommand(command);
+            break;
+        }
+        default: {
+            success = false;
+            break;
+        }
+    }
+
+    return success;
+}
+
+bool SLCANBase::execConfigCommand(const char* command) {
+    bool success = false;
+    size_t len = strlen(command);
+    
+    // Validate command length
+    if (len != 1 && command[0] != 'S') {
+        return false;
+    } else if (command[0] == 'S' && len != 2) {
+        return false;
+    }
+
+    switch (command[0]) {
+        case 'S': {
+            bool known = true;
+            int baudrate;
+            switch (command[1]) {
+                case '0': baudrate =   10000; break;
+                case '1': baudrate =   20000; break;
+                case '2': baudrate =   50000; break;
+                case '3': baudrate =  100000; break;
+                case '4': baudrate =  125000; break;
+                case '5': baudrate =  250000; break;
+                case '6': baudrate =  500000; break;
+                case '7': baudrate =  800000; break;
+                case '8': baudrate = 1000000; break;
+                default:  known = false; break;
+            }
+
+            if (known) {
+                success = setBaudrate(baudrate);
+            }
+
+            break;
+        }
+        case 'O': {
+            success = setMode(CAN::Normal);
+            break;
+        }
+        case 'L': {
+            success = setMode(CAN::Silent);
+            break;
+        }
+        case 'l': {
+            success = setMode(CAN::SilentTest);
+            break;
+        }
+        case 'C': {
+            success = setMode(CAN::Reset);
+            break;
+        }
+        default: {
+            success = false;
+            break;
+        }
+    }
+
+    return success;
+}
+
+bool SLCANBase::execTransmitCommand(const char* command) {
+    bool success = false;
+
+    size_t len = strlen(command);
+
+    bool validMessage = false;
+    CANMessage msg;
+    if (command[0] == 't' || command[0] == 'T') {
+        msg.type = CANData;
+        msg.format = (command[0] == 't') ? CANStandard : CANExtended;
+        size_t idLen = msg.format == CANStandard ? 3 : 8;
+        if ((len >= idLen + 2) &&
+            parse_hex_digits(&command[1], idLen, (uint32_t*)&msg.id) &&
+            parse_dec_digit(&command[idLen + 1], &msg.len)) {
+            if ((len == idLen + 2 + 2*msg.len) &&
+                (msg.len <= 8) &&
+                parse_hex_values(&command[idLen + 2], msg.len, msg.data)) {
+                validMessage = true;
+            }
+        }
+    } else if (command[0] == 'r' || command[0] == 'R') {
+        msg.type = CANRemote;
+        msg.format = (command[0] == 'r') ? CANStandard : CANExtended;
+        size_t idLen = msg.format == CANStandard ? 3 : 8;
+        if ((len == idLen + 2) &&
+            parse_hex_digits(&command[1], idLen, (uint32_t*)(&msg.id)) &&
+            parse_dec_digit(&command[idLen + 1], &msg.len)) {
+            if (msg.len <= 8) {
+                validMessage = true;
+            }
+        }
+    }
+
+    if (validMessage) {
+        success = transmitMessage(msg);
+    }
+
+    return success;
+}
+
+USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
+    : stream(stream),
+      can(can),
+      messageQueued(false),
+      commandQueued(false),
+      commandOverflow(false),
+      inputCommandLen(0),
+      outputPacketLen(0) {
+    
+}
+
+bool USBSLCAN::setBaudrate(int baudrate) {
+    return (can.frequency(baudrate) == 1);
+}
+
+bool USBSLCAN::setMode(CAN::Mode mode) {
+    return (can.mode(mode) == 1);
+}
+
+bool USBSLCAN::transmitMessage(CANMessage& msg) {
+    return (can.write(msg) == 1);    
+}
+
+bool USBSLCAN::getNextCANMessage(CANMessage& msg) {
+    return (can.read(msg) == 1);
+}
+
+/* Parse and execute a single SLCAN command and enqueue the response */
+bool USBSLCAN::processCommands() {
+    bool active = false;
+    
+    // Buffer an entire command
+    while (!commandQueued && stream.readable()) {
+        char c = (char)stream.getc();
+        if (c == '\r') {
+            if (commandOverflow) {
+                // Replace with a dummy invalid command so we return an error
+                inputCommandBuffer[0] = '!';
+                inputCommandBuffer[1] = '\0';
+                inputCommandLen = 0;
+                commandOverflow = false;
+                active = true;
+            } else {
+                // Null-terminate the buffered command
+                inputCommandBuffer[inputCommandLen] = '\0';
+                stream.puts(inputCommandBuffer);
+                inputCommandLen = 0;
+                active = true;
+            }
+            commandQueued = true;
+        } else if (c == '\n' && inputCommandLen == 0) {
+            // Ignore line feeds immediately after a carriage return
+        } else if (commandOverflow) {
+            // Swallow the rest of the command when overflow occurs
+        } else {
+            // Append to the end of the command
+            inputCommandBuffer[inputCommandLen++] = c;
+
+            if (inputCommandLen >= sizeof(inputCommandBuffer)) {
+                commandOverflow = true;
+            }
+        }
+    }
+    
+    // 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';
+        }
+        commandQueued = false;
+        active = true;
+    }
+    
+    return active;
+}
+
+/* Read and enqueue as many received CAN messages as will fit */
+bool USBSLCAN::processCANMessages() {
+    bool active = false;
+    
+    size_t bytesAvailable = sizeof(outputPacketBuffer - outputPacketLen);
+    char* packetTail = &outputPacketBuffer[outputPacketLen];
+    
+    if (messageQueued) {
+        size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
+        if (bytesConsumed > 0) {
+            active = true;
+            messageQueued = false;
+            bytesAvailable -= bytesConsumed;
+            packetTail += bytesConsumed;
+            outputPacketLen += bytesConsumed;
+        }
+    }
+    
+    if (!messageQueued) {
+        while (getNextCANMessage(queuedMessage)) {
+            size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
+            if (bytesConsumed > 0) {
+                active = true;
+                bytesAvailable -= bytesConsumed;
+                packetTail += bytesConsumed;
+                outputPacketLen += bytesConsumed;
+            } else {
+                messageQueued = true;
+                break;
+            }
+        }
+    }
+    
+    return active;
+}
+
+/* Attempt to transmit the output queue */
+bool USBSLCAN::flush() {
+    bool active = false;
+    if (outputPacketLen > 0) {
+        bool sent = stream.writeBlock((uint8_t*)(outputPacketBuffer),
+                                        (uint16_t)(outputPacketLen));
+        if (sent) {
+            active = true;
+            outputPacketLen = 0;
+        }
+    }
+    return active;
+}
+
+SerialSLCAN::SerialSLCAN(Serial& stream, CAN& can)
+    : stream(stream),
+      can(can),
+      commandQueued(false),
+      commandOverflow(false),
+      inputCommandLen(0) {    
+}
+
+bool SerialSLCAN::setBaudrate(int baudrate) {
+    return (can.frequency(baudrate) == 1);
+}
+
+bool SerialSLCAN::setMode(CAN::Mode mode) {
+    return (can.mode(mode) == 1);
+}
+
+bool SerialSLCAN::transmitMessage(CANMessage& msg) {
+    return (can.write(msg) == 1);    
+}
+
+bool SerialSLCAN::getNextCANMessage(CANMessage& msg) {
+    return (can.read(msg) == 1);
+}
+
+/* Parse and execute a single SLCAN command and enqueue the response */
+bool SerialSLCAN::processCommands() {
+    bool active = false;
+    
+    // Buffer an entire command
+    while (!commandQueued && stream.readable()) {
+        char c = (char)stream.getc();
+        if (c == '\r') {
+            if (commandOverflow) {
+                // Replace with a dummy invalid command so we return an error
+                inputCommandBuffer[0] = '!';
+                inputCommandBuffer[1] = '\0';
+                inputCommandLen = 0;
+                commandOverflow = false;
+                active = true;
+            } else {
+                // Null-terminate the buffered command
+                inputCommandBuffer[inputCommandLen] = '\0';
+                inputCommandLen = 0;
+                active = true;
+            }
+            commandQueued = true;
+        } else if (c == '\n' && inputCommandLen == 0) {
+            // Ignore line feeds immediately after a carriage return
+        } else if (commandOverflow) {
+            // Swallow the rest of the command when overflow occurs
+        } else {
+            // Append to the end of the command
+            inputCommandBuffer[inputCommandLen++] = c;
+
+            if (inputCommandLen >= sizeof(inputCommandBuffer)) {
+                commandOverflow = true;
+            }
+        }
+    }
+    
+    // Process the current command if there's space to send the response
+    if (commandQueued) {
+        if (execCommand(inputCommandBuffer)) {
+            // Success
+            stream.putc('\r');
+        } else {
+            // Failure
+            stream.putc('\a');
+        }
+        commandQueued = false;
+        active = true;
+    }
+    
+    return active;
+}
+
+/* Read and enqueue as many received CAN messages as will fit */
+bool SerialSLCAN::processCANMessages() {
+    bool active = false;
+    CANMessage msg;
+    while (getNextCANMessage(msg)) {
+        char buffer[32];
+        size_t len = formatCANMessage(msg, buffer, sizeof(buffer));
+        buffer[len] = '\0';
+        stream.puts(buffer);
+        active = true;
+    }
+    
+    return active;
+}
+
+/* Attempt to transmit the output queue */
+bool SerialSLCAN::flush() {
+    return false;
+}
\ No newline at end of file