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
This commit is contained in:
XANTRONIX Development
XANTRONIX Industrial
parent
6d22d0286c
commit
776949b90c
3 changed files with 301 additions and 9 deletions
|
|
@ -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
|
||||
*/
|
||||
|
|
|
|||
88
src/server.c
88
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
191
src/sock.c
191
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) {
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue