Devan Lai / Mbed 2 deprecated SLCAN

SLCAN/CAN-USB implementation for mbed targets

Dependencies:   USBDevice mbed

slcan.cpp@2:1327e61cc56b, 2016-06-09 (annotated)

Committer:
devanlai
Date:
Thu Jun 09 06:29:08 2016 +0000
Revision:
2:1327e61cc56b
Parent:
1:3644b10bce2f
Child:
3:bc163d555ddc
Add enough commands to satisfy USBtinViewer

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 2:1327e61cc56b 151 if (command[0] == 'N') {
devanlai 2:1327e61cc56b 152 return 6;
devanlai 2:1327e61cc56b 153 } else if (command[0] == 'V') {
devanlai 2:1327e61cc56b 154 return 6;
devanlai 2:1327e61cc56b 155 } else if (command[0] == 'v') {
devanlai 2:1327e61cc56b 156 return 4;
devanlai 2:1327e61cc56b 157 } else {
devanlai 2:1327e61cc56b 158 return 1;
devanlai 2:1327e61cc56b 159 }
devanlai 2:1327e61cc56b 160 }
devanlai 2:1327e61cc56b 161
devanlai 2:1327e61cc56b 162 bool SLCANBase::execCommand(const char* command, char* response) {
devanlai 0:f2565808eea5 163 bool success = false;
devanlai 2:1327e61cc56b 164 if (response) {
devanlai 2:1327e61cc56b 165 response[0] = '\0';
devanlai 2:1327e61cc56b 166 }
devanlai 0:f2565808eea5 167
devanlai 0:f2565808eea5 168 switch (command[0]) {
devanlai 0:f2565808eea5 169 // Configuration commands
devanlai 0:f2565808eea5 170 case 'S':
devanlai 0:f2565808eea5 171 case 'O':
devanlai 0:f2565808eea5 172 case 'L':
devanlai 0:f2565808eea5 173 case 'l':
devanlai 0:f2565808eea5 174 case 'C': {
devanlai 0:f2565808eea5 175 success = execConfigCommand(command);
devanlai 0:f2565808eea5 176 break;
devanlai 0:f2565808eea5 177 }
devanlai 0:f2565808eea5 178 // Transmission commands
devanlai 0:f2565808eea5 179 case 't':
devanlai 0:f2565808eea5 180 case 'T':
devanlai 0:f2565808eea5 181 case 'r':
devanlai 0:f2565808eea5 182 case 'R': {
devanlai 0:f2565808eea5 183 success = execTransmitCommand(command);
devanlai 0:f2565808eea5 184 break;
devanlai 0:f2565808eea5 185 }
devanlai 2:1327e61cc56b 186 // Diagnostic commands
devanlai 2:1327e61cc56b 187 case 'V':
devanlai 2:1327e61cc56b 188 case 'v':
devanlai 2:1327e61cc56b 189 case 'N':
devanlai 2:1327e61cc56b 190 case 'W':
devanlai 2:1327e61cc56b 191 success = execDiagnosticCommand(command, response);
devanlai 2:1327e61cc56b 192 break;
devanlai 0:f2565808eea5 193 default: {
devanlai 0:f2565808eea5 194 success = false;
devanlai 0:f2565808eea5 195 break;
devanlai 0:f2565808eea5 196 }
devanlai 0:f2565808eea5 197 }
devanlai 0:f2565808eea5 198
devanlai 0:f2565808eea5 199 return success;
devanlai 0:f2565808eea5 200 }
devanlai 0:f2565808eea5 201
devanlai 0:f2565808eea5 202 bool SLCANBase::execConfigCommand(const char* command) {
devanlai 0:f2565808eea5 203 bool success = false;
devanlai 0:f2565808eea5 204 size_t len = strlen(command);
devanlai 0:f2565808eea5 205
devanlai 0:f2565808eea5 206 // Validate command length
devanlai 0:f2565808eea5 207 if (len != 1 && command[0] != 'S') {
devanlai 0:f2565808eea5 208 return false;
devanlai 0:f2565808eea5 209 } else if (command[0] == 'S' && len != 2) {
devanlai 0:f2565808eea5 210 return false;
devanlai 0:f2565808eea5 211 }
devanlai 0:f2565808eea5 212
devanlai 0:f2565808eea5 213 switch (command[0]) {
devanlai 0:f2565808eea5 214 case 'S': {
devanlai 0:f2565808eea5 215 bool known = true;
devanlai 0:f2565808eea5 216 int baudrate;
devanlai 0:f2565808eea5 217 switch (command[1]) {
devanlai 0:f2565808eea5 218 case '0': baudrate = 10000; break;
devanlai 0:f2565808eea5 219 case '1': baudrate = 20000; break;
devanlai 0:f2565808eea5 220 case '2': baudrate = 50000; break;
devanlai 0:f2565808eea5 221 case '3': baudrate = 100000; break;
devanlai 0:f2565808eea5 222 case '4': baudrate = 125000; break;
devanlai 0:f2565808eea5 223 case '5': baudrate = 250000; break;
devanlai 0:f2565808eea5 224 case '6': baudrate = 500000; break;
devanlai 0:f2565808eea5 225 case '7': baudrate = 800000; break;
devanlai 0:f2565808eea5 226 case '8': baudrate = 1000000; break;
devanlai 0:f2565808eea5 227 default: known = false; break;
devanlai 0:f2565808eea5 228 }
devanlai 0:f2565808eea5 229
devanlai 0:f2565808eea5 230 if (known) {
devanlai 0:f2565808eea5 231 success = setBaudrate(baudrate);
devanlai 0:f2565808eea5 232 }
devanlai 0:f2565808eea5 233
devanlai 0:f2565808eea5 234 break;
devanlai 0:f2565808eea5 235 }
devanlai 0:f2565808eea5 236 case 'O': {
devanlai 0:f2565808eea5 237 success = setMode(CAN::Normal);
devanlai 0:f2565808eea5 238 break;
devanlai 0:f2565808eea5 239 }
devanlai 0:f2565808eea5 240 case 'L': {
devanlai 0:f2565808eea5 241 success = setMode(CAN::Silent);
devanlai 0:f2565808eea5 242 break;
devanlai 0:f2565808eea5 243 }
devanlai 0:f2565808eea5 244 case 'l': {
devanlai 0:f2565808eea5 245 success = setMode(CAN::SilentTest);
devanlai 0:f2565808eea5 246 break;
devanlai 0:f2565808eea5 247 }
devanlai 0:f2565808eea5 248 case 'C': {
devanlai 0:f2565808eea5 249 success = setMode(CAN::Reset);
devanlai 0:f2565808eea5 250 break;
devanlai 0:f2565808eea5 251 }
devanlai 0:f2565808eea5 252 default: {
devanlai 0:f2565808eea5 253 success = false;
devanlai 0:f2565808eea5 254 break;
devanlai 0:f2565808eea5 255 }
devanlai 0:f2565808eea5 256 }
devanlai 0:f2565808eea5 257
devanlai 0:f2565808eea5 258 return success;
devanlai 0:f2565808eea5 259 }
devanlai 0:f2565808eea5 260
devanlai 0:f2565808eea5 261 bool SLCANBase::execTransmitCommand(const char* command) {
devanlai 0:f2565808eea5 262 bool success = false;
devanlai 0:f2565808eea5 263
devanlai 0:f2565808eea5 264 size_t len = strlen(command);
devanlai 0:f2565808eea5 265
devanlai 0:f2565808eea5 266 bool validMessage = false;
devanlai 0:f2565808eea5 267 CANMessage msg;
devanlai 0:f2565808eea5 268 if (command[0] == 't' || command[0] == 'T') {
devanlai 0:f2565808eea5 269 msg.type = CANData;
devanlai 0:f2565808eea5 270 msg.format = (command[0] == 't') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 271 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 272 if ((len >= idLen + 2) &&
devanlai 0:f2565808eea5 273 parse_hex_digits(&command[1], idLen, (uint32_t*)&msg.id) &&
devanlai 0:f2565808eea5 274 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 275 if ((len == idLen + 2 + 2*msg.len) &&
devanlai 0:f2565808eea5 276 (msg.len <= 8) &&
devanlai 0:f2565808eea5 277 parse_hex_values(&command[idLen + 2], msg.len, msg.data)) {
devanlai 0:f2565808eea5 278 validMessage = true;
devanlai 0:f2565808eea5 279 }
devanlai 0:f2565808eea5 280 }
devanlai 0:f2565808eea5 281 } else if (command[0] == 'r' || command[0] == 'R') {
devanlai 0:f2565808eea5 282 msg.type = CANRemote;
devanlai 0:f2565808eea5 283 msg.format = (command[0] == 'r') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 284 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 285 if ((len == idLen + 2) &&
devanlai 0:f2565808eea5 286 parse_hex_digits(&command[1], idLen, (uint32_t*)(&msg.id)) &&
devanlai 0:f2565808eea5 287 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 288 if (msg.len <= 8) {
devanlai 0:f2565808eea5 289 validMessage = true;
devanlai 0:f2565808eea5 290 }
devanlai 0:f2565808eea5 291 }
devanlai 0:f2565808eea5 292 }
devanlai 0:f2565808eea5 293
devanlai 0:f2565808eea5 294 if (validMessage) {
devanlai 0:f2565808eea5 295 success = transmitMessage(msg);
devanlai 0:f2565808eea5 296 }
devanlai 0:f2565808eea5 297
devanlai 0:f2565808eea5 298 return success;
devanlai 0:f2565808eea5 299 }
devanlai 0:f2565808eea5 300
devanlai 2:1327e61cc56b 301 bool SLCANBase::execDiagnosticCommand(const char* command, char* response) {
devanlai 2:1327e61cc56b 302 bool success = false;
devanlai 2:1327e61cc56b 303 size_t len = strlen(command);
devanlai 2:1327e61cc56b 304
devanlai 2:1327e61cc56b 305 // Validate command length
devanlai 2:1327e61cc56b 306 if (command[0] == 'W') {
devanlai 2:1327e61cc56b 307 if (len != 5) {
devanlai 2:1327e61cc56b 308 return false;
devanlai 2:1327e61cc56b 309 }
devanlai 2:1327e61cc56b 310 } else if (len != 1) {
devanlai 2:1327e61cc56b 311 return false;
devanlai 2:1327e61cc56b 312 }
devanlai 2:1327e61cc56b 313
devanlai 2:1327e61cc56b 314 if (!response) {
devanlai 2:1327e61cc56b 315 return false;
devanlai 2:1327e61cc56b 316 }
devanlai 2:1327e61cc56b 317
devanlai 2:1327e61cc56b 318 switch (command[0]) {
devanlai 2:1327e61cc56b 319 case 'V': {
devanlai 2:1327e61cc56b 320 success = true;
devanlai 2:1327e61cc56b 321 uint8_t hwVersion = getHardwareVersion();
devanlai 2:1327e61cc56b 322 uint8_t fwVersion = getFirmwareVersion();
devanlai 2:1327e61cc56b 323
devanlai 2:1327e61cc56b 324 response[0] = 'V';
devanlai 2:1327e61cc56b 325 response[1] = format_nibble(hwVersion >> 4);
devanlai 2:1327e61cc56b 326 response[2] = format_nibble(hwVersion >> 0);
devanlai 2:1327e61cc56b 327 response[3] = format_nibble(fwVersion >> 4);
devanlai 2:1327e61cc56b 328 response[4] = format_nibble(fwVersion >> 0);
devanlai 2:1327e61cc56b 329 response[5] = '\0';
devanlai 2:1327e61cc56b 330 break;
devanlai 2:1327e61cc56b 331 }
devanlai 2:1327e61cc56b 332 case 'v': {
devanlai 2:1327e61cc56b 333 success = true;
devanlai 2:1327e61cc56b 334 uint8_t fwVersion = getFirmwareVersion();
devanlai 2:1327e61cc56b 335 response[0] = 'v';
devanlai 2:1327e61cc56b 336 response[1] = format_nibble(fwVersion >> 4);
devanlai 2:1327e61cc56b 337 response[2] = format_nibble(fwVersion >> 0);
devanlai 2:1327e61cc56b 338 response[3] = '\0';
devanlai 2:1327e61cc56b 339 break;
devanlai 2:1327e61cc56b 340 }
devanlai 2:1327e61cc56b 341 case 'N': {
devanlai 2:1327e61cc56b 342 success = true;
devanlai 2:1327e61cc56b 343 const char* serial = getSerialString();
devanlai 2:1327e61cc56b 344 size_t index = 0;
devanlai 2:1327e61cc56b 345 response[index++] = 'N';
devanlai 2:1327e61cc56b 346 for (int i=0; i < 4; i++) {
devanlai 2:1327e61cc56b 347 char c = serial[i];
devanlai 2:1327e61cc56b 348 if (c == '\0') {
devanlai 2:1327e61cc56b 349 break;
devanlai 2:1327e61cc56b 350 } else {
devanlai 2:1327e61cc56b 351 response[index++] = c;
devanlai 2:1327e61cc56b 352 }
devanlai 2:1327e61cc56b 353 }
devanlai 2:1327e61cc56b 354 response[index] = '\0';
devanlai 2:1327e61cc56b 355 break;
devanlai 2:1327e61cc56b 356 }
devanlai 2:1327e61cc56b 357 case 'W': {
devanlai 2:1327e61cc56b 358 // Just swallow the MCP2515 register write command
devanlai 2:1327e61cc56b 359 success = true;
devanlai 2:1327e61cc56b 360 break;
devanlai 2:1327e61cc56b 361 }
devanlai 2:1327e61cc56b 362 default: {
devanlai 2:1327e61cc56b 363 success = false;
devanlai 2:1327e61cc56b 364 break;
devanlai 2:1327e61cc56b 365 }
devanlai 2:1327e61cc56b 366 }
devanlai 2:1327e61cc56b 367
devanlai 2:1327e61cc56b 368 return success;
devanlai 2:1327e61cc56b 369 }
devanlai 2:1327e61cc56b 370
devanlai 0:f2565808eea5 371 USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
devanlai 0:f2565808eea5 372 : stream(stream),
devanlai 0:f2565808eea5 373 can(can),
devanlai 0:f2565808eea5 374 messageQueued(false),
devanlai 0:f2565808eea5 375 commandQueued(false),
devanlai 0:f2565808eea5 376 commandOverflow(false),
devanlai 0:f2565808eea5 377 inputCommandLen(0),
devanlai 0:f2565808eea5 378 outputPacketLen(0) {
devanlai 0:f2565808eea5 379
devanlai 0:f2565808eea5 380 }
devanlai 0:f2565808eea5 381
devanlai 0:f2565808eea5 382 bool USBSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 383 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 384 }
devanlai 0:f2565808eea5 385
devanlai 0:f2565808eea5 386 bool USBSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 387 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 388 }
devanlai 0:f2565808eea5 389
devanlai 0:f2565808eea5 390 bool USBSLCAN::transmitMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 391 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 392 }
devanlai 0:f2565808eea5 393
devanlai 0:f2565808eea5 394 bool USBSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 395 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 396 }
devanlai 0:f2565808eea5 397
devanlai 0:f2565808eea5 398 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 399 bool USBSLCAN::processCommands() {
devanlai 0:f2565808eea5 400 bool active = false;
devanlai 0:f2565808eea5 401
devanlai 0:f2565808eea5 402 // Buffer an entire command
devanlai 0:f2565808eea5 403 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 404 char c = (char)stream.getc();
devanlai 0:f2565808eea5 405 if (c == '\r') {
devanlai 0:f2565808eea5 406 if (commandOverflow) {
devanlai 0:f2565808eea5 407 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 408 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 409 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 410 inputCommandLen = 0;
devanlai 0:f2565808eea5 411 commandOverflow = false;
devanlai 0:f2565808eea5 412 active = true;
devanlai 0:f2565808eea5 413 } else {
devanlai 0:f2565808eea5 414 // Null-terminate the buffered command
devanlai 0:f2565808eea5 415 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 416 inputCommandLen = 0;
devanlai 0:f2565808eea5 417 active = true;
devanlai 0:f2565808eea5 418 }
devanlai 0:f2565808eea5 419 commandQueued = true;
devanlai 0:f2565808eea5 420 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 421 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 422 } else if (commandOverflow) {
devanlai 0:f2565808eea5 423 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 424 } else {
devanlai 0:f2565808eea5 425 // Append to the end of the command
devanlai 0:f2565808eea5 426 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 427
devanlai 0:f2565808eea5 428 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 429 commandOverflow = true;
devanlai 0:f2565808eea5 430 }
devanlai 0:f2565808eea5 431 }
devanlai 0:f2565808eea5 432 }
devanlai 0:f2565808eea5 433
devanlai 0:f2565808eea5 434 // Process the current command if there's space to send the response
devanlai 2:1327e61cc56b 435 if (commandQueued) {
devanlai 2:1327e61cc56b 436 size_t responseLength = commandResponseLength(inputCommandBuffer);
devanlai 2:1327e61cc56b 437 if ((outputPacketLen + responseLength) <= sizeof(outputPacketBuffer)) {
devanlai 2:1327e61cc56b 438 char outputResponseBuffer[32];
devanlai 2:1327e61cc56b 439 outputResponseBuffer[0] = '\0';
devanlai 2:1327e61cc56b 440 if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
devanlai 2:1327e61cc56b 441 // Success
devanlai 2:1327e61cc56b 442 for (char* s = outputResponseBuffer; *s != '\0'; s++) {
devanlai 2:1327e61cc56b 443 outputPacketBuffer[outputPacketLen++] = *s;
devanlai 2:1327e61cc56b 444 }
devanlai 2:1327e61cc56b 445 outputPacketBuffer[outputPacketLen++] = '\r';
devanlai 2:1327e61cc56b 446 } else {
devanlai 2:1327e61cc56b 447 // Failure
devanlai 2:1327e61cc56b 448 outputPacketBuffer[outputPacketLen++] = '\a';
devanlai 2:1327e61cc56b 449 }
devanlai 2:1327e61cc56b 450 commandQueued = false;
devanlai 2:1327e61cc56b 451 active = true;
devanlai 0:f2565808eea5 452 }
devanlai 0:f2565808eea5 453 }
devanlai 0:f2565808eea5 454
devanlai 0:f2565808eea5 455 return active;
devanlai 0:f2565808eea5 456 }
devanlai 0:f2565808eea5 457
devanlai 0:f2565808eea5 458 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 459 bool USBSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 460 bool active = false;
devanlai 0:f2565808eea5 461
devanlai 1:3644b10bce2f 462 size_t bytesAvailable = sizeof(outputPacketBuffer) - outputPacketLen;
devanlai 0:f2565808eea5 463 char* packetTail = &outputPacketBuffer[outputPacketLen];
devanlai 0:f2565808eea5 464
devanlai 0:f2565808eea5 465 if (messageQueued) {
devanlai 0:f2565808eea5 466 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 467 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 468 active = true;
devanlai 0:f2565808eea5 469 messageQueued = false;
devanlai 0:f2565808eea5 470 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 471 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 472 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 473 }
devanlai 0:f2565808eea5 474 }
devanlai 0:f2565808eea5 475
devanlai 0:f2565808eea5 476 if (!messageQueued) {
devanlai 0:f2565808eea5 477 while (getNextCANMessage(queuedMessage)) {
devanlai 0:f2565808eea5 478 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 479 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 480 active = true;
devanlai 0:f2565808eea5 481 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 482 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 483 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 484 } else {
devanlai 0:f2565808eea5 485 messageQueued = true;
devanlai 0:f2565808eea5 486 break;
devanlai 0:f2565808eea5 487 }
devanlai 0:f2565808eea5 488 }
devanlai 0:f2565808eea5 489 }
devanlai 0:f2565808eea5 490
devanlai 0:f2565808eea5 491 return active;
devanlai 0:f2565808eea5 492 }
devanlai 0:f2565808eea5 493
devanlai 0:f2565808eea5 494 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 495 bool USBSLCAN::flush() {
devanlai 0:f2565808eea5 496 bool active = false;
devanlai 0:f2565808eea5 497 if (outputPacketLen > 0) {
devanlai 0:f2565808eea5 498 bool sent = stream.writeBlock((uint8_t*)(outputPacketBuffer),
devanlai 0:f2565808eea5 499 (uint16_t)(outputPacketLen));
devanlai 0:f2565808eea5 500 if (sent) {
devanlai 0:f2565808eea5 501 active = true;
devanlai 0:f2565808eea5 502 outputPacketLen = 0;
devanlai 0:f2565808eea5 503 }
devanlai 0:f2565808eea5 504 }
devanlai 0:f2565808eea5 505 return active;
devanlai 0:f2565808eea5 506 }
devanlai 0:f2565808eea5 507
devanlai 0:f2565808eea5 508 SerialSLCAN::SerialSLCAN(Serial& stream, CAN& can)
devanlai 0:f2565808eea5 509 : stream(stream),
devanlai 0:f2565808eea5 510 can(can),
devanlai 0:f2565808eea5 511 commandQueued(false),
devanlai 0:f2565808eea5 512 commandOverflow(false),
devanlai 0:f2565808eea5 513 inputCommandLen(0) {
devanlai 0:f2565808eea5 514 }
devanlai 0:f2565808eea5 515
devanlai 0:f2565808eea5 516 bool SerialSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 517 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 518 }
devanlai 0:f2565808eea5 519
devanlai 0:f2565808eea5 520 bool SerialSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 521 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 522 }
devanlai 0:f2565808eea5 523
devanlai 0:f2565808eea5 524 bool SerialSLCAN::transmitMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 525 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 526 }
devanlai 0:f2565808eea5 527
devanlai 0:f2565808eea5 528 bool SerialSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 529 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 530 }
devanlai 0:f2565808eea5 531
devanlai 0:f2565808eea5 532 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 533 bool SerialSLCAN::processCommands() {
devanlai 0:f2565808eea5 534 bool active = false;
devanlai 0:f2565808eea5 535
devanlai 0:f2565808eea5 536 // Buffer an entire command
devanlai 0:f2565808eea5 537 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 538 char c = (char)stream.getc();
devanlai 0:f2565808eea5 539 if (c == '\r') {
devanlai 0:f2565808eea5 540 if (commandOverflow) {
devanlai 0:f2565808eea5 541 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 542 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 543 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 544 inputCommandLen = 0;
devanlai 0:f2565808eea5 545 commandOverflow = false;
devanlai 0:f2565808eea5 546 active = true;
devanlai 0:f2565808eea5 547 } else {
devanlai 0:f2565808eea5 548 // Null-terminate the buffered command
devanlai 0:f2565808eea5 549 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 550 inputCommandLen = 0;
devanlai 0:f2565808eea5 551 active = true;
devanlai 0:f2565808eea5 552 }
devanlai 0:f2565808eea5 553 commandQueued = true;
devanlai 0:f2565808eea5 554 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 555 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 556 } else if (commandOverflow) {
devanlai 0:f2565808eea5 557 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 558 } else {
devanlai 0:f2565808eea5 559 // Append to the end of the command
devanlai 0:f2565808eea5 560 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 561
devanlai 0:f2565808eea5 562 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 563 commandOverflow = true;
devanlai 0:f2565808eea5 564 }
devanlai 0:f2565808eea5 565 }
devanlai 0:f2565808eea5 566 }
devanlai 0:f2565808eea5 567
devanlai 2:1327e61cc56b 568 // Process the current command
devanlai 0:f2565808eea5 569 if (commandQueued) {
devanlai 2:1327e61cc56b 570 char outputResponseBuffer[32];
devanlai 2:1327e61cc56b 571 outputResponseBuffer[0] = '\0';
devanlai 2:1327e61cc56b 572 if (execCommand(inputCommandBuffer, outputResponseBuffer)) {
devanlai 0:f2565808eea5 573 // Success
devanlai 2:1327e61cc56b 574 stream.puts(outputResponseBuffer);
devanlai 0:f2565808eea5 575 stream.putc('\r');
devanlai 0:f2565808eea5 576 } else {
devanlai 0:f2565808eea5 577 // Failure
devanlai 0:f2565808eea5 578 stream.putc('\a');
devanlai 0:f2565808eea5 579 }
devanlai 0:f2565808eea5 580 commandQueued = false;
devanlai 0:f2565808eea5 581 active = true;
devanlai 0:f2565808eea5 582 }
devanlai 0:f2565808eea5 583
devanlai 0:f2565808eea5 584 return active;
devanlai 0:f2565808eea5 585 }
devanlai 0:f2565808eea5 586
devanlai 0:f2565808eea5 587 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 588 bool SerialSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 589 bool active = false;
devanlai 0:f2565808eea5 590 CANMessage msg;
devanlai 0:f2565808eea5 591 while (getNextCANMessage(msg)) {
devanlai 0:f2565808eea5 592 char buffer[32];
devanlai 0:f2565808eea5 593 size_t len = formatCANMessage(msg, buffer, sizeof(buffer));
devanlai 0:f2565808eea5 594 buffer[len] = '\0';
devanlai 0:f2565808eea5 595 stream.puts(buffer);
devanlai 0:f2565808eea5 596 active = true;
devanlai 0:f2565808eea5 597 }
devanlai 0:f2565808eea5 598
devanlai 0:f2565808eea5 599 return active;
devanlai 0:f2565808eea5 600 }
devanlai 0:f2565808eea5 601
devanlai 0:f2565808eea5 602 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 603 bool SerialSLCAN::flush() {
devanlai 0:f2565808eea5 604 return false;
devanlai 0:f2565808eea5 605 }