Devan Lai / Mbed 2 deprecated SLCAN

SLCAN/CAN-USB implementation for mbed targets

Dependencies:   USBDevice mbed

slcan.cpp@3:bc163d555ddc, 2017-02-04 (annotated)

Committer:
devanlai
Date:
Sat Feb 04 09:49:57 2017 +0000
Revision:
3:bc163d555ddc
Parent:
2:1327e61cc56b 
Send z/Z response as expected for T/t/R/r commands.; Add minimal support for F/M/m/s commands for compatibility

Who changed what in which revision?

UserRevisionLine numberNew contents of line
devanlai 0:f2565808eea5 1 #include "slcan.h"
devanlai 0:f2565808eea5 2
devanlai 0:f2565808eea5 3 // Helper methods for parsing commands
devanlai 0:f2565808eea5 4 static bool parse_hex_digits(const char* input, uint8_t num_digits, uint32_t* value_out) {
devanlai 0:f2565808eea5 5 bool success = true;
devanlai 0:f2565808eea5 6 uint32_t value = 0;
devanlai 0:f2565808eea5 7
devanlai 0:f2565808eea5 8 uint8_t i;
devanlai 0:f2565808eea5 9 for (i=0; i < num_digits; i++) {
devanlai 0:f2565808eea5 10 uint32_t nibble = 0;
devanlai 0:f2565808eea5 11 if (input[i] >= '0' && input[i] <= '9') {
devanlai 0:f2565808eea5 12 nibble = 0x0 + (input[i] - '0');
devanlai 0:f2565808eea5 13 } else if (input[i] >= 'a' && input[i] <= 'f') {
devanlai 0:f2565808eea5 14 nibble = 0xA + (input[i] - 'a');
devanlai 0:f2565808eea5 15 } else if (input[i] >= 'A' && input[i] <= 'F') {
devanlai 0:f2565808eea5 16 nibble = 0xA + (input[i] - 'A');
devanlai 0:f2565808eea5 17 } else {
devanlai 0:f2565808eea5 18 success = false;
devanlai 0:f2565808eea5 19 break;
devanlai 0:f2565808eea5 20 }
devanlai 0:f2565808eea5 21 uint8_t offset = 4*(num_digits-i-1);
devanlai 0:f2565808eea5 22 value |= (nibble << offset);
devanlai 0:f2565808eea5 23 }
devanlai 0:f2565808eea5 24
devanlai 0:f2565808eea5 25 if (success) {
devanlai 0:f2565808eea5 26 *value_out = value;
devanlai 0:f2565808eea5 27 }
devanlai 0:f2565808eea5 28
devanlai 0:f2565808eea5 29 return success;
devanlai 0:f2565808eea5 30 }
devanlai 0:f2565808eea5 31
devanlai 0:f2565808eea5 32 static bool parse_hex_values(const char* input, uint8_t num_values, uint8_t* values_out) {
devanlai 0:f2565808eea5 33 uint8_t i;
devanlai 0:f2565808eea5 34 for (i=0; i < num_values; i++) {
devanlai 0:f2565808eea5 35 uint32_t value;
devanlai 0:f2565808eea5 36 if (parse_hex_digits(input, 2, &value)) {
devanlai 0:f2565808eea5 37 values_out[i] = (uint8_t)value;
devanlai 0:f2565808eea5 38 } else {
devanlai 0:f2565808eea5 39 return false;
devanlai 0:f2565808eea5 40 }
devanlai 0:f2565808eea5 41 input += 2;
devanlai 0:f2565808eea5 42 }
devanlai 0:f2565808eea5 43
devanlai 0:f2565808eea5 44 return true;
devanlai 0:f2565808eea5 45 }
devanlai 0:f2565808eea5 46
devanlai 0:f2565808eea5 47 static bool parse_dec_digit(const char* input, uint8_t* value_out) {
devanlai 0:f2565808eea5 48 if (input[0] >= '0' && input[0] <= '9') {
devanlai 0:f2565808eea5 49 *value_out = 0 + (input[0] - '0');
devanlai 0:f2565808eea5 50 return true;
devanlai 0:f2565808eea5 51 } else {
devanlai 0:f2565808eea5 52 return false;
devanlai 0:f2565808eea5 53 }
devanlai 0:f2565808eea5 54 }
devanlai 0:f2565808eea5 55
devanlai 0:f2565808eea5 56 static inline char format_nibble(uint8_t x) {
devanlai 0:f2565808eea5 57 uint8_t nibble = x & 0x0F;
devanlai 0:f2565808eea5 58 return (nibble < 10) ? ('0' + nibble) : ('A' + (nibble - 10));
devanlai 0:f2565808eea5 59 }
devanlai 0:f2565808eea5 60
devanlai 0:f2565808eea5 61 static inline char format_digit(uint8_t d) {
devanlai 0:f2565808eea5 62 return '0' + d;
devanlai 0:f2565808eea5 63 }
devanlai 0:f2565808eea5 64
devanlai 0:f2565808eea5 65 SLCANBase::SLCANBase() {
devanlai 0:f2565808eea5 66
devanlai 0:f2565808eea5 67 }
devanlai 0:f2565808eea5 68
devanlai 0:f2565808eea5 69 SLCANBase::~SLCANBase() {
devanlai 0:f2565808eea5 70
devanlai 0:f2565808eea5 71 }
devanlai 0:f2565808eea5 72
devanlai 2:1327e61cc56b 73 uint8_t SLCANBase::getFirmwareVersion() {
devanlai 2:1327e61cc56b 74 // firmware version in BCD
devanlai 2:1327e61cc56b 75 return 0x10;
devanlai 2:1327e61cc56b 76 }
devanlai 2:1327e61cc56b 77
devanlai 2:1327e61cc56b 78 uint8_t SLCANBase::getHardwareVersion() {
devanlai 2:1327e61cc56b 79 // hardware version in BCD
devanlai 2:1327e61cc56b 80 return 0x10;
devanlai 2:1327e61cc56b 81 }
devanlai 2:1327e61cc56b 82
devanlai 2:1327e61cc56b 83 const char* SLCANBase::getSerialString() {
devanlai 2:1327e61cc56b 84 // 4 character serial number
devanlai 2:1327e61cc56b 85 return "C254";
devanlai 2:1327e61cc56b 86 }
devanlai 2:1327e61cc56b 87
devanlai 0:f2565808eea5 88 bool SLCANBase::update() {
devanlai 0:f2565808eea5 89 bool active = false;
devanlai 0:f2565808eea5 90 if (processCommands()) {
devanlai 0:f2565808eea5 91 active = true;
devanlai 0:f2565808eea5 92 }
devanlai 0:f2565808eea5 93 if (processCANMessages()) {
devanlai 0:f2565808eea5 94 active = true;
devanlai 0:f2565808eea5 95 }
devanlai 0:f2565808eea5 96
devanlai 0:f2565808eea5 97 if (active) {
devanlai 0:f2565808eea5 98 flush();
devanlai 0:f2565808eea5 99 }
devanlai 0:f2565808eea5 100
devanlai 0:f2565808eea5 101 return active;
devanlai 0:f2565808eea5 102 }
devanlai 0:f2565808eea5 103
devanlai 0:f2565808eea5 104 size_t SLCANBase::formattedCANMessageLength(const CANMessage& msg) {
devanlai 0:f2565808eea5 105 size_t len;
devanlai 0:f2565808eea5 106 if (msg.format == CANStandard) {
devanlai 0:f2565808eea5 107 len = 1 + 3 + 1 + (2 * msg.len) + 1;
devanlai 0:f2565808eea5 108 } else {
devanlai 0:f2565808eea5 109 len = 1 + 8 + 1 + (2 * msg.len) + 1;
devanlai 0:f2565808eea5 110 }
devanlai 0:f2565808eea5 111
devanlai 0:f2565808eea5 112 return len;
devanlai 0:f2565808eea5 113 }
devanlai 0:f2565808eea5 114
devanlai 0:f2565808eea5 115 size_t SLCANBase::formatCANMessage(const CANMessage& msg, char* buf, size_t max_len) {
devanlai 0:f2565808eea5 116 size_t len = formattedCANMessageLength(msg);
devanlai 0:f2565808eea5 117 if (len > max_len) {
devanlai 0:f2565808eea5 118 return 0;
devanlai 0:f2565808eea5 119 }
devanlai 0:f2565808eea5 120
devanlai 0:f2565808eea5 121 if (msg.format == CANStandard) {
devanlai 0:f2565808eea5 122 *buf++ = (msg.type == CANData) ? 't' : 'r';
devanlai 0:f2565808eea5 123 *buf++ = format_nibble((uint8_t)(msg.id >> 8));
devanlai 0:f2565808eea5 124 *buf++ = format_nibble((uint8_t)(msg.id >> 4));
devanlai 0:f2565808eea5 125 *buf++ = format_nibble((uint8_t)(msg.id >> 0));
devanlai 0:f2565808eea5 126 *buf++ = format_digit(msg.len);
devanlai 0:f2565808eea5 127 } else {
devanlai 0:f2565808eea5 128 *buf++ = (msg.type == CANData) ? 'T' : 'R';
devanlai 0:f2565808eea5 129 *buf++ = format_nibble((uint8_t)(msg.id >> 28));
devanlai 0:f2565808eea5 130 *buf++ = format_nibble((uint8_t)(msg.id >> 24));
devanlai 0:f2565808eea5 131 *buf++ = format_nibble((uint8_t)(msg.id >> 20));
devanlai 0:f2565808eea5 132 *buf++ = format_nibble((uint8_t)(msg.id >> 16));
devanlai 0:f2565808eea5 133 *buf++ = format_nibble((uint8_t)(msg.id >> 12));
devanlai 0:f2565808eea5 134 *buf++ = format_nibble((uint8_t)(msg.id >> 8));
devanlai 0:f2565808eea5 135 *buf++ = format_nibble((uint8_t)(msg.id >> 4));
devanlai 0:f2565808eea5 136 *buf++ = format_nibble((uint8_t)(msg.id >> 0));
devanlai 0:f2565808eea5 137 *buf++ = format_digit(msg.len);
devanlai 0:f2565808eea5 138 }
devanlai 0:f2565808eea5 139
devanlai 0:f2565808eea5 140 for (unsigned char i=0; i < msg.len; i++) {
devanlai 0:f2565808eea5 141 *buf++ = format_nibble((uint8_t)(msg.data[i] >> 4));
devanlai 0:f2565808eea5 142 *buf++ = format_nibble((uint8_t)(msg.data[i] >> 0));
devanlai 0:f2565808eea5 143 }
devanlai 0:f2565808eea5 144
devanlai 0:f2565808eea5 145 *buf++ = '\r';
devanlai 0:f2565808eea5 146
devanlai 0:f2565808eea5 147 return len;
devanlai 0:f2565808eea5 148 }
devanlai 0:f2565808eea5 149
devanlai 2:1327e61cc56b 150 size_t SLCANBase::commandResponseLength(const char* command) {
devanlai 3:bc163d555ddc 151 switch (command[0]) {
devanlai 3:bc163d555ddc 152 case 'N':
devanlai 3:bc163d555ddc 153 case 'V': {
devanlai 3:bc163d555ddc 154 return 6;
devanlai 3:bc163d555ddc 155 }
devanlai 3:bc163d555ddc 156 case 'v':
devanlai 3:bc163d555ddc 157 case 'F': {
devanlai 3:bc163d555ddc 158 return 4;
devanlai 3:bc163d555ddc 159 }
devanlai 3:bc163d555ddc 160 case 'T':
devanlai 3:bc163d555ddc 161 case 't':
devanlai 3:bc163d555ddc 162 case 'R':
devanlai 3:bc163d555ddc 163 case 'r': {
devanlai 3:bc163d555ddc 164 return 2;
devanlai 3:bc163d555ddc 165 }
devanlai 3:bc163d555ddc 166 default: {
devanlai 3:bc163d555ddc 167 return 1;
devanlai 3:bc163d555ddc 168 }
devanlai 2:1327e61cc56b 169 }
devanlai 2:1327e61cc56b 170 }
devanlai 2:1327e61cc56b 171
devanlai 2:1327e61cc56b 172 bool SLCANBase::execCommand(const char* command, char* response) {
devanlai 0:f2565808eea5 173 bool success = false;
devanlai 2:1327e61cc56b 174 if (response) {
devanlai 2:1327e61cc56b 175 response[0] = '\0';
devanlai 2:1327e61cc56b 176 }
devanlai 0:f2565808eea5 177
devanlai 0:f2565808eea5 178 switch (command[0]) {
devanlai 0:f2565808eea5 179 // Configuration commands
devanlai 0:f2565808eea5 180 case 'S':
devanlai 3:bc163d555ddc 181 case 's':
devanlai 0:f2565808eea5 182 case 'O':
devanlai 0:f2565808eea5 183 case 'L':
devanlai 0:f2565808eea5 184 case 'l':
devanlai 3:bc163d555ddc 185 case 'C':
devanlai 3:bc163d555ddc 186 case 'Z':
devanlai 3:bc163d555ddc 187 case 'M':
devanlai 3:bc163d555ddc 188 case 'm': {
devanlai 0:f2565808eea5 189 success = execConfigCommand(command);
devanlai 0:f2565808eea5 190 break;
devanlai 0:f2565808eea5 191 }
devanlai 0:f2565808eea5 192 // Transmission commands
devanlai 0:f2565808eea5 193 case 't':
devanlai 0:f2565808eea5 194 case 'T':
devanlai 0:f2565808eea5 195 case 'r':
devanlai 0:f2565808eea5 196 case 'R': {
devanlai 3:bc163d555ddc 197 success = execTransmitCommand(command, response);
devanlai 0:f2565808eea5 198 break;
devanlai 0:f2565808eea5 199 }
devanlai 2:1327e61cc56b 200 // Diagnostic commands
devanlai 2:1327e61cc56b 201 case 'V':
devanlai 2:1327e61cc56b 202 case 'v':
devanlai 2:1327e61cc56b 203 case 'N':
devanlai 2:1327e61cc56b 204 case 'W':
devanlai 3:bc163d555ddc 205 case 'F':
devanlai 2:1327e61cc56b 206 success = execDiagnosticCommand(command, response);
devanlai 2:1327e61cc56b 207 break;
devanlai 0:f2565808eea5 208 default: {
devanlai 0:f2565808eea5 209 success = false;
devanlai 0:f2565808eea5 210 break;
devanlai 0:f2565808eea5 211 }
devanlai 0:f2565808eea5 212 }
devanlai 0:f2565808eea5 213
devanlai 0:f2565808eea5 214 return success;
devanlai 0:f2565808eea5 215 }
devanlai 0:f2565808eea5 216
devanlai 0:f2565808eea5 217 bool SLCANBase::execConfigCommand(const char* command) {
devanlai 0:f2565808eea5 218 bool success = false;
devanlai 0:f2565808eea5 219 size_t len = strlen(command);
devanlai 0:f2565808eea5 220
devanlai 0:f2565808eea5 221 // Validate command length
devanlai 3:bc163d555ddc 222 if (command[0] == 'M' || command[0] == 'm') {
devanlai 3:bc163d555ddc 223 if (len != 9) {
devanlai 3:bc163d555ddc 224 return false;
devanlai 3:bc163d555ddc 225 }
devanlai 3:bc163d555ddc 226 } else if (command[0] == 's') {
devanlai 3:bc163d555ddc 227 if (!((len == 5) || (len == 7))) {
devanlai 3:bc163d555ddc 228 return false;
devanlai 3:bc163d555ddc 229 }
devanlai 3:bc163d555ddc 230 } else if (command[0] == 'S' || command[0] == 'Z') {
devanlai 3:bc163d555ddc 231 if (len != 2) {
devanlai 3:bc163d555ddc 232 return false;
devanlai 3:bc163d555ddc 233 }
devanlai 3:bc163d555ddc 234 } else if (len != 1) {
devanlai 0:f2565808eea5 235 return false;
devanlai 0:f2565808eea5 236 }
devanlai 0:f2565808eea5 237
devanlai 0:f2565808eea5 238 switch (command[0]) {
devanlai 0:f2565808eea5 239 case 'S': {
devanlai 0:f2565808eea5 240 bool known = true;
devanlai 0:f2565808eea5 241 int baudrate;
devanlai 0:f2565808eea5 242 switch (command[1]) {
devanlai 0:f2565808eea5 243 case '0': baudrate = 10000; break;
devanlai 0:f2565808eea5 244 case '1': baudrate = 20000; break;
devanlai 0:f2565808eea5 245 case '2': baudrate = 50000; break;
devanlai 0:f2565808eea5 246 case '3': baudrate = 100000; break;
devanlai 0:f2565808eea5 247 case '4': baudrate = 125000; break;
devanlai 0:f2565808eea5 248 case '5': baudrate = 250000; break;
devanlai 0:f2565808eea5 249 case '6': baudrate = 500000; break;
devanlai 0:f2565808eea5 250 case '7': baudrate = 800000; break;
devanlai 0:f2565808eea5 251 case '8': baudrate = 1000000; break;
devanlai 0:f2565808eea5 252 default: known = false; break;
devanlai 0:f2565808eea5 253 }
devanlai 0:f2565808eea5 254
devanlai 0:f2565808eea5 255 if (known) {
devanlai 0:f2565808eea5 256 success = setBaudrate(baudrate);
devanlai 0:f2565808eea5 257 }
devanlai 0:f2565808eea5 258
devanlai 0:f2565808eea5 259 break;
devanlai 0:f2565808eea5 260 }
devanlai 0:f2565808eea5 261 case 'O': {
devanlai 0:f2565808eea5 262 success = setMode(CAN::Normal);
devanlai 0:f2565808eea5 263 break;
devanlai 0:f2565808eea5 264 }
devanlai 0:f2565808eea5 265 case 'L': {
devanlai 0:f2565808eea5 266 success = setMode(CAN::Silent);
devanlai 0:f2565808eea5 267 break;
devanlai 0:f2565808eea5 268 }
devanlai 0:f2565808eea5 269 case 'l': {
devanlai 0:f2565808eea5 270 success = setMode(CAN::SilentTest);
devanlai 0:f2565808eea5 271 break;
devanlai 0:f2565808eea5 272 }
devanlai 0:f2565808eea5 273 case 'C': {
devanlai 0:f2565808eea5 274 success = setMode(CAN::Reset);
devanlai 0:f2565808eea5 275 break;
devanlai 0:f2565808eea5 276 }
devanlai 3:bc163d555ddc 277 case 's': {
devanlai 3:bc163d555ddc 278 // TODO: implement direct BTR control
devanlai 3:bc163d555ddc 279 success = true;
devanlai 3:bc163d555ddc 280 break;
devanlai 3:bc163d555ddc 281 }
devanlai 3:bc163d555ddc 282 case 'M':
devanlai 3:bc163d555ddc 283 case 'm': {
devanlai 3:bc163d555ddc 284 // TODO: implement filtering
devanlai 3:bc163d555ddc 285 success = true;
devanlai 3:bc163d555ddc 286 break;
devanlai 3:bc163d555ddc 287 }
devanlai 3:bc163d555ddc 288 case 'Z': {
devanlai 3:bc163d555ddc 289 // TODO: implement timestamping
devanlai 3:bc163d555ddc 290 success = true;
devanlai 3:bc163d555ddc 291 break;
devanlai 3:bc163d555ddc 292 }
devanlai 0:f2565808eea5 293 default: {
devanlai 0:f2565808eea5 294 success = false;
devanlai 0:f2565808eea5 295 break;
devanlai 0:f2565808eea5 296 }
devanlai 0:f2565808eea5 297 }
devanlai 0:f2565808eea5 298
devanlai 0:f2565808eea5 299 return success;
devanlai 0:f2565808eea5 300 }
devanlai 0:f2565808eea5 301
devanlai 3:bc163d555ddc 302 bool SLCANBase::execTransmitCommand(const char* command, char* response) {
devanlai 0:f2565808eea5 303 bool success = false;
devanlai 0:f2565808eea5 304
devanlai 0:f2565808eea5 305 size_t len = strlen(command);
devanlai 0:f2565808eea5 306
devanlai 0:f2565808eea5 307 bool validMessage = false;
devanlai 0:f2565808eea5 308 CANMessage msg;
devanlai 3:bc163d555ddc 309
devanlai 0:f2565808eea5 310 if (command[0] == 't' || command[0] == 'T') {
devanlai 0:f2565808eea5 311 msg.type = CANData;
devanlai 0:f2565808eea5 312 msg.format = (command[0] == 't') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 313 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 314 if ((len >= idLen + 2) &&
devanlai 0:f2565808eea5 315 parse_hex_digits(&command[1], idLen, (uint32_t*)&msg.id) &&
devanlai 0:f2565808eea5 316 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 317 if ((len == idLen + 2 + 2*msg.len) &&
devanlai 0:f2565808eea5 318 (msg.len <= 8) &&
devanlai 0:f2565808eea5 319 parse_hex_values(&command[idLen + 2], msg.len, msg.data)) {
devanlai 0:f2565808eea5 320 validMessage = true;
devanlai 0:f2565808eea5 321 }
devanlai 0:f2565808eea5 322 }
devanlai 0:f2565808eea5 323 } else if (command[0] == 'r' || command[0] == 'R') {
devanlai 0:f2565808eea5 324 msg.type = CANRemote;
devanlai 0:f2565808eea5 325 msg.format = (command[0] == 'r') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 326 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 327 if ((len == idLen + 2) &&
devanlai 0:f2565808eea5 328 parse_hex_digits(&command[1], idLen, (uint32_t*)(&msg.id)) &&
devanlai 0:f2565808eea5 329 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 330 if (msg.len <= 8) {
devanlai 0:f2565808eea5 331 validMessage = true;
devanlai 0:f2565808eea5 332 }
devanlai 0:f2565808eea5 333 }
devanlai 0:f2565808eea5 334 }
devanlai 3:bc163d555ddc 335
devanlai 0:f2565808eea5 336 if (validMessage) {
devanlai 3:bc163d555ddc 337 if (command[0] == 'T' || command[0] == 'R') {
devanlai 3:bc163d555ddc 338 response[0] = 'Z';
devanlai 3:bc163d555ddc 339 response[1] = '\0';
devanlai 3:bc163d555ddc 340 } else if (command[0] == 't' || command[0] == 'r') {
devanlai 3:bc163d555ddc 341 response[0] = 'z';
devanlai 3:bc163d555ddc 342 response[1] = '\0';
devanlai 3:bc163d555ddc 343 }
devanlai 0:f2565808eea5 344 success = transmitMessage(msg);
devanlai 0:f2565808eea5 345 }
devanlai 0:f2565808eea5 346
devanlai 0:f2565808eea5 347 return success;
devanlai 0:f2565808eea5 348 }
devanlai 0:f2565808eea5 349
devanlai 2:1327e61cc56b 350 bool SLCANBase::execDiagnosticCommand(const char* command, char* response) {
devanlai 2:1327e61cc56b 351 bool success = false;
devanlai 2:1327e61cc56b 352 size_t len = strlen(command);
devanlai 2:1327e61cc56b 353
devanlai 2:1327e61cc56b 354 // Validate command length
devanlai 2:1327e61cc56b 355 if (command[0] == 'W') {
devanlai 2:1327e61cc56b 356 if (len != 5) {
devanlai 2:1327e61cc56b 357 return false;
devanlai 2:1327e61cc56b 358 }
devanlai 2:1327e61cc56b 359 } else if (len != 1) {
devanlai 2:1327e61cc56b 360 return false;
devanlai 2:1327e61cc56b 361 }
devanlai 2:1327e61cc56b 362
devanlai 2:1327e61cc56b 363 if (!response) {
devanlai 2:1327e61cc56b 364 return false;
devanlai 2:1327e61cc56b 365 }
devanlai 2:1327e61cc56b 366
devanlai 2:1327e61cc56b 367 switch (command[0]) {
devanlai 2:1327e61cc56b 368 case 'V': {
devanlai 2:1327e61cc56b 369 success = true;
devanlai 2:1327e61cc56b 370 uint8_t hwVersion = getHardwareVersion();
devanlai 2:1327e61cc56b 371 uint8_t fwVersion = getFirmwareVersion();
devanlai 2:1327e61cc56b 372
devanlai 2:1327e61cc56b 373 response[0] = 'V';
devanlai 2:1327e61cc56b 374 response[1] = format_nibble(hwVersion >> 4);
devanlai 2:1327e61cc56b 375 response[2] = format_nibble(hwVersion >> 0);
devanlai 2:1327e61cc56b 376 response[3] = format_nibble(fwVersion >> 4);
devanlai 2:1327e61cc56b 377 response[4] = format_nibble(fwVersion >> 0);
devanlai 2:1327e61cc56b 378 response[5] = '\0';
devanlai 2:1327e61cc56b 379 break;
devanlai 2:1327e61cc56b 380 }
devanlai 2:1327e61cc56b 381 case 'v': {
devanlai 2:1327e61cc56b 382 success = true;
devanlai 2:1327e61cc56b 383 uint8_t fwVersion = getFirmwareVersion();
devanlai 2:1327e61cc56b 384 response[0] = 'v';
devanlai 2:1327e61cc56b 385 response[1] = format_nibble(fwVersion >> 4);
devanlai 2:1327e61cc56b 386 response[2] = format_nibble(fwVersion >> 0);
devanlai 2:1327e61cc56b 387 response[3] = '\0';
devanlai 2:1327e61cc56b 388 break;
devanlai 2:1327e61cc56b 389 }
devanlai 2:1327e61cc56b 390 case 'N': {
devanlai 2:1327e61cc56b 391 success = true;
devanlai 2:1327e61cc56b 392 const char* serial = getSerialString();
devanlai 2:1327e61cc56b 393 size_t index = 0;
devanlai 2:1327e61cc56b 394 response[index++] = 'N';
devanlai 2:1327e61cc56b 395 for (int i=0; i < 4; i++) {
devanlai 2:1327e61cc56b 396 char c = serial[i];
devanlai 2:1327e61cc56b 397 if (c == '\0') {
devanlai 2:1327e61cc56b 398 break;
devanlai 2:1327e61cc56b 399 } else {
devanlai 2:1327e61cc56b 400 response[index++] = c;
devanlai 2:1327e61cc56b 401 }
devanlai 2:1327e61cc56b 402 }
devanlai 2:1327e61cc56b 403 response[index] = '\0';
devanlai 2:1327e61cc56b 404 break;
devanlai 2:1327e61cc56b 405 }
devanlai 3:bc163d555ddc 406 case 'F': {
devanlai 3:bc163d555ddc 407 success = true;
devanlai 3:bc163d555ddc 408 uint8_t status = 0x00;
devanlai 3:bc163d555ddc 409 response[0] = 'F';
devanlai 3:bc163d555ddc 410 response[1] = format_nibble(status >> 4);
devanlai 3:bc163d555ddc 411 response[2] = format_nibble(status >> 0);
devanlai 3:bc163d555ddc 412 response[3] = '\0';
devanlai 3:bc163d555ddc 413 }
devanlai 2:1327e61cc56b 414 case 'W': {
devanlai 2:1327e61cc56b 415 // Just swallow the MCP2515 register write command
devanlai 2:1327e61cc56b 416 success = true;
devanlai 2:1327e61cc56b 417 break;
devanlai 2:1327e61cc56b 418 }
devanlai 2:1327e61cc56b 419 default: {
devanlai 2:1327e61cc56b 420 success = false;
devanlai 2:1327e61cc56b 421 break;
devanlai 2:1327e61cc56b 422 }
devanlai 2:1327e61cc56b 423 }
devanlai 2:1327e61cc56b 424
devanlai 2:1327e61cc56b 425 return success;
devanlai 2:1327e61cc56b 426 }
devanlai 2:1327e61cc56b 427
devanlai 0:f2565808eea5 428 USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
devanlai 0:f2565808eea5 429 : stream(stream),
devanlai 0:f2565808eea5 430 can(can),
devanlai 0:f2565808eea5 431 messageQueued(false),
devanlai 0:f2565808eea5 432 commandQueued(false),
devanlai 0:f2565808eea5 433 commandOverflow(false),
devanlai 0:f2565808eea5 434 inputCommandLen(0),
devanlai 0:f2565808eea5 435 outputPacketLen(0) {
devanlai 0:f2565808eea5 436
devanlai 0:f2565808eea5 437 }
devanlai 0:f2565808eea5 438
devanlai 0:f2565808eea5 439 bool USBSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 440 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 441 }
devanlai 0:f2565808eea5 442
devanlai 0:f2565808eea5 443 bool USBSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 444 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 445 }
devanlai 0:f2565808eea5 446
devanlai 0:f2565808eea5 447 bool USBSLCAN::transmitMessage(CANMessage& msg) {
devanlai 3:bc163d555ddc 448 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 449 }
devanlai 0:f2565808eea5 450
devanlai 0:f2565808eea5 451 bool USBSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 452 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 453 }
devanlai 0:f2565808eea5 454
devanlai 0:f2565808eea5 455 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 456 bool USBSLCAN::processCommands() {
devanlai 0:f2565808eea5 457 bool active = false;
devanlai 0:f2565808eea5 458
devanlai 0:f2565808eea5 459 // Buffer an entire command
devanlai 0:f2565808eea5 460 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 461 char c = (char)stream.getc();
devanlai 0:f2565808eea5 462 if (c == '\r') {
devanlai 0:f2565808eea5 463 if (commandOverflow) {
devanlai 0:f2565808eea5 464 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 465 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 466 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 467 inputCommandLen = 0;
devanlai 0:f2565808eea5 468 commandOverflow = false;
devanlai 0:f2565808eea5 469 active = true;
devanlai 0:f2565808eea5 470 } else {
devanlai 0:f2565808eea5 471 // Null-terminate the buffered command
devanlai 0:f2565808eea5 472 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 473 inputCommandLen = 0;
devanlai 0:f2565808eea5 474 active = true;
devanlai 0:f2565808eea5 475 }
devanlai 0:f2565808eea5 476 commandQueued = true;
devanlai 0:f2565808eea5 477 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 478 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 479 } else if (commandOverflow) {
devanlai 0:f2565808eea5 480 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 481 } else {
devanlai 0:f2565808eea5 482 // Append to the end of the command
devanlai 0:f2565808eea5 483 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 484
devanlai 0:f2565808eea5 485 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 486 commandOverflow = true;
devanlai 0:f2565808eea5 487 }
devanlai 0:f2565808eea5 488 }
devanlai 0:f2565808eea5 489 }
devanlai 0:f2565808eea5 490
devanlai 0:f2565808eea5 491 // Process the current command if there's space to send the response
devanlai 2:1327e61cc56b 492 if (commandQueued) {
devanlai 2:1327e61cc56b 493 size_t responseLength = commandResponseLength(inputCommandBuffer);
devanlai 2:1327e61cc56b 494 if ((outputPacketLen + responseLength) <= sizeof(outputPacketBuffer)) {
devanlai 2:1327e61cc56b 495 char outputResponseBuffer[32];
devanlai 2:1327e61cc56b 496 outputResponseBuffer[0] = '\0';
devanlai 2:1327e61cc56b 497 if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
devanlai 2:1327e61cc56b 498 // Success
devanlai 2:1327e61cc56b 499 for (char* s = outputResponseBuffer; *s != '\0'; s++) {
devanlai 2:1327e61cc56b 500 outputPacketBuffer[outputPacketLen++] = *s;
devanlai 2:1327e61cc56b 501 }
devanlai 2:1327e61cc56b 502 outputPacketBuffer[outputPacketLen++] = '\r';
devanlai 2:1327e61cc56b 503 } else {
devanlai 2:1327e61cc56b 504 // Failure
devanlai 2:1327e61cc56b 505 outputPacketBuffer[outputPacketLen++] = '\a';
devanlai 2:1327e61cc56b 506 }
devanlai 2:1327e61cc56b 507 commandQueued = false;
devanlai 2:1327e61cc56b 508 active = true;
devanlai 0:f2565808eea5 509 }
devanlai 0:f2565808eea5 510 }
devanlai 0:f2565808eea5 511
devanlai 0:f2565808eea5 512 return active;
devanlai 0:f2565808eea5 513 }
devanlai 0:f2565808eea5 514
devanlai 0:f2565808eea5 515 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 516 bool USBSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 517 bool active = false;
devanlai 0:f2565808eea5 518
devanlai 1:3644b10bce2f 519 size_t bytesAvailable = sizeof(outputPacketBuffer) - outputPacketLen;
devanlai 0:f2565808eea5 520 char* packetTail = &outputPacketBuffer[outputPacketLen];
devanlai 0:f2565808eea5 521
devanlai 0:f2565808eea5 522 if (messageQueued) {
devanlai 0:f2565808eea5 523 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 524 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 525 active = true;
devanlai 0:f2565808eea5 526 messageQueued = false;
devanlai 0:f2565808eea5 527 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 528 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 529 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 530 }
devanlai 0:f2565808eea5 531 }
devanlai 0:f2565808eea5 532
devanlai 0:f2565808eea5 533 if (!messageQueued) {
devanlai 0:f2565808eea5 534 while (getNextCANMessage(queuedMessage)) {
devanlai 0:f2565808eea5 535 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 536 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 537 active = true;
devanlai 0:f2565808eea5 538 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 539 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 540 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 541 } else {
devanlai 0:f2565808eea5 542 messageQueued = true;
devanlai 0:f2565808eea5 543 break;
devanlai 0:f2565808eea5 544 }
devanlai 0:f2565808eea5 545 }
devanlai 0:f2565808eea5 546 }
devanlai 0:f2565808eea5 547
devanlai 0:f2565808eea5 548 return active;
devanlai 0:f2565808eea5 549 }
devanlai 0:f2565808eea5 550
devanlai 0:f2565808eea5 551 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 552 bool USBSLCAN::flush() {
devanlai 0:f2565808eea5 553 bool active = false;
devanlai 0:f2565808eea5 554 if (outputPacketLen > 0) {
devanlai 0:f2565808eea5 555 bool sent = stream.writeBlock((uint8_t*)(outputPacketBuffer),
devanlai 0:f2565808eea5 556 (uint16_t)(outputPacketLen));
devanlai 0:f2565808eea5 557 if (sent) {
devanlai 0:f2565808eea5 558 active = true;
devanlai 0:f2565808eea5 559 outputPacketLen = 0;
devanlai 0:f2565808eea5 560 }
devanlai 0:f2565808eea5 561 }
devanlai 0:f2565808eea5 562 return active;
devanlai 0:f2565808eea5 563 }
devanlai 0:f2565808eea5 564
devanlai 0:f2565808eea5 565 SerialSLCAN::SerialSLCAN(Serial& stream, CAN& can)
devanlai 0:f2565808eea5 566 : stream(stream),
devanlai 0:f2565808eea5 567 can(can),
devanlai 0:f2565808eea5 568 commandQueued(false),
devanlai 0:f2565808eea5 569 commandOverflow(false),
devanlai 0:f2565808eea5 570 inputCommandLen(0) {
devanlai 0:f2565808eea5 571 }
devanlai 0:f2565808eea5 572
devanlai 0:f2565808eea5 573 bool SerialSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 574 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 575 }
devanlai 0:f2565808eea5 576
devanlai 0:f2565808eea5 577 bool SerialSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 578 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 579 }
devanlai 0:f2565808eea5 580
devanlai 0:f2565808eea5 581 bool SerialSLCAN::transmitMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 582 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 583 }
devanlai 0:f2565808eea5 584
devanlai 0:f2565808eea5 585 bool SerialSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 586 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 587 }
devanlai 0:f2565808eea5 588
devanlai 0:f2565808eea5 589 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 590 bool SerialSLCAN::processCommands() {
devanlai 0:f2565808eea5 591 bool active = false;
devanlai 0:f2565808eea5 592
devanlai 0:f2565808eea5 593 // Buffer an entire command
devanlai 0:f2565808eea5 594 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 595 char c = (char)stream.getc();
devanlai 0:f2565808eea5 596 if (c == '\r') {
devanlai 0:f2565808eea5 597 if (commandOverflow) {
devanlai 0:f2565808eea5 598 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 599 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 600 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 601 inputCommandLen = 0;
devanlai 0:f2565808eea5 602 commandOverflow = false;
devanlai 0:f2565808eea5 603 active = true;
devanlai 0:f2565808eea5 604 } else {
devanlai 0:f2565808eea5 605 // Null-terminate the buffered command
devanlai 0:f2565808eea5 606 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 607 inputCommandLen = 0;
devanlai 0:f2565808eea5 608 active = true;
devanlai 0:f2565808eea5 609 }
devanlai 0:f2565808eea5 610 commandQueued = true;
devanlai 0:f2565808eea5 611 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 612 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 613 } else if (commandOverflow) {
devanlai 0:f2565808eea5 614 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 615 } else {
devanlai 0:f2565808eea5 616 // Append to the end of the command
devanlai 0:f2565808eea5 617 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 618
devanlai 0:f2565808eea5 619 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 620 commandOverflow = true;
devanlai 0:f2565808eea5 621 }
devanlai 0:f2565808eea5 622 }
devanlai 0:f2565808eea5 623 }
devanlai 0:f2565808eea5 624
devanlai 2:1327e61cc56b 625 // Process the current command
devanlai 0:f2565808eea5 626 if (commandQueued) {
devanlai 2:1327e61cc56b 627 char outputResponseBuffer[32];
devanlai 2:1327e61cc56b 628 outputResponseBuffer[0] = '\0';
devanlai 2:1327e61cc56b 629 if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
devanlai 0:f2565808eea5 630 // Success
devanlai 2:1327e61cc56b 631 stream.puts(outputResponseBuffer);
devanlai 0:f2565808eea5 632 stream.putc('\r');
devanlai 0:f2565808eea5 633 } else {
devanlai 0:f2565808eea5 634 // Failure
devanlai 0:f2565808eea5 635 stream.putc('\a');
devanlai 0:f2565808eea5 636 }
devanlai 0:f2565808eea5 637 commandQueued = false;
devanlai 0:f2565808eea5 638 active = true;
devanlai 0:f2565808eea5 639 }
devanlai 0:f2565808eea5 640
devanlai 0:f2565808eea5 641 return active;
devanlai 0:f2565808eea5 642 }
devanlai 0:f2565808eea5 643
devanlai 0:f2565808eea5 644 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 645 bool SerialSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 646 bool active = false;
devanlai 0:f2565808eea5 647 CANMessage msg;
devanlai 0:f2565808eea5 648 while (getNextCANMessage(msg)) {
devanlai 0:f2565808eea5 649 char buffer[32];
devanlai 0:f2565808eea5 650 size_t len = formatCANMessage(msg, buffer, sizeof(buffer));
devanlai 0:f2565808eea5 651 buffer[len] = '\0';
devanlai 0:f2565808eea5 652 stream.puts(buffer);
devanlai 0:f2565808eea5 653 active = true;
devanlai 0:f2565808eea5 654 }
devanlai 0:f2565808eea5 655
devanlai 0:f2565808eea5 656 return active;
devanlai 0:f2565808eea5 657 }
devanlai 0:f2565808eea5 658
devanlai 0:f2565808eea5 659 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 660 bool SerialSLCAN::flush() {
devanlai 0:f2565808eea5 661 return false;
devanlai 0:f2565808eea5 662 }