360 lines
8.9 KiB
C
360 lines
8.9 KiB
C
#include <string.h>
|
|
#include <errno.h>
|
|
|
|
#include <patty/ax25.h>
|
|
|
|
static ssize_t decode_station(patty_ax25_addr *addr,
|
|
void *data,
|
|
off_t offset) {
|
|
int i, space = 0;
|
|
|
|
for (i=0; i<PATTY_AX25_CALLSIGN_LEN; i++) {
|
|
uint8_t b = ((uint8_t *)data + offset)[i];
|
|
uint8_t c = b >> 1;
|
|
|
|
if (!PATTY_AX25_ADDRESS_CHAR_VALID(c) || PATTY_AX25_ADDRESS_OCTET_LAST(b)) {
|
|
errno = EINVAL;
|
|
|
|
goto error;
|
|
}
|
|
|
|
if (c == ' ' && !space) {
|
|
space = 1;
|
|
} else if (c != ' ' && space) {
|
|
errno = EINVAL;
|
|
|
|
goto error;
|
|
}
|
|
}
|
|
|
|
memcpy(addr, ((uint8_t *)data) + offset, sizeof(*addr));
|
|
|
|
return PATTY_AX25_ADDRESS_SIZE;
|
|
|
|
error:
|
|
return -1;
|
|
}
|
|
|
|
static ssize_t decode_hops(patty_ax25_frame *frame,
|
|
void *data,
|
|
off_t start) {
|
|
ssize_t decoded;
|
|
off_t offset = start;
|
|
|
|
int i;
|
|
|
|
patty_ax25_addr *addr = NULL;
|
|
|
|
/*
|
|
* Try to count the AX.25-specified maximum number of hops in the current
|
|
* frame.
|
|
*/
|
|
for (i=0; i<PATTY_AX25_MAX_HOPS; i++) {
|
|
addr = (patty_ax25_addr *)((uint8_t *)data + offset);
|
|
|
|
if ((decoded = decode_station(&frame->repeaters[i], data, offset)) < 0) {
|
|
goto error;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
|
|
frame->hops++;
|
|
|
|
if (PATTY_AX25_ADDRESS_OCTET_LAST(addr->ssid)) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* If the last hop does not have the address extension bit set, then
|
|
* that's a big problem.
|
|
*/
|
|
if (addr && !PATTY_AX25_ADDRESS_OCTET_LAST(addr->ssid)) {
|
|
errno = EINVAL;
|
|
|
|
goto error;
|
|
}
|
|
|
|
return offset - start;
|
|
|
|
error:
|
|
return -1;
|
|
}
|
|
|
|
static ssize_t decode_address(patty_ax25_frame *frame,
|
|
void *data,
|
|
off_t start) {
|
|
off_t offset = start;
|
|
ssize_t decoded;
|
|
|
|
if ((decoded = decode_station(&frame->dest, data, offset)) < 0) {
|
|
goto error;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
|
|
if ((decoded = decode_station(&frame->src, data, offset)) < 0) {
|
|
goto error;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
|
|
/*
|
|
* If the source address is not the final address in the frame, begin
|
|
* decoding repeater addresses.
|
|
*/
|
|
if (!PATTY_AX25_ADDRESS_OCTET_LAST(frame->src.ssid)) {
|
|
if ((decoded = decode_hops(frame, data, offset)) < 0) {
|
|
goto error;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
}
|
|
|
|
return offset - start;
|
|
|
|
error:
|
|
return -1;
|
|
}
|
|
|
|
static ssize_t decode_info(patty_ax25_frame *frame,
|
|
void *data,
|
|
off_t offset,
|
|
size_t size) {
|
|
uint8_t control = ((uint8_t *)data + offset)[0];
|
|
size_t decoded = 0;
|
|
|
|
int info = 0;
|
|
|
|
if (PATTY_AX25_CONTROL_INFO(control)) {
|
|
info = 1;
|
|
} else if (PATTY_AX25_CONTROL_UNNUMBERED(control)) {
|
|
if (PATTY_AX25_CONTROL_UNNUMBERED_INFO(control)) {
|
|
info = 1;
|
|
}
|
|
} else {
|
|
errno = EINVAL;
|
|
|
|
goto error;
|
|
}
|
|
|
|
frame->control = control;
|
|
|
|
decoded++;
|
|
|
|
/*
|
|
* If we've received either an Info or Unnumbered Info frame, then decode
|
|
* the Protocol Identifier field, and calculate the location and size of
|
|
* the payload as well.
|
|
*/
|
|
if (info) {
|
|
decoded++;
|
|
|
|
frame->proto = ((uint8_t *)data + offset)[1];
|
|
frame->info = (void *)((uint8_t *)data + offset + decoded);
|
|
frame->infolen = size - offset - decoded;
|
|
|
|
decoded += frame->infolen;
|
|
}
|
|
|
|
return decoded;
|
|
|
|
error:
|
|
return -1;
|
|
}
|
|
|
|
ssize_t patty_ax25_frame_decode(patty_ax25_frame *frame,
|
|
enum patty_ax25_frame_format format,
|
|
void *src,
|
|
size_t size) {
|
|
ssize_t decoded;
|
|
off_t offset = 0;
|
|
|
|
memset(frame, '\0', sizeof(*frame));
|
|
|
|
/*
|
|
* First, decode the variable-length Address field.
|
|
*/
|
|
if ((decoded = decode_address(frame, src, offset)) < 0) {
|
|
errno = EIO;
|
|
|
|
goto error_decode;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
|
|
/*
|
|
* Next, decode the remaining Control Field, optional Protocol Identifier
|
|
* field, and Info payload that may follow.
|
|
*/
|
|
if ((decoded = decode_info(frame, src, offset, size)) < 0) {
|
|
errno = EIO;
|
|
|
|
goto error_decode;
|
|
} else {
|
|
offset += decoded;
|
|
}
|
|
|
|
return offset;
|
|
|
|
error_decode:
|
|
return -1;
|
|
}
|
|
|
|
static ssize_t encode_address(patty_ax25_frame *frame,
|
|
void *dest,
|
|
size_t len) {
|
|
size_t i,
|
|
offset = 0,
|
|
hops = frame->hops > PATTY_AX25_MAX_HOPS?
|
|
PATTY_AX25_MAX_HOPS: frame->hops;
|
|
|
|
if ((2 + hops) * sizeof(patty_ax25_addr) > len) {
|
|
goto error_toobig;
|
|
}
|
|
|
|
memcpy((uint8_t *)dest + offset, &frame->dest, sizeof(patty_ax25_addr));
|
|
offset += sizeof(patty_ax25_addr);
|
|
((uint8_t *)dest)[offset-1] &= ~1;
|
|
|
|
memcpy((uint8_t *)dest + offset, &frame->src, sizeof(patty_ax25_addr));
|
|
offset += sizeof(patty_ax25_addr);
|
|
((uint8_t *)dest)[offset-1] &= ~1;
|
|
|
|
for (i=0; i<hops; i++) {
|
|
if (frame->repeaters[i].callsign[0] == '\0') {
|
|
break;
|
|
}
|
|
|
|
memcpy((uint8_t *)dest + offset,
|
|
&frame->repeaters[i],
|
|
sizeof(patty_ax25_addr));
|
|
|
|
offset += sizeof(patty_ax25_addr);
|
|
}
|
|
|
|
((uint8_t *)dest)[offset-1] |= 1;
|
|
|
|
return offset;
|
|
|
|
error_toobig:
|
|
return -1;
|
|
}
|
|
|
|
ssize_t patty_ax25_frame_encode(patty_ax25_frame *frame,
|
|
enum patty_ax25_frame_format format,
|
|
void *dest,
|
|
size_t len) {
|
|
size_t offset = 0;
|
|
|
|
offset = encode_address(frame, dest, len);
|
|
|
|
switch (format) {
|
|
case PATTY_AX25_FRAME_NORMAL:
|
|
((uint8_t *)dest)[offset++] = frame->control;
|
|
|
|
break;
|
|
|
|
case PATTY_AX25_FRAME_EXTENDED:
|
|
((uint8_t *)dest)[offset++] = (frame->control & 0xff00) >> 8;
|
|
((uint8_t *)dest)[offset++] = frame->control & 0xf00ff;
|
|
|
|
break;
|
|
}
|
|
|
|
if (PATTY_AX25_CONTROL_INFO(frame->control)) {
|
|
if (1 + offset + frame->infolen > len) {
|
|
goto error_toobig;
|
|
}
|
|
|
|
((uint8_t *)dest)[offset++] = frame->proto;
|
|
|
|
memcpy((uint8_t *)dest + offset, frame->info, frame->infolen);
|
|
|
|
offset += frame->infolen;
|
|
}
|
|
|
|
return offset;
|
|
|
|
error_toobig:
|
|
return -1;
|
|
}
|
|
|
|
enum patty_ax25_version patty_ax25_frame_version(patty_ax25_frame *frame) {
|
|
return PATTY_AX25_ADDRESS_SSID_C(frame->src.ssid) ==
|
|
PATTY_AX25_ADDRESS_SSID_C(frame->dest.ssid)?
|
|
PATTY_AX25_2_0: PATTY_AX25_OLD;
|
|
}
|
|
|
|
enum patty_ax25_frame_type patty_ax25_frame_type(patty_ax25_frame *frame) {
|
|
if (PATTY_AX25_CONTROL_INFO(frame->control)) {
|
|
return PATTY_AX25_FRAME_INFO;
|
|
} else if (PATTY_AX25_CONTROL_SUPER(frame->control)) {
|
|
return PATTY_AX25_FRAME_SUPER;
|
|
} else if (PATTY_AX25_CONTROL_UNNUMBERED(frame->control)) {
|
|
return PATTY_AX25_FRAME_UNNUMBERED;
|
|
}
|
|
|
|
return PATTY_AX25_FRAME_UNKNOWN;
|
|
}
|
|
|
|
enum patty_ax25_frame_cr patty_ax25_frame_cr(patty_ax25_frame *frame) {
|
|
return PATTY_AX25_ADDRESS_SSID_C(frame->src.ssid)?
|
|
PATTY_AX25_FRAME_RESPONSE:
|
|
PATTY_AX25_FRAME_COMMAND;
|
|
}
|
|
|
|
ssize_t patty_ax25_frame_info(patty_ax25_frame *frame,
|
|
void **info) {
|
|
if (frame == NULL || frame->info == NULL || info == NULL) {
|
|
errno = EINVAL;
|
|
|
|
goto error_invalid_args;
|
|
}
|
|
|
|
*info = frame->info;
|
|
|
|
return frame->infolen;
|
|
|
|
error_invalid_args:
|
|
return -1;
|
|
}
|
|
|
|
void patty_ax25_frame_copy_reply_address(patty_ax25_frame *rx,
|
|
patty_ax25_frame *tx) {
|
|
size_t i,
|
|
max = rx->hops > PATTY_AX25_MAX_HOPS? PATTY_AX25_MAX_HOPS: rx->hops;
|
|
|
|
memcpy(&tx->dest, &rx->src, sizeof(patty_ax25_addr));
|
|
memcpy(&tx->src, &rx->dest, sizeof(patty_ax25_addr));
|
|
|
|
memset(&tx->repeaters, '\0', sizeof(tx->repeaters));
|
|
|
|
for (i=0; i<max; i++) {
|
|
memcpy(&tx->repeaters[i], &rx->repeaters[max-i-1], sizeof(patty_ax25_addr));
|
|
}
|
|
|
|
tx->hops = rx->hops;
|
|
}
|
|
|
|
void patty_ax25_frame_set(patty_ax25_frame *frame,
|
|
uint16_t control,
|
|
uint8_t proto,
|
|
void *info,
|
|
size_t infolen) {
|
|
frame->control = control;
|
|
frame->proto = proto;
|
|
frame->info = info;
|
|
frame->infolen = infolen;
|
|
}
|
|
|
|
void patty_ax25_frame_set_ui(patty_ax25_frame *frame,
|
|
uint8_t flags,
|
|
uint8_t proto,
|
|
void *info,
|
|
size_t infolen) {
|
|
frame->control = PATTY_AX25_FRAME_U_UI | flags;
|
|
frame->proto = proto;
|
|
frame->info = info;
|
|
frame->infolen = infolen;
|
|
}
|