Devan Lai
/
SLCAN
SLCAN/CAN-USB implementation for mbed targets
slcan.cpp
- Committer:
- devanlai
- Date:
- 2016-06-04
- Revision:
- 0:f2565808eea5
- Child:
- 1:3644b10bce2f
File content as of revision 0:f2565808eea5:
#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; }