slcan.cpp

00001 #include "slcan.h"
00002 
00003 // Helper methods for parsing commands
00004 static bool parse_hex_digits(const char* input, uint8_t num_digits, uint32_t* value_out) {
00005     bool success = true;
00006     uint32_t value = 0;
00007 
00008     uint8_t i;
00009     for (i=0; i < num_digits; i++) {
00010         uint32_t nibble = 0;
00011         if (input[i] >= '0' && input[i] <= '9') {
00012             nibble = 0x0 + (input[i] - '0');
00013         } else if (input[i] >= 'a' && input[i] <= 'f') {
00014             nibble = 0xA + (input[i] - 'a');
00015         } else if (input[i] >= 'A' && input[i] <= 'F') {
00016             nibble = 0xA + (input[i] - 'A');
00017         } else {
00018             success = false;
00019             break;
00020         }
00021         uint8_t offset = 4*(num_digits-i-1);
00022         value |= (nibble << offset);
00023     }
00024 
00025     if (success) {
00026         *value_out = value;
00027     }
00028 
00029     return success;
00030 }
00031 
00032 static bool parse_hex_values(const char* input, uint8_t num_values, uint8_t* values_out) {
00033     uint8_t i;
00034     for (i=0; i < num_values; i++) {
00035         uint32_t value;
00036         if (parse_hex_digits(input, 2, &value)) {
00037             values_out[i] = (uint8_t)value;
00038         } else {
00039             return false;
00040         }
00041         input += 2;
00042     }
00043 
00044     return true;
00045 }
00046 
00047 static bool parse_dec_digit(const char* input, uint8_t* value_out) {
00048     if (input[0] >= '0' && input[0] <= '9') {
00049         *value_out = 0 + (input[0] - '0');
00050         return true;
00051     } else {
00052         return false;
00053     }
00054 }
00055 
00056 static inline char format_nibble(uint8_t x) {
00057     uint8_t nibble = x & 0x0F;
00058     return (nibble < 10) ? ('0' + nibble) : ('A' + (nibble - 10));
00059 }
00060 
00061 static inline char format_digit(uint8_t d) {
00062     return '0' + d;
00063 }
00064 
00065 SLCANBase::SLCANBase() {
00066     
00067 }
00068 
00069 SLCANBase::~SLCANBase() {
00070 
00071 }
00072 
00073 uint8_t SLCANBase::getFirmwareVersion() {
00074     // firmware version in BCD
00075     return 0x10;
00076 }
00077 
00078 uint8_t SLCANBase::getHardwareVersion() {
00079     // hardware version in BCD
00080     return 0x10;
00081 }
00082 
00083 const char* SLCANBase::getSerialString() {
00084     // 4 character serial number
00085     return "C254";
00086 }
00087 
00088 bool SLCANBase::update() {
00089     bool active = false;
00090     if (processCommands()) {
00091         active = true;
00092     }
00093     if (processCANMessages()) {
00094         active = true;
00095     }
00096     
00097     if (active) {
00098         flush();
00099     }
00100     
00101     return active;
00102 }
00103 
00104 size_t SLCANBase::formattedCANMessageLength(const CANMessage& msg) {
00105     size_t len;
00106     if (msg.format == CANStandard) {
00107         len = 1 + 3 + 1 + (2 * msg.len) + 1;
00108     } else {
00109         len = 1 + 8 + 1 + (2 * msg.len) + 1;
00110     }
00111     
00112     return len;
00113 }
00114 
00115 size_t SLCANBase::formatCANMessage(const CANMessage& msg, char* buf, size_t max_len) {
00116     size_t len = formattedCANMessageLength(msg);
00117     if (len > max_len) {
00118         return 0;
00119     }
00120     
00121     if (msg.format == CANStandard) {
00122         *buf++ = (msg.type == CANData) ? 't' : 'r';
00123         *buf++ = format_nibble((uint8_t)(msg.id >> 8));
00124         *buf++ = format_nibble((uint8_t)(msg.id >> 4));
00125         *buf++ = format_nibble((uint8_t)(msg.id >> 0));
00126         *buf++ = format_digit(msg.len);
00127     } else {
00128         *buf++ = (msg.type == CANData) ? 'T' : 'R';
00129         *buf++ = format_nibble((uint8_t)(msg.id >> 28));
00130         *buf++ = format_nibble((uint8_t)(msg.id >> 24));
00131         *buf++ = format_nibble((uint8_t)(msg.id >> 20));
00132         *buf++ = format_nibble((uint8_t)(msg.id >> 16));
00133         *buf++ = format_nibble((uint8_t)(msg.id >> 12));
00134         *buf++ = format_nibble((uint8_t)(msg.id >>  8));
00135         *buf++ = format_nibble((uint8_t)(msg.id >>  4));
00136         *buf++ = format_nibble((uint8_t)(msg.id >>  0));
00137         *buf++ = format_digit(msg.len);
00138     }
00139     
00140     for (unsigned char i=0; i < msg.len; i++) {
00141         *buf++ = format_nibble((uint8_t)(msg.data[i] >> 4));
00142         *buf++ = format_nibble((uint8_t)(msg.data[i] >> 0));
00143     }
00144     
00145     *buf++ = '\r';
00146     
00147     return len;
00148 }
00149 
00150 size_t SLCANBase::commandResponseLength(const char* command) {
00151     switch (command[0]) {
00152         case 'N':
00153         case 'V': {
00154             return 6;
00155         }
00156         case 'v':
00157         case 'F': {
00158             return 4;
00159         }
00160         case 'T':
00161         case 't':
00162         case 'R':
00163         case 'r': {
00164             return 2;
00165         }
00166         default: {
00167             return 1;
00168         }
00169     }
00170 }
00171 
00172 bool SLCANBase::execCommand(const char* command, char* response) {
00173     bool success = false;
00174     if (response) {
00175         response[0] = '\0';
00176     }
00177 
00178     switch (command[0]) {
00179         // Configuration commands
00180         case 'S':
00181         case 's':
00182         case 'O':
00183         case 'L':
00184         case 'l':
00185         case 'C':
00186         case 'Z':
00187         case 'M':
00188         case 'm': {
00189             success = execConfigCommand(command);
00190             break;
00191         }
00192         // Transmission commands
00193         case 't':
00194         case 'T':
00195         case 'r':
00196         case 'R': {
00197             success = execTransmitCommand(command, response);
00198             break;
00199         }
00200         // Diagnostic commands
00201         case 'V':
00202         case 'v':
00203         case 'N':
00204         case 'W':
00205         case 'F':
00206             success = execDiagnosticCommand(command, response);
00207             break;
00208         default: {
00209             success = false;
00210             break;
00211         }
00212     }
00213 
00214     return success;
00215 }
00216 
00217 bool SLCANBase::execConfigCommand(const char* command) {
00218     bool success = false;
00219     size_t len = strlen(command);
00220     
00221     // Validate command length
00222     if (command[0] == 'M' || command[0] == 'm') {
00223         if (len != 9) {
00224             return false;
00225         }
00226     } else if (command[0] == 's') {
00227         if (!((len == 5) || (len == 7))) {
00228             return false;
00229         }
00230     } else if (command[0] == 'S' || command[0] == 'Z') {
00231         if (len != 2) {
00232             return false;
00233         }
00234     } else if (len != 1) {
00235         return false;
00236     }
00237 
00238     switch (command[0]) {
00239         case 'S': {
00240             bool known = true;
00241             int baudrate;
00242             switch (command[1]) {
00243                 case '0': baudrate =   10000; break;
00244                 case '1': baudrate =   20000; break;
00245                 case '2': baudrate =   50000; break;
00246                 case '3': baudrate =  100000; break;
00247                 case '4': baudrate =  125000; break;
00248                 case '5': baudrate =  250000; break;
00249                 case '6': baudrate =  500000; break;
00250                 case '7': baudrate =  800000; break;
00251                 case '8': baudrate = 1000000; break;
00252                 default:  known = false; break;
00253             }
00254 
00255             if (known) {
00256                 success = setBaudrate(baudrate);
00257             }
00258 
00259             break;
00260         }
00261         case 'O': {
00262             success = setMode(CAN::Normal);
00263             break;
00264         }
00265         case 'L': {
00266             success = setMode(CAN::Silent);
00267             break;
00268         }
00269         case 'l': {
00270             success = setMode(CAN::SilentTest);
00271             break;
00272         }
00273         case 'C': {
00274             success = setMode(CAN::Reset);
00275             break;
00276         }
00277         case 's': {
00278             // TODO: implement direct BTR control
00279             success = true;
00280             break;
00281         }
00282         case 'M':
00283         case 'm': {
00284             // TODO: implement filtering
00285             success = true;
00286             break;
00287         }
00288         case 'Z': {
00289             // TODO: implement timestamping
00290             success = true;
00291             break;
00292         }
00293         default: {
00294             success = false;
00295             break;
00296         }
00297     }
00298 
00299     return success;
00300 }
00301 
00302 bool SLCANBase::execTransmitCommand(const char* command, char* response) {
00303     bool success = false;
00304 
00305     size_t len = strlen(command);
00306 
00307     bool validMessage = false;
00308     CANMessage msg;
00309 
00310     if (command[0] == 't' || command[0] == 'T') {
00311         msg.type = CANData;
00312         msg.format = (command[0] == 't') ? CANStandard : CANExtended;
00313         size_t idLen = msg.format == CANStandard ? 3 : 8;
00314         if ((len >= idLen + 2) &&
00315             parse_hex_digits(&command[1], idLen, (uint32_t*)&msg.id) &&
00316             parse_dec_digit(&command[idLen + 1], &msg.len)) {
00317             if ((len == idLen + 2 + 2*msg.len) &&
00318                 (msg.len <= 8) &&
00319                 parse_hex_values(&command[idLen + 2], msg.len, msg.data)) {
00320                 validMessage = true;
00321             }
00322         }
00323     } else if (command[0] == 'r' || command[0] == 'R') {
00324         msg.type = CANRemote;
00325         msg.format = (command[0] == 'r') ? CANStandard : CANExtended;
00326         size_t idLen = msg.format == CANStandard ? 3 : 8;
00327         if ((len == idLen + 2) &&
00328             parse_hex_digits(&command[1], idLen, (uint32_t*)(&msg.id)) &&
00329             parse_dec_digit(&command[idLen + 1], &msg.len)) {
00330             if (msg.len <= 8) {
00331                 validMessage = true;
00332             }
00333         }
00334     }
00335     
00336     if (validMessage) {
00337         if (command[0] == 'T' || command[0] == 'R') {
00338             response[0] = 'Z';
00339             response[1] = '\0';
00340         } else if (command[0] == 't' || command[0] == 'r') {
00341             response[0] = 'z';
00342             response[1] = '\0';
00343         }
00344         success = transmitMessage(msg);
00345     }
00346 
00347     return success;
00348 }
00349 
00350 bool SLCANBase::execDiagnosticCommand(const char* command, char* response) {
00351     bool success = false;
00352     size_t len = strlen(command);
00353     
00354     // Validate command length
00355     if (command[0] == 'W') {
00356         if (len != 5) {
00357             return false;
00358         }
00359     } else if (len != 1) {
00360         return false;
00361     }
00362     
00363     if (!response) {
00364         return false;
00365     }
00366 
00367     switch (command[0]) {
00368         case 'V': {
00369             success = true;
00370             uint8_t hwVersion = getHardwareVersion();
00371             uint8_t fwVersion = getFirmwareVersion();
00372             
00373             response[0] = 'V';
00374             response[1] = format_nibble(hwVersion >> 4);
00375             response[2] = format_nibble(hwVersion >> 0);
00376             response[3] = format_nibble(fwVersion >> 4);
00377             response[4] = format_nibble(fwVersion >> 0);
00378             response[5] = '\0';
00379             break;
00380         }
00381         case 'v': {
00382             success = true;
00383             uint8_t fwVersion = getFirmwareVersion();
00384             response[0] = 'v';
00385             response[1] = format_nibble(fwVersion >> 4);
00386             response[2] = format_nibble(fwVersion >> 0);
00387             response[3] = '\0';
00388             break;
00389         }
00390         case 'N': {
00391             success = true;
00392             const char* serial = getSerialString();
00393             size_t index = 0;
00394             response[index++] = 'N';
00395             for (int i=0; i < 4; i++) {
00396                 char c = serial[i];
00397                 if (c == '\0') {
00398                     break;
00399                 } else {
00400                     response[index++] = c;
00401                 }
00402             }
00403             response[index] = '\0';
00404             break;
00405         }
00406         case 'F': {
00407             success = true;
00408             uint8_t status = 0x00;
00409             response[0] = 'F';
00410             response[1] = format_nibble(status >> 4);
00411             response[2] = format_nibble(status >> 0);
00412             response[3] = '\0';
00413         }
00414         case 'W': {
00415             // Just swallow the MCP2515 register write command
00416             success = true;
00417             break;
00418         }
00419         default: {
00420             success = false;
00421             break;
00422         }
00423     }
00424     
00425     return success;
00426 }
00427 
00428 USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
00429     : stream(stream),
00430       can(can),
00431       messageQueued(false),
00432       commandQueued(false),
00433       commandOverflow(false),
00434       inputCommandLen(0),
00435       outputPacketLen(0) {
00436     
00437 }
00438 
00439 bool USBSLCAN::setBaudrate(int baudrate) {
00440     return (can.frequency(baudrate) == 1);
00441 }
00442 
00443 bool USBSLCAN::setMode(CAN::Mode mode) {
00444     return (can.mode(mode) == 1);
00445 }
00446 
00447 bool USBSLCAN::transmitMessage(CANMessage& msg) {
00448     return (can.write(msg) == 1);
00449 }
00450 
00451 bool USBSLCAN::getNextCANMessage(CANMessage& msg) {
00452     return (can.read(msg) == 1);
00453 }
00454 
00455 /* Parse and execute a single SLCAN command and enqueue the response */
00456 bool USBSLCAN::processCommands() {
00457     bool active = false;
00458     
00459     // Buffer an entire command
00460     while (!commandQueued && stream.readable()) {
00461         char c = (char)stream.getc();
00462         if (c == '\r') {
00463             if (commandOverflow) {
00464                 // Replace with a dummy invalid command so we return an error
00465                 inputCommandBuffer[0] = '!';
00466                 inputCommandBuffer[1] = '\0';
00467                 inputCommandLen = 0;
00468                 commandOverflow = false;
00469                 active = true;
00470             } else {
00471                 // Null-terminate the buffered command
00472                 inputCommandBuffer[inputCommandLen] = '\0';
00473                 inputCommandLen = 0;
00474                 active = true;
00475             }
00476             commandQueued = true;
00477         } else if (c == '\n' && inputCommandLen == 0) {
00478             // Ignore line feeds immediately after a carriage return
00479         } else if (commandOverflow) {
00480             // Swallow the rest of the command when overflow occurs
00481         } else {
00482             // Append to the end of the command
00483             inputCommandBuffer[inputCommandLen++] = c;
00484 
00485             if (inputCommandLen >= sizeof(inputCommandBuffer)) {
00486                 commandOverflow = true;
00487             }
00488         }
00489     }
00490     
00491     // Process the current command if there's space to send the response
00492     if (commandQueued) {
00493         size_t responseLength = commandResponseLength(inputCommandBuffer);
00494         if ((outputPacketLen + responseLength) <= sizeof(outputPacketBuffer)) {
00495             char outputResponseBuffer[32];
00496             outputResponseBuffer[0] = '\0';
00497             if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
00498                 // Success
00499                 for (char* s = outputResponseBuffer; *s != '\0'; s++) {
00500                     outputPacketBuffer[outputPacketLen++] = *s;
00501                 }
00502                 outputPacketBuffer[outputPacketLen++] = '\r';
00503             } else {
00504                 // Failure
00505                 outputPacketBuffer[outputPacketLen++] = '\a';
00506             }
00507             commandQueued = false;
00508             active = true;
00509         }
00510     }
00511     
00512     return active;
00513 }
00514 
00515 /* Read and enqueue as many received CAN messages as will fit */
00516 bool USBSLCAN::processCANMessages() {
00517     bool active = false;
00518     
00519     size_t bytesAvailable = sizeof(outputPacketBuffer) - outputPacketLen;
00520     char* packetTail = &outputPacketBuffer[outputPacketLen];
00521     
00522     if (messageQueued) {
00523         size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
00524         if (bytesConsumed > 0) {
00525             active = true;
00526             messageQueued = false;
00527             bytesAvailable -= bytesConsumed;
00528             packetTail += bytesConsumed;
00529             outputPacketLen += bytesConsumed;
00530         }
00531     }
00532     
00533     if (!messageQueued) {
00534         while (getNextCANMessage(queuedMessage)) {
00535             size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
00536             if (bytesConsumed > 0) {
00537                 active = true;
00538                 bytesAvailable -= bytesConsumed;
00539                 packetTail += bytesConsumed;
00540                 outputPacketLen += bytesConsumed;
00541             } else {
00542                 messageQueued = true;
00543                 break;
00544             }
00545         }
00546     }
00547     
00548     return active;
00549 }
00550 
00551 /* Attempt to transmit the output queue */
00552 bool USBSLCAN::flush() {
00553     bool active = false;
00554     if (outputPacketLen > 0) {
00555         bool sent = stream.writeBlock((uint8_t*)(outputPacketBuffer),
00556                                         (uint16_t)(outputPacketLen));
00557         if (sent) {
00558             active = true;
00559             outputPacketLen = 0;
00560         }
00561     }
00562     return active;
00563 }
00564 
00565 SerialSLCAN::SerialSLCAN(Serial& stream, CAN& can)
00566     : stream(stream),
00567       can(can),
00568       commandQueued(false),
00569       commandOverflow(false),
00570       inputCommandLen(0) {    
00571 }
00572 
00573 bool SerialSLCAN::setBaudrate(int baudrate) {
00574     return (can.frequency(baudrate) == 1);
00575 }
00576 
00577 bool SerialSLCAN::setMode(CAN::Mode mode) {
00578     return (can.mode(mode) == 1);
00579 }
00580 
00581 bool SerialSLCAN::transmitMessage(CANMessage& msg) {
00582     return (can.write(msg) == 1);    
00583 }
00584 
00585 bool SerialSLCAN::getNextCANMessage(CANMessage& msg) {
00586     return (can.read(msg) == 1);
00587 }
00588 
00589 /* Parse and execute a single SLCAN command and enqueue the response */
00590 bool SerialSLCAN::processCommands() {
00591     bool active = false;
00592     
00593     // Buffer an entire command
00594     while (!commandQueued && stream.readable()) {
00595         char c = (char)stream.getc();
00596         if (c == '\r') {
00597             if (commandOverflow) {
00598                 // Replace with a dummy invalid command so we return an error
00599                 inputCommandBuffer[0] = '!';
00600                 inputCommandBuffer[1] = '\0';
00601                 inputCommandLen = 0;
00602                 commandOverflow = false;
00603                 active = true;
00604             } else {
00605                 // Null-terminate the buffered command
00606                 inputCommandBuffer[inputCommandLen] = '\0';
00607                 inputCommandLen = 0;
00608                 active = true;
00609             }
00610             commandQueued = true;
00611         } else if (c == '\n' && inputCommandLen == 0) {
00612             // Ignore line feeds immediately after a carriage return
00613         } else if (commandOverflow) {
00614             // Swallow the rest of the command when overflow occurs
00615         } else {
00616             // Append to the end of the command
00617             inputCommandBuffer[inputCommandLen++] = c;
00618 
00619             if (inputCommandLen >= sizeof(inputCommandBuffer)) {
00620                 commandOverflow = true;
00621             }
00622         }
00623     }
00624     
00625     // Process the current command
00626     if (commandQueued) {
00627         char outputResponseBuffer[32];
00628         outputResponseBuffer[0] = '\0';
00629         if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
00630             // Success
00631             stream.puts(outputResponseBuffer);
00632             stream.putc('\r');
00633         } else {
00634             // Failure
00635             stream.putc('\a');
00636         }
00637         commandQueued = false;
00638         active = true;
00639     }
00640     
00641     return active;
00642 }
00643 
00644 /* Read and enqueue as many received CAN messages as will fit */
00645 bool SerialSLCAN::processCANMessages() {
00646     bool active = false;
00647     CANMessage msg;
00648     while (getNextCANMessage(msg)) {
00649         char buffer[32];
00650         size_t len = formatCANMessage(msg, buffer, sizeof(buffer));
00651         buffer[len] = '\0';
00652         stream.puts(buffer);
00653         active = true;
00654     }
00655     
00656     return active;
00657 }
00658 
00659 /* Attempt to transmit the output queue */
00660 bool SerialSLCAN::flush() {
00661     return false;
00662 }