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;
}