From cf2570bdba6ca30496ed67663852037da97bc99b Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Mon, 23 Sep 2019 20:23:29 +0200 Subject: WIP: SABP server Change-Id: Iff1d33c7482ff767aa5446a8ccfe9086fee57365 --- src/sabp_common.c | 157 +++++++++++++++++++++++++++++++++++++++++++++++-- src/sabp_server.c | 171 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/sabp_server.h | 41 +++++++++++++ 3 files changed, 365 insertions(+), 4 deletions(-) create mode 100644 src/sabp_server.c create mode 100644 src/sabp_server.h diff --git a/src/sabp_common.c b/src/sabp_common.c index cdb0e32..75c89ef 100644 --- a/src/sabp_common.c +++ b/src/sabp_common.c @@ -1,6 +1,6 @@ /* common SABP code */ -/* (C) 2015 by Harald Welte +/* (C) 2019 by Harald Welte * All Rights Reserved * * This program is free software; you can redistribute it and/or modify @@ -19,6 +19,11 @@ */ #include +#include +#include +#include + +#include #include @@ -73,14 +78,17 @@ const struct value_string sabp_cause_vals[] = { { 0, NULL } }; -static struct msgb *sabp_msgb_alloc(void) +struct msgb *osmo_sabp_msgb_alloc(void *ctx, const char *name) { - return msgb_alloc_headroom(1024+512, 512, "SABP Tx"); + /* make the messages rather large as the cell lists can be long! */ + return msgb_alloc_headroom_c(ctx, 65535, 16, name); } +extern void *tall_msgb_ctx; + static struct msgb *_sabp_gen_msg(SABP_SABP_PDU_t *pdu) { - struct msgb *msg = sabp_msgb_alloc(); + struct msgb *msg = osmo_sabp_msgb_alloc(tall_msgb_ctx, __func__); asn_enc_rval_t rval; if (!msg) @@ -213,3 +221,144 @@ void sabp_set_log_area(int log_area) { _sabp_DSABP = log_area; } + +/*********************************************************************** + * Message Reception + ***********************************************************************/ + +/* SABP was specified as ASN.1 APER encoded messages *directly* inside a TCP + * stream, without any intermediate framing layer. As TCP doesn't preserve + * message boundaries, and SABP messages are variable length, we need to + * actually parse the message up to the point of the APER-internal length + * determinant (which itself is variable-length encoded). */ + +/* Three bytes for TriggeringMessage, procedureCode and Criticality */ +#define SABP_HDR_LEN 3 + + +/*! parse a single APER length determinant. + * \param[in] in input data as received from peer + * \param[in] in_len length of 'in' + * \param[out] len_len Length of the current length determinant in octets + * \returns parsed length or -EAGAIN if more data is needed, or -EIO in case of parse error */ +static int parse_aper_len_det(const uint8_t *in, unsigned int in_len, unsigned int *len_len) +{ + /* Variable-length Length encoding according to X.961 Section 9.1 NOTE 2 */ + switch (in[0] & 0xC0) { + case 0x00: + /* total length is encoded in this octet */ + *len_len = 1; + return in[0]; + case 0x80: + /* total length (up to 16k) encoded in two octets */ + *len_len = 2; + if (in_len < 2) + return -EAGAIN; /* we need one more byte */ + return ((in[0] & 0x3F) << 8) | in[1]; + case 0xC0: + /* total length not known, encoded in chunks; first chunk length now known */ + *len_len = 1; + return (in[0] & 0x3f) * 16384; + /* we must read the chunk and then look at the variable-length encoded length + * of the next chunk */ + default: + return -EIO; + } +} + +/* msg->l1h points to first byte of current length determinant */ +#define MSGB_LEN_NEEDED(msg) (msg)->cb[0] /* total length of msgb needed (as known so far) */ +#define MSGB_APER_STATE(msg) (msg)->cb[1] /* '1' if we're expecting another length determinant */ + +/*! Read one SABP message from socket fd or store part if still not fully received. + * \param[in] ctx talloc context from which to allocate new msgb. + * \param[in] fd The fd for the socket to read from. + * \param[out] rmsg internally allocated msgb containing a fully received SABP message. + * \param[inout] tmp_msg internally allocated msgb caching data for not yet fully received message. + * + * Function is designed just like ipa_msg_recv_buffered() + */ +int osmo_sabp_recv_buffered(void *ctx, int fd, struct msgb **rmsg, struct msgb **tmp_msg) +{ + struct msgb *msg = tmp_msg ? *tmp_msg : NULL; + unsigned int len_len; + bool first_call = false; + int rc; + + if (!msg) { + msg = osmo_sabp_msgb_alloc(ctx, __func__); + if (!msg) + return -ENOMEM; + } + + if (!msgb_l1(msg)) { + /* new msgb; we expect first length determinant after 3 bytes */ + msg->l1h = msgb_data(msg) + SABP_HDR_LEN; + MSGB_LEN_NEEDED(msg) = SABP_HDR_LEN + 1; /* 1 == minimum length of length-field */ + first_call = true; + } + + /* attempt to receive missing/needed amount of bytes */ + rc = recv(fd, msg->tail, MSGB_LEN_NEEDED(msg)-msgb_length(msg), 0); + if (rc == 0) + goto discard_msg; /* dead socket */ + else if (rc < 0) { + if (errno == EAGAIN || errno == EINTR) + rc = 0; + else { + rc = -errno; + goto discard_msg; + } + } + msgb_put(msg, rc); + + /* check if we have received sufficient amount of data */ + if (msgb_length(msg) < MSGB_LEN_NEEDED(msg)) { + if (msg->len == 0) { + rc = -EAGAIN; + goto discard_msg; + } + + if (!tmp_msg) { + rc = -EIO; + goto discard_msg; + } + *tmp_msg = msg; + return -EAGAIN; + } + + if (!first_call && MSGB_APER_STATE(msg) == 0) { + /* we have read all needed bytes by now, and hence can return successfully */ + return msgb_length(msg); + } + + /* below code is only executed on first call/iteration, or if last length-det was chunked */ + OSMO_ASSERT(first_call || MSGB_APER_STATE(msg) == 1); + + OSMO_ASSERT(msg->l1h < msg->tail); + rc = parse_aper_len_det(msgb_l1(msg), msgb_l1len(msg), &len_len); + if (rc == -EIO) { + return -EIO; + } else if (rc == -EAGAIN) { + /* need (typically 1 byte) more data */ + OSMO_ASSERT(len_len > 1); + MSGB_LEN_NEEDED(msg) += len_len-1; + return -EAGAIN; + } else if (rc >= 16384) { + /* read up to next length determinant */ + MSGB_LEN_NEEDED(msg) += len_len-1 + rc + 1 /* new length determinant */; + msg->l1h += (len_len-1) + rc; /* start of next length determinant */ + MSGB_APER_STATE(msg) = 1; + return -EAGAIN; + } else { + /* full length is known now */ + MSGB_LEN_NEEDED(msg) += len_len-1 + rc; + return -EAGAIN; + } + +discard_msg: + if (tmp_msg) + *tmp_msg = NULL; + msgb_free(msg); + return rc; +} diff --git a/src/sabp_server.c b/src/sabp_server.c new file mode 100644 index 0000000..6d67a5e --- /dev/null +++ b/src/sabp_server.c @@ -0,0 +1,171 @@ +/* (C) 2019 by Harald Welte + * All Rights Reserved + * + * SPDX-License-Identifier: AGPL-3.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Affero General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see . + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "sabp_server.h" + +static int sabp_cbc_read_cb(struct osmo_stream_srv *conn) +{ + struct osmo_stream_srv_link *link = osmo_stream_srv_get_master(conn); + struct osmo_sabp_server_client *client = osmo_stream_srv_get_data(conn); + struct osmo_sabp_server *srv = osmo_stream_srv_link_get_data(link); + struct osmo_fd *ofd = osmo_stream_srv_get_ofd(conn); + struct msgb *msg = NULL; + int rc; + + LOGPSC(client, LOGL_DEBUG, "read_cb rx_msg=%p\n", client->rx_msg); + + /* message de-segmentation */ + rc = osmo_sabp_recv_buffered(conn, ofd->fd, &msg, &client->rx_msg); + if (rc < 0) { + if (rc == -EAGAIN || rc == -EINTR) { + /* more data needs to be read */ + return 0; + } else if (rc == -EPIPE || rc == -ECONNRESET) { + /* lost connection with server */ + } else if (rc == 0) { + /* connection closed with server */ + } + /* destroy connection */ + osmo_stream_srv_destroy(conn); + return -EBADF; + } + OSMO_ASSERT(msg); + LOGPSC(client, LOGL_DEBUG, "Received SABP %s\n", msgb_hexdump(msg)); + /* decode + dispatch message */ + decoded =...; + if (decoded) { + LOGPSC(client, LOGL_INFO, "Received SABP %s\n", + get_value_string(FIXME)); + srv->rx_cb(client, decoded); + } else { + LOGPSC(client, LOGL_ERROR, "Unable to decode %s\n", msgb_hexdump(msg)); + } +} + +static int sabp_server_closed_cb(struct osmo_stream_srv *conn) +{ + struct osmo_sabp_server_client *client = osmo_stream_srv_get_data(conn); + LOGSC(client, LOGL_INFO, "connection closed\n"); + llist_del(&client->list); + osmo_fsm_inst_term(client->fi, OSMO_FSM_TERM_REQUEST, NULL); + talloc_free(client); + return 0; +} + +static int sabp_server_accept_cb(struct omso_stream_srv_link *link, int fd) +{ + struct osmo_sabp_server *srv = osmo_stream_srv_link_get_data(link); + struct osmo_sabp_server_client *client = talloc_zero(srv, struct osmo_sabp_server_client); + char remote_ip[INET6_ADDRSTRLEN], portbuf[6]; + int remote_port; + OSMO_ASSERT(client); + + remote_ip[0] = '\0'; + portbuf[0] = '\0'; + osmo_sock_get_ip_and_port(fd, remote_ip, sizeof(remote_ip), portbuf, sizeof(portbuf), false); + remote_port = atoi(portbuf); + + client->conn = osmo_stream_srv_create(link, lnk, fd, sabp_server_read_cb, + sabp_srever_closed_cb, client); + if (!client->conn) { + LOGP(DSABP, LOGL_ERROR, "Unable to create stream server for %s:%d\n", + remote_ip, remote_port); + talloc_free(client); + return -1; + } + client->fi = osmo_fsm_inst_alloc(&sabp_server_fsm, client, client, LOGL_DEBUG, NULL); + if (!client->fi) { + LOGPSC(client, LOGL_ERROR, "Unable to allocate FSM\n"); + osmo_stream_srv_destroy(client->conn); + talloc_free(client); + return -1; + } + llist_add_tail(&client->lits, &srv->clients); + + /* TODO: Match client to peer? */ + + LOGPSC(client, LOGL_INFO, "New SABP client connection\n"); + osmo_fsm_inst_dispatch(client->fi, SABP_SRV_E_CMD_RESET, NULL); + + return 0; +} + +#if 0 +void sabp_server_client_tx(struct osmo_sabp_cbc_client *client, struct SABP_SABP_PDU_t *sabp) +{ + struct msgb *msg = _sabp_gen_msg(pdu); + LOGPSC(client, LOGL_INFO, "Transmitting %s\n", + get_value_string( + if (!msg) { + LOGPSC(client, LOGL_ERROR, "Failed to encode SABP %s\n", + get_value_string()); + ASN_STRUCT_FREE(sabp); + return; + } + ASN_STRUCT_FREE(sabp); + osmo_stream_srv_send(client->conn, msg); +} +#endif + +void sabp_server_client_close(struct osmo_sabp_server_client *client) +{ + osmo_stream_srv_destroy(client->conn); + /* FIXME: do we need to unlink/free the client? */ +} + + +struct osmo_sabp_server *sabp_server_create(void *ctx, const char *bind_ip, int bind_port, + int (*rx_cb)(struct osmo_sabp_server_client *client, + struct SABP_PDU *dec)) +{ + struct osmo_sabp_server *srv = talloc_zero(ctx, struct osmo_sabp_server); + int rc; + + if (bind_port == -1) + bind_port = SABP_TCP_PORT; + + OSMO_ASSERT(srv); + srv->rx_cb = rx_cbp; + INIT_LLIST_HEAD(&srv->clients); + srv->link = osmo_stream_srv_link_create(srv); + osmo_stream_srv_link_set_data(srv->link, srv); + osmo_stream_srv_link_set_nodelay(srv->link, true); + osmo_stream_srv_link_set_port(srv->link, bind_port); + if (bind_ip) + osmo_stream_srv_link_set_addr(srv->link, bind_ip); + osmo_stream_srv_link_set_accept_cb(srv->link, sabp_server_accept_cb); + rc = osmo_stream_serv_link_open(srv->link); + OSMO_ASSERT(rc == 0); + LOGP(DSABP, LOGL_NOTICE, "Listening for SABP at %s\n", + osmo_stream_srv_link_get_sockname(srv->link)); + + return srv; +} diff --git a/src/sabp_server.h b/src/sabp_server.h new file mode 100644 index 0000000..5cb0fb2 --- /dev/null +++ b/src/sabp_server.h @@ -0,0 +1,41 @@ +#pragma once +#include +#include + +#define LOGPSC(client, level, fmt, args...) \ + LOGP(DSABP, level, "%s: " fmt, sabp_server_client_name(client), ## args) + +struct osmo_sabp_server_client; +struct osmo_fsm_inst; + +/* a SABP server */ +struct osmo_sabp_sever { + /* libosmo-netif stream server */ + struct osmo_stream_srv_link *link; + + /* clients connected to this server */ + struct llist_head clients; + + /* receive call-back; called for every received message */ + int (*rx_cb)(struct osmo_sabp_server_client *client, struct SABP_SABP_PDU *dec); +}; + +struct osmo_sabp_server_client { + /* entry in osmo_cbsp_cbc.clients */ + struct llist_head list; + /* stream server connection for this client */ + struct osmo_stream_srv *conn; + /* partially received CBSP message (rx completion pending) */ + struct msgb *rx_msg; + + struct osmo_fsm_inst *fi; + + void *peer; +}; + +const char *sabp_server_client_name(const struct osmo_sabp_server_client *client); + +void sabp_server_client_close(struct osmo_sabp_server_client *client); +struct osmo_sabp_server *sabp_server_create(void *ctx, const char *bind_ip, int bind_port, + int (*rx_cb)(struct osmo_sabp_server_client *client, + struct SABP_SABP_PDU *dec)); -- cgit v1.2.3