diff --git a/src/shearwater_common.c b/src/shearwater_common.c index 68d7b46..a6b454d 100644 --- a/src/shearwater_common.c +++ b/src/shearwater_common.c @@ -144,64 +144,63 @@ static dc_status_t shearwater_common_slip_write (shearwater_common_device_t *device, const unsigned char data[], unsigned int size) { dc_status_t status = DC_STATUS_SUCCESS; - const unsigned char end[] = {END}; - const unsigned char esc_end[] = {ESC, ESC_END}; - const unsigned char esc_esc[] = {ESC, ESC_ESC}; unsigned char buffer[32]; unsigned int nbytes = 0; #if 0 // Send an initial END character to flush out any data that may have // accumulated in the receiver due to line noise. - status = dc_iostream_write (device->iostream, end, sizeof (end), NULL); - if (status != DC_STATUS_SUCCESS) { - return status; - } + buffer[nbytes++] = END; #endif for (unsigned int i = 0; i < size; ++i) { - const unsigned char *seq = NULL; - unsigned int len = 0; - switch (data[i]) { - case END: - // Escape the END character. - seq = esc_end; - len = sizeof (esc_end); - break; - case ESC: - // Escape the ESC character. - seq = esc_esc; - len = sizeof (esc_esc); - break; - default: - // Normal character. - seq = data + i; - len = 1; - break; + unsigned char c = data[i]; + + if (c == END || c == ESC) { + // Append the escape character. + buffer[nbytes++] = ESC; + + // Flush the buffer if necessary. + if (nbytes >= sizeof(buffer)) { + status = dc_iostream_write (device->iostream, buffer, nbytes, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR (device->base.context, "Failed to send the packet."); + return status; + } + + nbytes = 0; + } + + // Escape the character. + if (c == END) { + c = ESC_END; + } else { + c = ESC_ESC; + } } + // Append the character. + buffer[nbytes++] = c; + // Flush the buffer if necessary. - if (nbytes + len + sizeof(end) > sizeof(buffer)) { + if (nbytes >= sizeof(buffer)) { status = dc_iostream_write (device->iostream, buffer, nbytes, NULL); if (status != DC_STATUS_SUCCESS) { + ERROR (device->base.context, "Failed to send the packet."); return status; } nbytes = 0; } - - // Append the escaped character. - memcpy(buffer + nbytes, seq, len); - nbytes += len; } // Append the END character to indicate the end of the packet. - memcpy(buffer + nbytes, end, sizeof(end)); - nbytes += sizeof(end); + buffer[nbytes++] = END; // Flush the buffer. status = dc_iostream_write (device->iostream, buffer, nbytes, NULL); if (status != DC_STATUS_SUCCESS) { + ERROR (device->base.context, "Failed to send the packet."); return status; } @@ -213,7 +212,8 @@ static dc_status_t shearwater_common_slip_read (shearwater_common_device_t *device, unsigned char data[], unsigned int size, unsigned int *actual) { dc_status_t status = DC_STATUS_SUCCESS; - unsigned int received = 0; + unsigned int escaped = 0; + unsigned int nbytes = 0; // Read bytes until a complete packet has been received. If the // buffer runs out of space, bytes are dropped. The caller can @@ -225,28 +225,37 @@ shearwater_common_slip_read (shearwater_common_device_t *device, unsigned char d // Get a single character to process. status = dc_iostream_read (device->iostream, &c, 1, NULL); if (status != DC_STATUS_SUCCESS) { + ERROR (device->base.context, "Failed to receive the packet."); return status; } - switch (c) { - case END: - // If it's an END character then we're done. - // As a minor optimization, empty packets are ignored. This - // is to avoid bothering the upper layers with all the empty - // packets generated by the duplicate END characters which - // are sent to try to detect line noise. - if (received) - goto done; - else - break; - case ESC: - // If it's an ESC character, get another character and then - // figure out what to store in the packet based on that. - status = dc_iostream_read (device->iostream, &c, 1, NULL); - if (status != DC_STATUS_SUCCESS) { - return status; + if (c == END || c == ESC) { + if (escaped) { + // If the END or ESC characters are escaped, then we + // have a protocol violation, and an error is reported. + ERROR (device->base.context, "SLIP frame escaped the special character %02x.", c); + return DC_STATUS_PROTOCOL; } + if (c == END) { + // If it's an END character then we're done. + // As a minor optimization, empty packets are ignored. This + // is to avoid bothering the upper layers with all the empty + // packets generated by the duplicate END characters which + // are sent to try to detect line noise. + if (nbytes) { + goto done; + } + } else { + // If it's an ESC character, get another character and then + // figure out what to store in the packet based on that. + escaped = 1; + } + + continue; + } + + if (escaped) { // If it's not one of the two escaped characters, then we // have a protocol violation. The best bet seems to be to // leave the byte alone and just stuff it into the packet. @@ -257,24 +266,29 @@ shearwater_common_slip_read (shearwater_common_device_t *device, unsigned char d case ESC_ESC: c = ESC; break; + default: + break; } - // Fall-through! - default: - if (received < size) - data[received] = c; - received++; + + escaped = 0; } + + if (nbytes < size) + data[nbytes] = c; + nbytes++; } done: - if (received > size) + if (nbytes > size) { + ERROR (device->base.context, "Insufficient buffer space available."); return DC_STATUS_PROTOCOL; + } if (actual) - *actual = received; + *actual = nbytes; - return DC_STATUS_SUCCESS; + return status; }