88 lines
2.2 KiB
C
88 lines
2.2 KiB
C
#include <stdio.h>
|
|
#include "slip.h"
|
|
|
|
enum slip_result slip_encode(unsigned char *frame, unsigned long frameLength, unsigned char *output,
|
|
unsigned long maxOutputSize, unsigned long *outputSize)
|
|
{
|
|
unsigned long outputIndex = 0;
|
|
for (unsigned long inIndex = 0; inIndex < frameLength; inIndex++) {
|
|
// Check if we ran out of space on the output
|
|
if (maxOutputSize <= outputIndex) {
|
|
return SLIP_BUFFER_OVERFLOW;
|
|
}
|
|
|
|
// Grab one byte from the input and check if we need to escape it
|
|
unsigned char c = frame[inIndex];
|
|
switch (c) {
|
|
case SLIP_END:
|
|
output[outputIndex] = SLIP_ESC;
|
|
output[outputIndex + 1] = SLIP_ESC_END;
|
|
outputIndex += 2;
|
|
break;
|
|
case SLIP_ESC:
|
|
output[outputIndex] = SLIP_ESC;
|
|
output[outputIndex + 1] = SLIP_ESC_ESC;
|
|
outputIndex += 2;
|
|
break;
|
|
default:
|
|
// No need to escape, copy as it is
|
|
output[outputIndex] = c;
|
|
outputIndex += 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Mark the frame end
|
|
output[outputIndex] = SLIP_END;
|
|
|
|
// Return the output size
|
|
*outputSize = outputIndex + 1;
|
|
return SLIP_OK;
|
|
}
|
|
|
|
enum slip_result slip_decode(unsigned char *encodedFrame, unsigned long frameLength,
|
|
unsigned char *output, unsigned long maxOutputSize, unsigned long *outputSize)
|
|
{
|
|
int invalidEscape = 0;
|
|
|
|
unsigned long outputIndex = 0;
|
|
for (unsigned long inIndex = 0; inIndex < frameLength; inIndex++) {
|
|
// Check if we ran out of space on the output buffer
|
|
if (maxOutputSize <= outputIndex) {
|
|
return SLIP_BUFFER_OVERFLOW;
|
|
}
|
|
|
|
unsigned char inByte = encodedFrame[inIndex];
|
|
if (inByte == SLIP_ESC) {
|
|
switch (encodedFrame[inIndex + 1]) {
|
|
case SLIP_ESC_END:
|
|
output[outputIndex] = SLIP_END;
|
|
break;
|
|
case SLIP_ESC_ESC:
|
|
output[outputIndex] = SLIP_ESC;
|
|
break;
|
|
default:
|
|
// Escape sequence invalid, complain on stderr
|
|
output[outputIndex] = SLIP_ESC;
|
|
invalidEscape = 1;
|
|
fprintf(stderr, "SLIP escape error! (Input bytes 0x%02x, 0x%02x)\n", inByte, encodedFrame[inIndex + 1]);
|
|
break;
|
|
}
|
|
inIndex += 1;
|
|
} else if (inByte == SLIP_END) {
|
|
// End of packet, stop the loop
|
|
outputIndex++;
|
|
break;
|
|
} else {
|
|
output[outputIndex] = inByte;
|
|
}
|
|
|
|
outputIndex++;
|
|
}
|
|
|
|
*outputSize = outputIndex;
|
|
if (invalidEscape)
|
|
return SLIP_INVALID_ESCAPE;
|
|
return SLIP_OK;
|
|
}
|