From 776949b90c933e6d92ec4a1a3efbef9c36737459 Mon Sep 17 00:00:00 2001 From: XANTRONIX Development Date: Fri, 14 Aug 2020 22:24:17 -0400 Subject: [PATCH] Implement frame segmentation/reassembly Implement frame segmentation/reassembly state machine as per AX.25 v2.2 specifications, as per Section 6.6 "Disassembler/Reassembler" and associated appendices --- include/patty/ax25/sock.h | 31 ++++++- src/server.c | 88 +++++++++++++++++- src/sock.c | 191 ++++++++++++++++++++++++++++++++++++-- 3 files changed, 301 insertions(+), 9 deletions(-) diff --git a/include/patty/ax25/sock.h b/include/patty/ax25/sock.h index 965d1bb..72e2bb4 100644 --- a/include/patty/ax25/sock.h +++ b/include/patty/ax25/sock.h @@ -55,6 +55,11 @@ #define PATTY_AX25_SOCK_PARAM_ACK (1 << PATTY_AX25_PARAM_ACK) #define PATTY_AX25_SOCK_PARAM_RETRY (1 << PATTY_AX25_PARAM_RETRY) +/* + * Segmenter/reassembler information + */ +#define PATTY_AX25_SOCK_SEGMENTS_MAX 128 + enum patty_ax25_sock_type { PATTY_AX25_SOCK_STREAM, PATTY_AX25_SOCK_DGRAM, @@ -87,6 +92,12 @@ enum patty_ax25_sock_opt { PATTY_AX25_SOCK_IF }; +typedef struct _patty_ax25_sock_assembler { + size_t total, + remaining, + offset; +} patty_ax25_sock_assembler; + typedef struct _patty_ax25_sock { enum patty_ax25_proto proto; enum patty_ax25_sock_type type; @@ -127,8 +138,9 @@ typedef struct _patty_ax25_sock { void *tx_slots; - patty_ax25_if *iface; + patty_ax25_sock_assembler *assembler; + patty_ax25_if *iface; patty_kiss_tnc *raw; patty_ax25_addr local, @@ -188,6 +200,23 @@ void patty_ax25_sock_vs_incr(patty_ax25_sock *sock); void patty_ax25_sock_vr_incr(patty_ax25_sock *sock); +/* + * Frame segment reassembly state machine + */ +int patty_ax25_sock_assembler_init(patty_ax25_sock *sock, size_t total); + +void patty_ax25_sock_assembler_stop(patty_ax25_sock *sock); + +int patty_ax25_sock_assembler_ready(patty_ax25_sock *sock, size_t remaining); + +ssize_t patty_ax25_sock_assembler_save(patty_ax25_sock *sock, + void *buf, + size_t len); + +void *patty_ax25_sock_assembler_read(patty_ax25_sock *sock, + uint8_t *proto, + size_t *len); + /* * Socket I/O methods */ diff --git a/src/server.c b/src/server.c index 59a6869..2b34c8c 100644 --- a/src/server.c +++ b/src/server.c @@ -1420,6 +1420,79 @@ error_client_by_sock: return -1; } +static int handle_segment(patty_ax25_server *server, + patty_ax25_if *iface, + patty_ax25_sock *sock, + patty_ax25_frame *frame) { + uint8_t *info = (uint8_t *)frame->info; + + uint8_t first, + remaining; + + if (frame->infolen < 2) { + goto reply_rej; + } + + first = info[0] & 0x80; + remaining = info[0] & 0x7f; + + if (first) { + if (remaining == 0) { + goto reply_rej; + } + + if (patty_ax25_sock_assembler_init(sock, remaining + 1) < 0) { + goto error_sock_assembler_init; + } + } else if (remaining > 0) { + if (frame->infolen != sock->n_maxlen_rx) { + goto reply_rej; + } + } else if (remaining == 0) { + if (frame->infolen > sock->n_maxlen_rx) { + goto reply_rej; + } + } + + if (!patty_ax25_sock_assembler_ready(sock, remaining)) { + patty_ax25_sock_assembler_stop(sock); + + goto reply_rej; + } + + if (patty_ax25_sock_assembler_save(sock, + info + 1, + frame->infolen - 1) < 0) { + goto error_sock_assembler_save; + } + + if (remaining == 0) { + size_t len; + void *buf; + + if ((buf = patty_ax25_sock_assembler_read(sock, NULL, &len)) == NULL) { + goto error_sock_assembler_read; + } + + if (write(sock->fd, buf, len) < 0) { + goto error_write; + } + + patty_ax25_sock_assembler_stop(sock); + } + + return 0; + +reply_rej: + return patty_ax25_sock_send_rej(sock, PATTY_AX25_FRAME_RESPONSE, 1); + +error_write: +error_sock_assembler_read: +error_sock_assembler_save: +error_sock_assembler_init: + return -1; +} + static int handle_i(patty_ax25_server *server, patty_ax25_if *iface, patty_ax25_sock *sock, @@ -1434,8 +1507,14 @@ static int handle_i(patty_ax25_server *server, return patty_ax25_sock_send_rej(sock, PATTY_AX25_FRAME_RESPONSE, 1); } - if (write(sock->fd, frame->info, frame->infolen) < 0) { - goto error_write; + if (frame->proto == PATTY_AX25_PROTO_FRAGMENT) { + if (handle_segment(server, iface, sock, frame) < 0) { + goto error_handle_segment; + } + } else { + if (write(sock->fd, frame->info, frame->infolen) < 0) { + goto error_write; + } } sock->va = frame->nr; @@ -1453,6 +1532,7 @@ static int handle_i(patty_ax25_server *server, return 0; +error_handle_segment: error_write: return -1; } @@ -1465,6 +1545,10 @@ static int handle_ui(patty_ax25_server *server, return 0; } + if (frame->proto == PATTY_AX25_PROTO_FRAGMENT) { + return handle_segment(server, iface, sock, frame); + } + return write(sock->fd, frame->info, frame->infolen); } diff --git a/src/sock.c b/src/sock.c index ef41686..bd4e3b3 100644 --- a/src/sock.c +++ b/src/sock.c @@ -198,6 +198,10 @@ void patty_ax25_sock_destroy(patty_ax25_sock *sock) { (void)close(sock->fd); } + if (sock->assembler) { + free(sock->assembler); + } + if (sock->tx_slots) { free(sock->tx_slots); } @@ -410,6 +414,97 @@ void patty_ax25_sock_vr_incr(patty_ax25_sock *sock) { } } +int patty_ax25_sock_assembler_init(patty_ax25_sock *sock, size_t total) { + if (total < 2) { + errno = EINVAL; + + goto error_invalid; + } + + if (sock->assembler == NULL || sock->assembler->total < total) { + size_t size = sizeof(patty_ax25_sock_assembler) + + total * sock->n_maxlen_rx; + + if ((sock->assembler = malloc(size)) == NULL) { + goto error_malloc; + } + + sock->assembler->total = total; + sock->assembler->remaining = total; + sock->assembler->offset = 0; + } + + return 0; + +error_malloc: +error_invalid: + return -1; +} + +int patty_ax25_sock_assembler_ready(patty_ax25_sock *sock, size_t remaining) { + if (sock->assembler == NULL || sock->assembler->total == 0) { + return 0; + } + + return remaining + 1 == sock->assembler->remaining? 1: 0; +} + +void patty_ax25_sock_assembler_stop(patty_ax25_sock *sock) { + if (sock->assembler == NULL) { + return; + } + + sock->assembler->remaining = 0; + sock->assembler->total = 0; +} + +ssize_t patty_ax25_sock_assembler_save(patty_ax25_sock *sock, + void *buf, + size_t len) { + uint8_t *dest = (uint8_t *)(sock->assembler + 1); + + if (len > sock->n_maxlen_rx - 1) { + errno = EOVERFLOW; + + goto error_overflow; + } + + if (sock->assembler->remaining == 0) { + errno = EIO; + + goto error_underflow; + } + + memcpy(dest + sock->assembler->offset, buf, len); + + sock->assembler->offset += len; + sock->assembler->remaining--; + + return len; + +error_underflow: +error_overflow: + return -1; +} + +void *patty_ax25_sock_assembler_read(patty_ax25_sock *sock, + uint8_t *proto, + size_t *len) { + uint8_t *buf = (uint8_t *)(sock->assembler + 1); + + if (sock->assembler == NULL) { + if (proto) *proto = 0; + if (len) *len = 0; + + return NULL; + } + + if (proto) *proto = buf[0]; + if (len) *len = sock->assembler->offset - 1; + + return buf + 1; +} + static inline int toobig(patty_ax25_sock *sock, size_t infolen) { return infolen > PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_tx; @@ -743,11 +838,91 @@ ssize_t patty_ax25_sock_send_test(patty_ax25_sock *sock, infolen); } +static ssize_t write_segmented(patty_ax25_sock *sock, + void *buf, + size_t len) { + size_t segments, + seglen; + + size_t i = 0; + + int first = 1; + + if (sock->n_maxlen_tx < 2) { + errno = EOVERFLOW; + + goto error_toobig; + } + + seglen = sock->n_maxlen_tx - 1; + segments = (len + 1) / seglen; + + if (len % seglen) { + segments++; + } + + if (segments > PATTY_AX25_SOCK_SEGMENTS_MAX) { + errno = EOVERFLOW; + + goto error_toobig; + } + + while (segments--) { + uint8_t *dest = (uint8_t *)sock->tx_buf; + + uint8_t header = (uint8_t)(segments & 0xff); + + size_t copylen = segments == 0? + len - (len % seglen): + seglen & 0xff; + + size_t o = 0; + + uint16_t control = sock->type == PATTY_AX25_SOCK_STREAM? + control_i(sock, 0): + control_ui(0); + + if (first) { + header |= 0x80; + } + + dest[o++] = header; + + if (first) { + dest[o++] = sock->proto; + copylen--; + first = 0; + } + + memcpy(dest + o, (uint8_t *)buf + i, copylen); + + i += copylen; + o += copylen; + + if (frame_send(sock, + PATTY_AX25_FRAME_COMMAND, + control, + PATTY_AX25_PROTO_FRAGMENT, + sock->tx_buf, + o) < 0) { + goto error_frame_send; + } + + if (sock->type == PATTY_AX25_SOCK_STREAM) { + patty_ax25_sock_vs_incr(sock); + } + } + + return len; + +error_frame_send: +error_toobig: + return -1; +} + ssize_t patty_ax25_sock_write(patty_ax25_sock *sock, void *buf, size_t len) { - uint16_t control; - if (sock->mode == PATTY_AX25_SOCK_DM) { errno = EBADF; @@ -756,11 +931,13 @@ ssize_t patty_ax25_sock_write(patty_ax25_sock *sock, switch (sock->type) { case PATTY_AX25_SOCK_STREAM: - control = control_i(sock, 0); + if (len > sock->n_maxlen_tx) { + return write_segmented(sock, buf, len); + } if (frame_send(sock, PATTY_AX25_FRAME_COMMAND, - control, + control_i(sock, 0), sock->proto, buf, len) < 0) { @@ -772,11 +949,13 @@ ssize_t patty_ax25_sock_write(patty_ax25_sock *sock, break; case PATTY_AX25_SOCK_DGRAM: - control = control_ui(0); + if (len > sock->n_maxlen_tx) { + return write_segmented(sock, buf, len); + } if (frame_send(sock, PATTY_AX25_FRAME_COMMAND, - control, + control_ui(0), sock->proto, buf, len) < 0) {