From 89195bde69a49bb6aa9ddafb358c1892607105cf Mon Sep 17 00:00:00 2001 From: XANTRONIX Development Date: Sun, 28 Jun 2020 14:00:50 -0400 Subject: [PATCH] Holy crap, getting deeper and deeper --- include/patty/ax25/frame.h | 26 +++---- src/frame.c | 144 +++++++++++++++++++++++-------------- src/server.c | 142 +++++++++++++++++++++++++----------- 3 files changed, 199 insertions(+), 113 deletions(-) diff --git a/include/patty/ax25/frame.h b/include/patty/ax25/frame.h index 2fd3ca0..e03400e 100644 --- a/include/patty/ax25/frame.h +++ b/include/patty/ax25/frame.h @@ -66,10 +66,19 @@ ssize_t patty_ax25_frame_decode(patty_ax25_frame *frame, size_t len); ssize_t patty_ax25_frame_encode(patty_ax25_frame *frame, + void *buf, enum patty_ax25_frame_format format, - void *dest, size_t len); +ssize_t patty_ax25_frame_encode_reply_to(patty_ax25_frame *frame, + void *buf, + enum patty_ax25_frame_format format, + uint16_t control, + uint8_t proto, + void *info, + size_t infolen, + size_t len); + enum patty_ax25_version patty_ax25_frame_version(patty_ax25_frame *frame); enum patty_ax25_frame_type patty_ax25_frame_type(patty_ax25_frame *frame); @@ -79,25 +88,10 @@ enum patty_ax25_frame_cr patty_ax25_frame_cr(patty_ax25_frame *frame); ssize_t patty_ax25_frame_info(patty_ax25_frame *frame, void **info); -void patty_ax25_frame_copy_reply_address(patty_ax25_frame *rx, - patty_ax25_frame *tx); - void patty_ax25_frame_set(patty_ax25_frame *frame, uint16_t control, uint8_t proto, void *info, size_t infolen); -void patty_ax25_frame_set_ui(patty_ax25_frame *frame, - uint8_t flags, - uint8_t proto, - void *info, - size_t infolen); - -void patty_ax25_frame_set_sabme(patty_ax25_frame *frame, - enum patty_ax25_frame_u_flags flags); - -void patty_ax25_frame_set_sabm(patty_ax25_frame *frame, - enum patty_ax25_frame_u_flags flags); - #endif /* _PATTY_AX25_FRAME_H */ diff --git a/src/frame.c b/src/frame.c index 09ca9eb..8783f8b 100644 --- a/src/frame.c +++ b/src/frame.c @@ -241,23 +241,63 @@ error_toobig: return -1; } +static ssize_t encode_reply_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->src, sizeof(patty_ax25_addr)); + offset += sizeof(patty_ax25_addr); + ((uint8_t *)dest)[offset-1] &= ~1; + + memcpy((uint8_t *)dest + offset, &frame->dest, sizeof(patty_ax25_addr)); + offset += sizeof(patty_ax25_addr); + ((uint8_t *)dest)[offset-1] &= ~1; + + for (i=0; irepeaters[i].callsign[0] == '\0') { + break; + } + + memcpy((uint8_t *)dest + offset, + &frame->repeaters[hops-1-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, + void *buf, enum patty_ax25_frame_format format, - void *dest, size_t len) { size_t offset = 0; - offset = encode_address(frame, dest, len); + offset = encode_address(frame, buf, len); switch (format) { case PATTY_AX25_FRAME_NORMAL: - ((uint8_t *)dest)[offset++] = frame->control; + ((uint8_t *)buf)[offset++] = frame->control; break; case PATTY_AX25_FRAME_EXTENDED: - ((uint8_t *)dest)[offset++] = (frame->control & 0xff00) >> 8; - ((uint8_t *)dest)[offset++] = frame->control & 0xf00ff; + ((uint8_t *)buf)[offset++] = (frame->control & 0xff00) >> 8; + ((uint8_t *)buf)[offset++] = frame->control & 0xf00ff; break; } @@ -267,9 +307,9 @@ ssize_t patty_ax25_frame_encode(patty_ax25_frame *frame, goto error_toobig; } - ((uint8_t *)dest)[offset++] = frame->proto; + ((uint8_t *)buf)[offset++] = frame->proto; - memcpy((uint8_t *)dest + offset, frame->info, frame->infolen); + memcpy((uint8_t *)buf + offset, frame->info, frame->infolen); offset += frame->infolen; } @@ -280,6 +320,49 @@ error_toobig: return -1; } +ssize_t patty_ax25_frame_encode_reply_to(patty_ax25_frame *frame, + void *buf, + enum patty_ax25_frame_format format, + uint16_t control, + uint8_t proto, + void *info, + size_t infolen, + size_t len) { + size_t offset = 0; + + offset = encode_reply_address(frame, buf, len); + + switch (format) { + case PATTY_AX25_FRAME_NORMAL: + ((uint8_t *)buf)[offset++] = control; + + break; + + case PATTY_AX25_FRAME_EXTENDED: + ((uint8_t *)buf)[offset++] = (control & 0xff00) >> 8; + ((uint8_t *)buf)[offset++] = control & 0xf00ff; + + break; + } + + if (PATTY_AX25_CONTROL_INFO(control)) { + if (1 + offset + infolen > len) { + goto error_toobig; + } + + ((uint8_t *)buf)[offset++] = proto; + + memcpy((uint8_t *)buf + offset, info, infolen); + + offset += 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)? @@ -320,23 +403,6 @@ 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; irepeaters[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, @@ -347,33 +413,3 @@ void patty_ax25_frame_set(patty_ax25_frame *frame, 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) { - patty_ax25_frame_set(frame, - PATTY_AX25_FRAME_U_UI | flags, - proto, - info, - infolen); -} - -void patty_ax25_frame_set_sabme(patty_ax25_frame *frame, - enum patty_ax25_frame_u_flags flags) { - patty_ax25_frame_set(frame, - PATTY_AX25_FRAME_U_SABME | flags, - PATTY_AX25_PROTO_NONE, - NULL, - 0); -} - -void patty_ax25_frame_set_sabm(patty_ax25_frame *frame, - enum patty_ax25_frame_u_flags flags) { - patty_ax25_frame_set_ui(frame, - PATTY_AX25_FRAME_U_SABM | flags, - PATTY_AX25_PROTO_NONE, - NULL, - 0); -} diff --git a/src/server.c b/src/server.c index 821bfd8..003c299 100644 --- a/src/server.c +++ b/src/server.c @@ -287,6 +287,17 @@ error_dict_delete_established: return -1; } +static int sock_delete_by_addr(patty_dict *dict, + patty_ax25_addr *addr) { + uint32_t hash; + + patty_hash_init(&hash); + patty_ax25_addr_hash(&hash, addr); + patty_hash_end(&hash); + + return patty_dict_delete_with_hash(dict, hash); +} + static int sock_delete_by_addrpair(patty_dict *dict, patty_ax25_addr *local, patty_ax25_addr *remote) { @@ -1041,67 +1052,112 @@ static int handle_clients(patty_ax25_server *server) { return patty_dict_each(server->clients, handle_client, server); } -static void copy_reply_repeaters(patty_ax25_addr *to, - patty_ax25_addr *from, - unsigned int hops) { +static void save_reply_addr(patty_ax25_sock *sock, + patty_ax25_frame *frame) { unsigned int i, - max = hops > PATTY_AX25_MAX_HOPS? - PATTY_AX25_MAX_HOPS: hops; + hops = frame->hops > PATTY_AX25_MAX_HOPS? + PATTY_AX25_MAX_HOPS: hops; - memset(to, '\0', PATTY_AX25_MAX_HOPS * sizeof(patty_ax25_addr)); + memcpy(&sock->local, &frame->dest, sizeof(patty_ax25_addr)); + memcpy(&sock->remote, &frame->src, sizeof(patty_ax25_addr)); - for (i=0; irepeaters[i], + &frame->repeaters[hops-1-i], + sizeof(patty_ax25_addr)); } + + sock->hops = hops; } -static int handle_frame_rx(patty_ax25_server *server, - patty_ax25_if *iface, - void *buf, - size_t len) { - patty_ax25_frame frame_rx, - frame_tx; +static int reply_to(patty_ax25_frame *frame, + patty_ax25_if *iface, + enum patty_ax25_frame_format format, + uint16_t control, + uint8_t proto, + void *info, + size_t infolen) { + ssize_t len; - if (patty_ax25_frame_decode(&frame_rx, buf, len) < 0) { + if ((len = patty_ax25_frame_encode_reply_to(frame, + iface->tx_buf, + PATTY_AX25_FRAME_NORMAL, + PATTY_AX25_FRAME_U_DISC, + PATTY_AX25_PROTO_NONE, + NULL, + 0, + iface->tx_bufsz)) < 0) { + goto error_toobig; + } + + return patty_ax25_if_send(iface, iface->tx_buf, len); + +error_toobig: + return -1; +} + +static int reply_dm(patty_ax25_frame *frame, + patty_ax25_if *iface, + enum patty_ax25_frame_u_flags flags) { + return reply_to(frame, + iface, + PATTY_AX25_FRAME_NORMAL, + PATTY_AX25_FRAME_U_DM | flags, + PATTY_AX25_PROTO_NONE, + NULL, + 0); +} + +static int reply_ua(patty_ax25_frame *frame, + patty_ax25_if *iface, + enum patty_ax25_frame_u_flags flags) { + return reply_to(frame, + iface, + PATTY_AX25_FRAME_NORMAL, + PATTY_AX25_FRAME_U_UA | flags, + PATTY_AX25_PROTO_NONE, + NULL, + 0); +} + +static int handle_frame(patty_ax25_server *server, + patty_ax25_if *iface, + void *buf, + size_t len) { + patty_ax25_frame frame; + + if (patty_ax25_frame_decode(&frame, + PATTY_AX25_FRAME_NORMAL, + buf, + len) < 0) { goto error_io; } - if (PATTY_AX25_CONTROL_UNNUMBERED_SABM(frame_rx.control)) { + if (PATTY_AX25_CONTROL_UNNUMBERED_SABM(frame.control)) { patty_ax25_sock *sock; if ((sock = sock_by_addr(server->socks_pending_accept, - &frame_rx.dest)) == NULL) { - - if (send_dm(iface, &frame.src, &frame.dest, NULL) < 0) { + &frame.dest)) == NULL) { + if (reply_dm(&frame, iface, PATTY_AX25_FRAME_U_FINAL) < 0) { goto error_io; } + } - sock->status = PATTY_AX25_SOCK_ESTABLISHED; - sock->mode = PATTY_AX25_SOCK_SABM; + sock->status = PATTY_AX25_SOCK_ESTABLISHED; + sock->mode = PATTY_AX25_SOCK_SABM; - memcpy(&sock->local, &frame.dest, sizeof(patty_ax25_sock)); - memcpy(&sock->remote, &frame.src, sizeof(patty_ax25_sock)); + save_reply_addr(sock, &frame); - copy_reply_repeaters(&sock->repeaters, - &frame.repeaters, - sock->hops); + if (sock_save_by_addrpair(server->socks_established, + sock, + &sock->local, + &sock->remote) < 0) { + goto error_io; + } - if (sock_delete_by_addrpair(server->socks_pending_accept, - &sock->local, - &sock->remote) < 0) { - goto error_io; - } - - if (sock_save_by_addrpair(server->socks_established, - sock, - &sock->local, - &sock->remote) < 0) { - goto error_io; - } - } else { - if (send_frmr(iface, &frame.src, &frame.dest, NULL) < 0) { - goto error_io; - } + if (sock_delete_by_addr(server->socks_pending_accept, + &frame.dest) < 0) { + goto error_io; } } @@ -1129,7 +1185,7 @@ static int handle_iface(patty_ax25_server *server, patty_ax25_if *iface) { goto done; } - if (handle_frame_rx(server, iface, buf, readlen) < 0) { + if (handle_frame(server, iface, buf, readlen) < 0) { goto error_io; }