85 lines
2.5 KiB
C
85 lines
2.5 KiB
C
#include <stdbool.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include "KISS.h"
|
|
#include "Serial.h"
|
|
|
|
int frame_len;
|
|
bool IN_FRAME;
|
|
bool ESCAPE;
|
|
|
|
uint8_t kiss_command = CMD_UNKNOWN;
|
|
uint8_t frame_buffer[MAX_PAYLOAD];
|
|
uint8_t write_buffer[MAX_PAYLOAD*2+3];
|
|
|
|
extern bool verbose;
|
|
extern bool daemonize;
|
|
extern int attached_if;
|
|
extern int device_type;
|
|
extern void cleanup(void);
|
|
|
|
void kiss_frame_received(int frame_len) {
|
|
if ( (device_type == IF_TUN && frame_len >= TUN_MIN_FRAME_SIZE) || (device_type == IF_TAP && frame_len >= ETHERNET_MIN_FRAME_SIZE) ) {
|
|
int written = write(attached_if, frame_buffer, frame_len);
|
|
if (written == -1) {
|
|
if (verbose && !daemonize) printf("Could not write received KISS frame (%d bytes) to network interface, is the interface up?\r\n", frame_len);
|
|
} else if (written != frame_len) {
|
|
if (!daemonize) printf("Error: Could only write %d of %d bytes to interface", written, frame_len);
|
|
cleanup();
|
|
exit(1);
|
|
}
|
|
if (verbose && !daemonize) printf("Got %d bytes from TNC, wrote %d bytes to interface\r\n", frame_len, written);
|
|
}
|
|
}
|
|
|
|
void kiss_serial_read(uint8_t sbyte) {
|
|
if (IN_FRAME && sbyte == FEND && kiss_command == CMD_DATA) {
|
|
IN_FRAME = false;
|
|
kiss_frame_received(frame_len);
|
|
} else if (sbyte == FEND) {
|
|
IN_FRAME = true;
|
|
kiss_command = CMD_UNKNOWN;
|
|
frame_len = 0;
|
|
} else if (IN_FRAME && frame_len < MAX_PAYLOAD) {
|
|
// Have a look at the command byte first
|
|
if (frame_len == 0 && kiss_command == CMD_UNKNOWN) {
|
|
// Strip of port nibble
|
|
kiss_command = sbyte & 0x0F;
|
|
} else if (kiss_command == CMD_DATA) {
|
|
if (sbyte == FESC) {
|
|
ESCAPE = true;
|
|
} else {
|
|
if (ESCAPE) {
|
|
if (sbyte == TFEND) sbyte = FEND;
|
|
if (sbyte == TFESC) sbyte = FESC;
|
|
ESCAPE = false;
|
|
}
|
|
|
|
if (frame_len < MAX_PAYLOAD) {
|
|
frame_buffer[frame_len++] = sbyte;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int kiss_write_frame(int serial_port, uint8_t* buffer, int frame_len) {
|
|
int write_len = 0;
|
|
write_buffer[write_len++] = FEND;
|
|
write_buffer[write_len++] = CMD_DATA;
|
|
for (int i = 0; i < frame_len; i++) {
|
|
uint8_t byte = buffer[i];
|
|
if (byte == FEND) {
|
|
write_buffer[write_len++] = FESC;
|
|
write_buffer[write_len++] = TFEND;
|
|
} else if (byte == FESC) {
|
|
write_buffer[write_len++] = FESC;
|
|
write_buffer[write_len++] = TFESC;
|
|
} else {
|
|
write_buffer[write_len++] = byte;
|
|
}
|
|
}
|
|
write_buffer[write_len++] = FEND;
|
|
|
|
return write(serial_port, write_buffer, write_len);
|
|
} |