Devan Lai
SLCAN/CAN-USB implementation for mbed targets
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