Devan Lai / Mbed 2 deprecated SLCAN

SLCAN/CAN-USB implementation for mbed targets

Dependencies:   USBDevice mbed

slcan.cpp@1:3644b10bce2f, 2016-06-09 (annotated)

Committer:
devanlai
Date:
Thu Jun 09 05:30:21 2016 +0000
Revision:
1:3644b10bce2f
Parent:
0:f2565808eea5
Child:
2:1327e61cc56b
Fixed bug in USB serial output buffer

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 0:f2565808eea5 73 bool SLCANBase::update() {
devanlai 0:f2565808eea5 74 bool active = false;
devanlai 0:f2565808eea5 75 if (processCommands()) {
devanlai 0:f2565808eea5 76 active = true;
devanlai 0:f2565808eea5 77 }
devanlai 0:f2565808eea5 78 if (processCANMessages()) {
devanlai 0:f2565808eea5 79 active = true;
devanlai 0:f2565808eea5 80 }
devanlai 0:f2565808eea5 81
devanlai 0:f2565808eea5 82 if (active) {
devanlai 0:f2565808eea5 83 flush();
devanlai 0:f2565808eea5 84 }
devanlai 0:f2565808eea5 85
devanlai 0:f2565808eea5 86 return active;
devanlai 0:f2565808eea5 87 }
devanlai 0:f2565808eea5 88
devanlai 0:f2565808eea5 89 size_t SLCANBase::formattedCANMessageLength(const CANMessage& msg) {
devanlai 0:f2565808eea5 90 size_t len;
devanlai 0:f2565808eea5 91 if (msg.format == CANStandard) {
devanlai 0:f2565808eea5 92 len = 1 + 3 + 1 + (2 * msg.len) + 1;
devanlai 0:f2565808eea5 93 } else {
devanlai 0:f2565808eea5 94 len = 1 + 8 + 1 + (2 * msg.len) + 1;
devanlai 0:f2565808eea5 95 }
devanlai 0:f2565808eea5 96
devanlai 0:f2565808eea5 97 return len;
devanlai 0:f2565808eea5 98 }
devanlai 0:f2565808eea5 99
devanlai 0:f2565808eea5 100 size_t SLCANBase::formatCANMessage(const CANMessage& msg, char* buf, size_t max_len) {
devanlai 0:f2565808eea5 101 size_t len = formattedCANMessageLength(msg);
devanlai 0:f2565808eea5 102 if (len > max_len) {
devanlai 0:f2565808eea5 103 return 0;
devanlai 0:f2565808eea5 104 }
devanlai 0:f2565808eea5 105
devanlai 0:f2565808eea5 106 if (msg.format == CANStandard) {
devanlai 0:f2565808eea5 107 *buf++ = (msg.type == CANData) ? 't' : 'r';
devanlai 0:f2565808eea5 108 *buf++ = format_nibble((uint8_t)(msg.id >> 8));
devanlai 0:f2565808eea5 109 *buf++ = format_nibble((uint8_t)(msg.id >> 4));
devanlai 0:f2565808eea5 110 *buf++ = format_nibble((uint8_t)(msg.id >> 0));
devanlai 0:f2565808eea5 111 *buf++ = format_digit(msg.len);
devanlai 0:f2565808eea5 112 } else {
devanlai 0:f2565808eea5 113 *buf++ = (msg.type == CANData) ? 'T' : 'R';
devanlai 0:f2565808eea5 114 *buf++ = format_nibble((uint8_t)(msg.id >> 28));
devanlai 0:f2565808eea5 115 *buf++ = format_nibble((uint8_t)(msg.id >> 24));
devanlai 0:f2565808eea5 116 *buf++ = format_nibble((uint8_t)(msg.id >> 20));
devanlai 0:f2565808eea5 117 *buf++ = format_nibble((uint8_t)(msg.id >> 16));
devanlai 0:f2565808eea5 118 *buf++ = format_nibble((uint8_t)(msg.id >> 12));
devanlai 0:f2565808eea5 119 *buf++ = format_nibble((uint8_t)(msg.id >> 8));
devanlai 0:f2565808eea5 120 *buf++ = format_nibble((uint8_t)(msg.id >> 4));
devanlai 0:f2565808eea5 121 *buf++ = format_nibble((uint8_t)(msg.id >> 0));
devanlai 0:f2565808eea5 122 *buf++ = format_digit(msg.len);
devanlai 0:f2565808eea5 123 }
devanlai 0:f2565808eea5 124
devanlai 0:f2565808eea5 125 for (unsigned char i=0; i < msg.len; i++) {
devanlai 0:f2565808eea5 126 *buf++ = format_nibble((uint8_t)(msg.data[i] >> 4));
devanlai 0:f2565808eea5 127 *buf++ = format_nibble((uint8_t)(msg.data[i] >> 0));
devanlai 0:f2565808eea5 128 }
devanlai 0:f2565808eea5 129
devanlai 0:f2565808eea5 130 *buf++ = '\r';
devanlai 0:f2565808eea5 131
devanlai 0:f2565808eea5 132 return len;
devanlai 0:f2565808eea5 133 }
devanlai 0:f2565808eea5 134
devanlai 0:f2565808eea5 135 bool SLCANBase::execCommand(const char* command) {
devanlai 0:f2565808eea5 136 bool success = false;
devanlai 0:f2565808eea5 137
devanlai 0:f2565808eea5 138 switch (command[0]) {
devanlai 0:f2565808eea5 139 // Configuration commands
devanlai 0:f2565808eea5 140 case 'S':
devanlai 0:f2565808eea5 141 case 'O':
devanlai 0:f2565808eea5 142 case 'L':
devanlai 0:f2565808eea5 143 case 'l':
devanlai 0:f2565808eea5 144 case 'C': {
devanlai 0:f2565808eea5 145 success = execConfigCommand(command);
devanlai 0:f2565808eea5 146 break;
devanlai 0:f2565808eea5 147 }
devanlai 0:f2565808eea5 148 // Transmission commands
devanlai 0:f2565808eea5 149 case 't':
devanlai 0:f2565808eea5 150 case 'T':
devanlai 0:f2565808eea5 151 case 'r':
devanlai 0:f2565808eea5 152 case 'R': {
devanlai 0:f2565808eea5 153 success = execTransmitCommand(command);
devanlai 0:f2565808eea5 154 break;
devanlai 0:f2565808eea5 155 }
devanlai 0:f2565808eea5 156 default: {
devanlai 0:f2565808eea5 157 success = false;
devanlai 0:f2565808eea5 158 break;
devanlai 0:f2565808eea5 159 }
devanlai 0:f2565808eea5 160 }
devanlai 0:f2565808eea5 161
devanlai 0:f2565808eea5 162 return success;
devanlai 0:f2565808eea5 163 }
devanlai 0:f2565808eea5 164
devanlai 0:f2565808eea5 165 bool SLCANBase::execConfigCommand(const char* command) {
devanlai 0:f2565808eea5 166 bool success = false;
devanlai 0:f2565808eea5 167 size_t len = strlen(command);
devanlai 0:f2565808eea5 168
devanlai 0:f2565808eea5 169 // Validate command length
devanlai 0:f2565808eea5 170 if (len != 1 && command[0] != 'S') {
devanlai 0:f2565808eea5 171 return false;
devanlai 0:f2565808eea5 172 } else if (command[0] == 'S' && len != 2) {
devanlai 0:f2565808eea5 173 return false;
devanlai 0:f2565808eea5 174 }
devanlai 0:f2565808eea5 175
devanlai 0:f2565808eea5 176 switch (command[0]) {
devanlai 0:f2565808eea5 177 case 'S': {
devanlai 0:f2565808eea5 178 bool known = true;
devanlai 0:f2565808eea5 179 int baudrate;
devanlai 0:f2565808eea5 180 switch (command[1]) {
devanlai 0:f2565808eea5 181 case '0': baudrate = 10000; break;
devanlai 0:f2565808eea5 182 case '1': baudrate = 20000; break;
devanlai 0:f2565808eea5 183 case '2': baudrate = 50000; break;
devanlai 0:f2565808eea5 184 case '3': baudrate = 100000; break;
devanlai 0:f2565808eea5 185 case '4': baudrate = 125000; break;
devanlai 0:f2565808eea5 186 case '5': baudrate = 250000; break;
devanlai 0:f2565808eea5 187 case '6': baudrate = 500000; break;
devanlai 0:f2565808eea5 188 case '7': baudrate = 800000; break;
devanlai 0:f2565808eea5 189 case '8': baudrate = 1000000; break;
devanlai 0:f2565808eea5 190 default: known = false; break;
devanlai 0:f2565808eea5 191 }
devanlai 0:f2565808eea5 192
devanlai 0:f2565808eea5 193 if (known) {
devanlai 0:f2565808eea5 194 success = setBaudrate(baudrate);
devanlai 0:f2565808eea5 195 }
devanlai 0:f2565808eea5 196
devanlai 0:f2565808eea5 197 break;
devanlai 0:f2565808eea5 198 }
devanlai 0:f2565808eea5 199 case 'O': {
devanlai 0:f2565808eea5 200 success = setMode(CAN::Normal);
devanlai 0:f2565808eea5 201 break;
devanlai 0:f2565808eea5 202 }
devanlai 0:f2565808eea5 203 case 'L': {
devanlai 0:f2565808eea5 204 success = setMode(CAN::Silent);
devanlai 0:f2565808eea5 205 break;
devanlai 0:f2565808eea5 206 }
devanlai 0:f2565808eea5 207 case 'l': {
devanlai 0:f2565808eea5 208 success = setMode(CAN::SilentTest);
devanlai 0:f2565808eea5 209 break;
devanlai 0:f2565808eea5 210 }
devanlai 0:f2565808eea5 211 case 'C': {
devanlai 0:f2565808eea5 212 success = setMode(CAN::Reset);
devanlai 0:f2565808eea5 213 break;
devanlai 0:f2565808eea5 214 }
devanlai 0:f2565808eea5 215 default: {
devanlai 0:f2565808eea5 216 success = false;
devanlai 0:f2565808eea5 217 break;
devanlai 0:f2565808eea5 218 }
devanlai 0:f2565808eea5 219 }
devanlai 0:f2565808eea5 220
devanlai 0:f2565808eea5 221 return success;
devanlai 0:f2565808eea5 222 }
devanlai 0:f2565808eea5 223
devanlai 0:f2565808eea5 224 bool SLCANBase::execTransmitCommand(const char* command) {
devanlai 0:f2565808eea5 225 bool success = false;
devanlai 0:f2565808eea5 226
devanlai 0:f2565808eea5 227 size_t len = strlen(command);
devanlai 0:f2565808eea5 228
devanlai 0:f2565808eea5 229 bool validMessage = false;
devanlai 0:f2565808eea5 230 CANMessage msg;
devanlai 0:f2565808eea5 231 if (command[0] == 't' || command[0] == 'T') {
devanlai 0:f2565808eea5 232 msg.type = CANData;
devanlai 0:f2565808eea5 233 msg.format = (command[0] == 't') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 234 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 235 if ((len >= idLen + 2) &&
devanlai 0:f2565808eea5 236 parse_hex_digits(&command[1], idLen, (uint32_t*)&msg.id) &&
devanlai 0:f2565808eea5 237 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 238 if ((len == idLen + 2 + 2*msg.len) &&
devanlai 0:f2565808eea5 239 (msg.len <= 8) &&
devanlai 0:f2565808eea5 240 parse_hex_values(&command[idLen + 2], msg.len, msg.data)) {
devanlai 0:f2565808eea5 241 validMessage = true;
devanlai 0:f2565808eea5 242 }
devanlai 0:f2565808eea5 243 }
devanlai 0:f2565808eea5 244 } else if (command[0] == 'r' || command[0] == 'R') {
devanlai 0:f2565808eea5 245 msg.type = CANRemote;
devanlai 0:f2565808eea5 246 msg.format = (command[0] == 'r') ? CANStandard : CANExtended;
devanlai 0:f2565808eea5 247 size_t idLen = msg.format == CANStandard ? 3 : 8;
devanlai 0:f2565808eea5 248 if ((len == idLen + 2) &&
devanlai 0:f2565808eea5 249 parse_hex_digits(&command[1], idLen, (uint32_t*)(&msg.id)) &&
devanlai 0:f2565808eea5 250 parse_dec_digit(&command[idLen + 1], &msg.len)) {
devanlai 0:f2565808eea5 251 if (msg.len <= 8) {
devanlai 0:f2565808eea5 252 validMessage = true;
devanlai 0:f2565808eea5 253 }
devanlai 0:f2565808eea5 254 }
devanlai 0:f2565808eea5 255 }
devanlai 0:f2565808eea5 256
devanlai 0:f2565808eea5 257 if (validMessage) {
devanlai 0:f2565808eea5 258 success = transmitMessage(msg);
devanlai 0:f2565808eea5 259 }
devanlai 0:f2565808eea5 260
devanlai 0:f2565808eea5 261 return success;
devanlai 0:f2565808eea5 262 }
devanlai 0:f2565808eea5 263
devanlai 0:f2565808eea5 264 USBSLCAN::USBSLCAN(USBSerial& stream, CAN& can)
devanlai 0:f2565808eea5 265 : stream(stream),
devanlai 0:f2565808eea5 266 can(can),
devanlai 0:f2565808eea5 267 messageQueued(false),
devanlai 0:f2565808eea5 268 commandQueued(false),
devanlai 0:f2565808eea5 269 commandOverflow(false),
devanlai 0:f2565808eea5 270 inputCommandLen(0),
devanlai 0:f2565808eea5 271 outputPacketLen(0) {
devanlai 0:f2565808eea5 272
devanlai 0:f2565808eea5 273 }
devanlai 0:f2565808eea5 274
devanlai 0:f2565808eea5 275 bool USBSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 276 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 277 }
devanlai 0:f2565808eea5 278
devanlai 0:f2565808eea5 279 bool USBSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 280 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 281 }
devanlai 0:f2565808eea5 282
devanlai 0:f2565808eea5 283 bool USBSLCAN::transmitMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 284 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 285 }
devanlai 0:f2565808eea5 286
devanlai 0:f2565808eea5 287 bool USBSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 288 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 289 }
devanlai 0:f2565808eea5 290
devanlai 0:f2565808eea5 291 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 292 bool USBSLCAN::processCommands() {
devanlai 0:f2565808eea5 293 bool active = false;
devanlai 0:f2565808eea5 294
devanlai 0:f2565808eea5 295 // Buffer an entire command
devanlai 0:f2565808eea5 296 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 297 char c = (char)stream.getc();
devanlai 0:f2565808eea5 298 if (c == '\r') {
devanlai 0:f2565808eea5 299 if (commandOverflow) {
devanlai 0:f2565808eea5 300 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 301 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 302 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 303 inputCommandLen = 0;
devanlai 0:f2565808eea5 304 commandOverflow = false;
devanlai 0:f2565808eea5 305 active = true;
devanlai 0:f2565808eea5 306 } else {
devanlai 0:f2565808eea5 307 // Null-terminate the buffered command
devanlai 0:f2565808eea5 308 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 309 inputCommandLen = 0;
devanlai 0:f2565808eea5 310 active = true;
devanlai 0:f2565808eea5 311 }
devanlai 0:f2565808eea5 312 commandQueued = true;
devanlai 0:f2565808eea5 313 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 314 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 315 } else if (commandOverflow) {
devanlai 0:f2565808eea5 316 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 317 } else {
devanlai 0:f2565808eea5 318 // Append to the end of the command
devanlai 0:f2565808eea5 319 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 320
devanlai 0:f2565808eea5 321 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 322 commandOverflow = true;
devanlai 0:f2565808eea5 323 }
devanlai 0:f2565808eea5 324 }
devanlai 0:f2565808eea5 325 }
devanlai 0:f2565808eea5 326
devanlai 0:f2565808eea5 327 // Process the current command if there's space to send the response
devanlai 0:f2565808eea5 328 if (commandQueued && (outputPacketLen < sizeof(outputPacketBuffer))) {
devanlai 0:f2565808eea5 329 if (execCommand(inputCommandBuffer)) {
devanlai 0:f2565808eea5 330 // Success
devanlai 0:f2565808eea5 331 outputPacketBuffer[outputPacketLen++] = '\r';
devanlai 0:f2565808eea5 332 } else {
devanlai 0:f2565808eea5 333 // Failure
devanlai 0:f2565808eea5 334 outputPacketBuffer[outputPacketLen++] = '\a';
devanlai 0:f2565808eea5 335 }
devanlai 0:f2565808eea5 336 commandQueued = false;
devanlai 0:f2565808eea5 337 active = true;
devanlai 0:f2565808eea5 338 }
devanlai 0:f2565808eea5 339
devanlai 0:f2565808eea5 340 return active;
devanlai 0:f2565808eea5 341 }
devanlai 0:f2565808eea5 342
devanlai 0:f2565808eea5 343 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 344 bool USBSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 345 bool active = false;
devanlai 0:f2565808eea5 346
devanlai 1:3644b10bce2f 347 size_t bytesAvailable = sizeof(outputPacketBuffer) - outputPacketLen;
devanlai 0:f2565808eea5 348 char* packetTail = &outputPacketBuffer[outputPacketLen];
devanlai 0:f2565808eea5 349
devanlai 0:f2565808eea5 350 if (messageQueued) {
devanlai 0:f2565808eea5 351 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 352 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 353 active = true;
devanlai 0:f2565808eea5 354 messageQueued = false;
devanlai 0:f2565808eea5 355 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 356 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 357 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 358 }
devanlai 0:f2565808eea5 359 }
devanlai 0:f2565808eea5 360
devanlai 0:f2565808eea5 361 if (!messageQueued) {
devanlai 0:f2565808eea5 362 while (getNextCANMessage(queuedMessage)) {
devanlai 0:f2565808eea5 363 size_t bytesConsumed = formatCANMessage(queuedMessage, packetTail, bytesAvailable);
devanlai 0:f2565808eea5 364 if (bytesConsumed > 0) {
devanlai 0:f2565808eea5 365 active = true;
devanlai 0:f2565808eea5 366 bytesAvailable -= bytesConsumed;
devanlai 0:f2565808eea5 367 packetTail += bytesConsumed;
devanlai 0:f2565808eea5 368 outputPacketLen += bytesConsumed;
devanlai 0:f2565808eea5 369 } else {
devanlai 0:f2565808eea5 370 messageQueued = true;
devanlai 0:f2565808eea5 371 break;
devanlai 0:f2565808eea5 372 }
devanlai 0:f2565808eea5 373 }
devanlai 0:f2565808eea5 374 }
devanlai 0:f2565808eea5 375
devanlai 0:f2565808eea5 376 return active;
devanlai 0:f2565808eea5 377 }
devanlai 0:f2565808eea5 378
devanlai 0:f2565808eea5 379 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 380 bool USBSLCAN::flush() {
devanlai 0:f2565808eea5 381 bool active = false;
devanlai 0:f2565808eea5 382 if (outputPacketLen > 0) {
devanlai 0:f2565808eea5 383 bool sent = stream.writeBlock((uint8_t*)(outputPacketBuffer),
devanlai 0:f2565808eea5 384 (uint16_t)(outputPacketLen));
devanlai 0:f2565808eea5 385 if (sent) {
devanlai 0:f2565808eea5 386 active = true;
devanlai 0:f2565808eea5 387 outputPacketLen = 0;
devanlai 0:f2565808eea5 388 }
devanlai 0:f2565808eea5 389 }
devanlai 0:f2565808eea5 390 return active;
devanlai 0:f2565808eea5 391 }
devanlai 0:f2565808eea5 392
devanlai 0:f2565808eea5 393 SerialSLCAN::SerialSLCAN(Serial& stream, CAN& can)
devanlai 0:f2565808eea5 394 : stream(stream),
devanlai 0:f2565808eea5 395 can(can),
devanlai 0:f2565808eea5 396 commandQueued(false),
devanlai 0:f2565808eea5 397 commandOverflow(false),
devanlai 0:f2565808eea5 398 inputCommandLen(0) {
devanlai 0:f2565808eea5 399 }
devanlai 0:f2565808eea5 400
devanlai 0:f2565808eea5 401 bool SerialSLCAN::setBaudrate(int baudrate) {
devanlai 0:f2565808eea5 402 return (can.frequency(baudrate) == 1);
devanlai 0:f2565808eea5 403 }
devanlai 0:f2565808eea5 404
devanlai 0:f2565808eea5 405 bool SerialSLCAN::setMode(CAN::Mode mode) {
devanlai 0:f2565808eea5 406 return (can.mode(mode) == 1);
devanlai 0:f2565808eea5 407 }
devanlai 0:f2565808eea5 408
devanlai 0:f2565808eea5 409 bool SerialSLCAN::transmitMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 410 return (can.write(msg) == 1);
devanlai 0:f2565808eea5 411 }
devanlai 0:f2565808eea5 412
devanlai 0:f2565808eea5 413 bool SerialSLCAN::getNextCANMessage(CANMessage& msg) {
devanlai 0:f2565808eea5 414 return (can.read(msg) == 1);
devanlai 0:f2565808eea5 415 }
devanlai 0:f2565808eea5 416
devanlai 0:f2565808eea5 417 /* Parse and execute a single SLCAN command and enqueue the response */
devanlai 0:f2565808eea5 418 bool SerialSLCAN::processCommands() {
devanlai 0:f2565808eea5 419 bool active = false;
devanlai 0:f2565808eea5 420
devanlai 0:f2565808eea5 421 // Buffer an entire command
devanlai 0:f2565808eea5 422 while (!commandQueued && stream.readable()) {
devanlai 0:f2565808eea5 423 char c = (char)stream.getc();
devanlai 0:f2565808eea5 424 if (c == '\r') {
devanlai 0:f2565808eea5 425 if (commandOverflow) {
devanlai 0:f2565808eea5 426 // Replace with a dummy invalid command so we return an error
devanlai 0:f2565808eea5 427 inputCommandBuffer[0] = '!';
devanlai 0:f2565808eea5 428 inputCommandBuffer[1] = '\0';
devanlai 0:f2565808eea5 429 inputCommandLen = 0;
devanlai 0:f2565808eea5 430 commandOverflow = false;
devanlai 0:f2565808eea5 431 active = true;
devanlai 0:f2565808eea5 432 } else {
devanlai 0:f2565808eea5 433 // Null-terminate the buffered command
devanlai 0:f2565808eea5 434 inputCommandBuffer[inputCommandLen] = '\0';
devanlai 0:f2565808eea5 435 inputCommandLen = 0;
devanlai 0:f2565808eea5 436 active = true;
devanlai 0:f2565808eea5 437 }
devanlai 0:f2565808eea5 438 commandQueued = true;
devanlai 0:f2565808eea5 439 } else if (c == '\n' && inputCommandLen == 0) {
devanlai 0:f2565808eea5 440 // Ignore line feeds immediately after a carriage return
devanlai 0:f2565808eea5 441 } else if (commandOverflow) {
devanlai 0:f2565808eea5 442 // Swallow the rest of the command when overflow occurs
devanlai 0:f2565808eea5 443 } else {
devanlai 0:f2565808eea5 444 // Append to the end of the command
devanlai 0:f2565808eea5 445 inputCommandBuffer[inputCommandLen++] = c;
devanlai 0:f2565808eea5 446
devanlai 0:f2565808eea5 447 if (inputCommandLen >= sizeof(inputCommandBuffer)) {
devanlai 0:f2565808eea5 448 commandOverflow = true;
devanlai 0:f2565808eea5 449 }
devanlai 0:f2565808eea5 450 }
devanlai 0:f2565808eea5 451 }
devanlai 0:f2565808eea5 452
devanlai 0:f2565808eea5 453 // Process the current command if there's space to send the response
devanlai 0:f2565808eea5 454 if (commandQueued) {
devanlai 0:f2565808eea5 455 if (execCommand(inputCommandBuffer)) {
devanlai 0:f2565808eea5 456 // Success
devanlai 0:f2565808eea5 457 stream.putc('\r');
devanlai 0:f2565808eea5 458 } else {
devanlai 0:f2565808eea5 459 // Failure
devanlai 0:f2565808eea5 460 stream.putc('\a');
devanlai 0:f2565808eea5 461 }
devanlai 0:f2565808eea5 462 commandQueued = false;
devanlai 0:f2565808eea5 463 active = true;
devanlai 0:f2565808eea5 464 }
devanlai 0:f2565808eea5 465
devanlai 0:f2565808eea5 466 return active;
devanlai 0:f2565808eea5 467 }
devanlai 0:f2565808eea5 468
devanlai 0:f2565808eea5 469 /* Read and enqueue as many received CAN messages as will fit */
devanlai 0:f2565808eea5 470 bool SerialSLCAN::processCANMessages() {
devanlai 0:f2565808eea5 471 bool active = false;
devanlai 0:f2565808eea5 472 CANMessage msg;
devanlai 0:f2565808eea5 473 while (getNextCANMessage(msg)) {
devanlai 0:f2565808eea5 474 char buffer[32];
devanlai 0:f2565808eea5 475 size_t len = formatCANMessage(msg, buffer, sizeof(buffer));
devanlai 0:f2565808eea5 476 buffer[len] = '\0';
devanlai 0:f2565808eea5 477 stream.puts(buffer);
devanlai 0:f2565808eea5 478 active = true;
devanlai 0:f2565808eea5 479 }
devanlai 0:f2565808eea5 480
devanlai 0:f2565808eea5 481 return active;
devanlai 0:f2565808eea5 482 }
devanlai 0:f2565808eea5 483
devanlai 0:f2565808eea5 484 /* Attempt to transmit the output queue */
devanlai 0:f2565808eea5 485 bool SerialSLCAN::flush() {
devanlai 0:f2565808eea5 486 return false;
devanlai 0:f2565808eea5 487 }