Devan Lai
/
SLCAN
SLCAN/CAN-USB implementation for mbed targets
Diff: slcan.cpp
- Revision:
- 0:f2565808eea5
- Child:
- 1:3644b10bce2f
diff -r 000000000000 -r f2565808eea5 slcan.cpp --- /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