From 5304db9a38572e2662c5c62e6accf13b9ecfe65a Mon Sep 17 00:00:00 2001 From: Alexander Couzens Date: Wed, 2 Nov 2016 21:31:49 +0100 Subject: [PATCH] implement lapd decoding/encoding and rx/tx data path --- siu/l2tp/Makefile | 2 +- siu/l2tp/l2tp_protocol.h | 4 + siu/l2tp/l2tpd.c | 13 +- siu/l2tp/l2tpd.h | 16 ++- siu/l2tp/l2tpd_fsm.c | 15 +++ siu/l2tp/l2tpd_lapd.c | 260 +++++++++++++++++++++++++++++++++++++++ siu/l2tp/l2tpd_lapd.h | 7 ++ siu/l2tp/l2tpd_packet.c | 48 +++++++- 8 files changed, 359 insertions(+), 6 deletions(-) create mode 100644 siu/l2tp/l2tpd_lapd.c create mode 100644 siu/l2tp/l2tpd_lapd.h diff --git a/siu/l2tp/Makefile b/siu/l2tp/Makefile index ad95bd9..ba7bde5 100644 --- a/siu/l2tp/Makefile +++ b/siu/l2tp/Makefile @@ -8,7 +8,7 @@ CFLAGS += $(EXTRA_CFLAGS) all: l2tpd -l2tpd: l2tpd.o l2tpd_fsm.o l2tpd_data.o l2tpd_packet.o l2tpd_socket.c +l2tpd: l2tpd.o l2tpd_fsm.o l2tpd_data.o l2tpd_packet.o l2tpd_socket.c l2tpd_lapd.c $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) %.o: %.c diff --git a/siu/l2tp/l2tp_protocol.h b/siu/l2tp/l2tp_protocol.h index 3424875..77bc1c0 100644 --- a/siu/l2tp/l2tp_protocol.h +++ b/siu/l2tp/l2tp_protocol.h @@ -126,3 +126,7 @@ enum l2tp_eric_ctrlmsg { ERIC_CTRLMSG_ALTCRP = 5, ERIC_CTRLMSG_LCCSN = 6, }; + +#define TC_GROUP_TRAU 0x6 +#define TC_GROUP_PGSL 0x8 +#define TC_GROUP_RSL_OML 0x11 diff --git a/siu/l2tp/l2tpd.c b/siu/l2tp/l2tpd.c index f37fd31..16905b3 100644 --- a/siu/l2tp/l2tpd.c +++ b/siu/l2tp/l2tpd.c @@ -19,6 +19,8 @@ #include "l2tpd_data.h" #include "l2tpd_fsm.h" #include "l2tpd_packet.h" +#include "l2tpd_lapd.h" +#include "l2tpd_socket.h" struct l2tpd_instance *l2i; /* FIXME: global static instance */ @@ -99,12 +101,14 @@ int main(int argc, char **argv) l2i = talloc_zero(tall_l2tp_ctx, struct l2tpd_instance); l2i->cfg.bind_ip = "0.0.0.0"; + l2i->cfg.rsl_oml_path = "/tmp/rsl_oml"; + l2i->cfg.pgsl_path = "/tmp/pgsl_oml"; + l2i->cfg.trau_path = "/tmp/trau_oml"; /* connection id starts with 1 */ l2i->next_l_cc_id = 1; /* session id starts with 1 */ l2i->next_l_sess_id = 1; - rc = l2tpd_instance_start(l2i); if (rc < 0) exit(1); @@ -114,6 +118,13 @@ int main(int argc, char **argv) log_add_target(stderr_target); log_set_print_filename(stderr_target, 0); + l2tp_socket_init(&l2i->rsl_oml.state, l2i->cfg.rsl_oml_path, 100, DL2TP); + l2tp_socket_init(&l2i->trau.state, l2i->cfg.trau_path, 100, DL2TP); + l2tp_socket_init(&l2i->pgsl.state, l2i->cfg.pgsl_path, 100, DL2TP); + + l2tp_set_read_callback(&l2i->rsl_oml.state, unix_rsl_oml_cb); + l2tp_set_read_callback(&l2i->pgsl.state, unix_rsl_oml_cb); + l2tp_set_read_callback(&l2i->trau.state, unix_rsl_oml_cb); while (1) { osmo_select_main(0); diff --git a/siu/l2tp/l2tpd.h b/siu/l2tp/l2tpd.h index e646d63..fe50ef5 100644 --- a/siu/l2tp/l2tpd.h +++ b/siu/l2tp/l2tpd.h @@ -9,6 +9,8 @@ #include #include +#include "l2tpd_socket.h" + static inline void *msgb_l2tph(struct msgb *msg) { return msg->l2h; @@ -73,6 +75,11 @@ struct l2tpd_session { /* TODO: sockets for TRAU and PCU */ }; +struct traffic_channel { + struct l2tp_socket_state state; + struct l2tpd_session *session; +}; + struct l2tpd_instance { /* list of l2tpd_connection */ struct llist_head connections; @@ -81,11 +88,18 @@ struct l2tpd_instance { /* next local session id */ uint32_t next_l_sess_id; - struct osmo_fd l2tp_ofd; + /* unix sockets */ + + struct traffic_channel rsl_oml; + struct traffic_channel trau; + struct traffic_channel pgsl; struct { const char *bind_ip; + const char *rsl_oml_path; + const char *pgsl_path; + const char *trau_path; } cfg; }; diff --git a/siu/l2tp/l2tpd_fsm.c b/siu/l2tp/l2tpd_fsm.c index 01495a8..f1e3d3d 100644 --- a/siu/l2tp/l2tpd_fsm.c +++ b/siu/l2tp/l2tpd_fsm.c @@ -1,6 +1,8 @@ #include +#include "l2tp_protocol.h" + #include "l2tpd.h" #include "l2tpd_packet.h" #include "l2tpd_data.h" @@ -297,12 +299,25 @@ static void l2tp_ic_s_wait_conn(struct osmo_fsm_inst *fi, uint32_t event, void * if (!l2tp_tx_ack(l2c)) { osmo_fsm_inst_state_chg(fi, L2IC_S_ESTABLISHED, 0, 0); osmo_fsm_inst_dispatch(l2c->conf_fsm, L2CONF_E_ESTABLISH_SESSION, data); + switch (l2s->remote_end_id) { + /* FIXME: kick out the old session */ + case TC_GROUP_PGSL: + l2i->pgsl.session = l2s; + break; + case TC_GROUP_RSL_OML: + l2i->rsl_oml.session = l2s; + break; + case TC_GROUP_TRAU: + l2i->trau.session = l2s; + break; + } } } } static void l2tp_ic_s_established(struct osmo_fsm_inst *fi, uint32_t event, void *data) { + /* FIXME: remove old session if it got dealloc from l2i->trau.session etc. */ } diff --git a/siu/l2tp/l2tpd_lapd.c b/siu/l2tp/l2tpd_lapd.c new file mode 100644 index 0000000..9b6095a --- /dev/null +++ b/siu/l2tp/l2tpd_lapd.c @@ -0,0 +1,260 @@ + + +#include +#include +#include +#include + +#include "l2tp_protocol.h" + +#include "l2tpd.h" +#include "l2tpd_lapd.h" +#include "l2tpd_packet.h" +#include "l2tpd_socket.h" + +/* lapd and ehdlc differs in the first 16 bit + * lapd saves tei, sapi, c/r bit, ea1, ea2 bit + * ehdlc saves tei, sapi, c/r and length in a compressed way, + * meaining tei & sapi are limited to certain values */ + +#define LAPD_SAPI_SHIFT 10 +#define LAPD_SAPI_MASK 0xfc00 +#define LAPD_TEI_SHIFT 1 +#define LAPD_TEI_MASK 0x00fe +#define LAPD_CR_BIT_MASK 0x0200 +#define LAPD_EA2_BIT_MASK 0x0001 + +#define EHDLC_CSAPI_MASK 0xe000 +#define EHDLC_CSAPI_SHIFT 13 +#define EHDLC_CTEI_MASK 0x1e00 +#define EHDLC_CTEI_SHIFT 9 +#define EHDLC_LENGTH_MASK 0x01ff + +/*! + * \brief csapi_to_sapi + * \param csapi + * \return sapi + */ +static int csapi_to_sapi(int csapi) +{ + switch (csapi) { + case 0: + case 1: + return 0; + case 2: + return 10; + + case 3: + return 11; + case 4: + return 12; + case 5: + case 6: + return 62; + default: + return -1; + } +} + +/*! + * \brief csapi_to_cr + * \param csapi + * \return cr bit + */ +static int csapi_to_cr(int csapi) +{ + switch (csapi) { + case 0: + return 0; + case 1: + return 1; + case 2: + case 3: + case 4: + case 5: + return 0; + case 6: + return 1; + default: + return -1; + } +} + +/*! + * \brief sapi_to_csapi + * \param sapi + * \param cr c/r bit + * \return + */ +static int sapi_to_csapi(int sapi, int cr) +{ + switch (sapi) { + case 0: + return cr ? 1 : 0; + case 10: + return 2; + case 11: + return 3; + case 12: + return 4; + case 62: + return cr ? 6 : 5; + default: + return -1; + } +} + +static int tei_to_ctei(int tei) +{ + if (tei >= 0 && tei <= 11) + return tei; + else if (tei >= 60 && tei <= 63) + return tei - 48; + else /* invalid */ + return -1; +} + +static int ctei_to_tei(int ctei) +{ + if (ctei >= 0 && ctei <= 11) + return ctei; + else if (ctei >= 12 && ctei <= 15) + return ctei + 48; + else /* invalid */ + return -1; +} + +int lapd_lapd_to_ehdlc(struct l2tpd_instance *l2i, struct msgb *msg) +{ + uint16_t lapd_address = osmo_load16be(msgb_data(msg)); + uint16_t ehdlc_compressed = 0; + int sapi = lapd_address >> LAPD_SAPI_SHIFT; + int tei = (lapd_address & LAPD_TEI_MASK) >> LAPD_TEI_SHIFT; + int cr = lapd_address & LAPD_CR_BIT_MASK; + int length = msgb_length(msg); + + ehdlc_compressed |= (sapi_to_csapi(sapi, cr) << EHDLC_CSAPI_SHIFT) & EHDLC_CSAPI_MASK; + ehdlc_compressed |= (tei_to_ctei(tei) << EHDLC_CTEI_SHIFT) & EHDLC_CTEI_MASK; + ehdlc_compressed |= length & EHDLC_LENGTH_MASK; + + osmo_store16be(ehdlc_compressed, msgb_data(msg)); + + return l2tp_tx_data(msg); +} + +/*! + * \brief lapd_ehdlc_to_lapd + * \param l2i + * \param session which received the packets + * \param msg + * \return + */ +int lapd_ehdlc_to_lapd(struct l2tpd_instance *l2i, struct l2tpd_session *l2s, struct msgb *msg) +{ + struct traffic_channel *channel = NULL; + switch (l2s->remote_end_id) { + case TC_GROUP_PGSL: + channel = &l2i->pgsl; + break; + + case TC_GROUP_RSL_OML: + channel = &l2i->rsl_oml; + break; + case TC_GROUP_TRAU: + channel = &l2i->trau; + break; + } + + if (!channel) { + LOGP(DL2TP, LOGL_NOTICE, "Can not find traffic channel for session %d\n", l2s->l_sess_id); + return -1; + } + + /* FIXME: do we have to sent empty packets ? */ + while (msgb_length(msg) > 2) { + struct msgb *send_msg; + uint16_t lapd_address = 0; + uint16_t ehdlc_compressed = osmo_load16be(msgb_data(msg)); + int csapi = (ehdlc_compressed & EHDLC_CSAPI_MASK) >> EHDLC_CSAPI_SHIFT; + int ctei = (ehdlc_compressed & EHDLC_CTEI_MASK) >> EHDLC_CTEI_SHIFT; + int length = (ehdlc_compressed & EHDLC_LENGTH_MASK); + + lapd_address |= (csapi_to_sapi(csapi) << LAPD_SAPI_SHIFT) & LAPD_SAPI_MASK; + lapd_address |= csapi_to_cr(csapi) ? LAPD_CR_BIT_MASK : 0; + lapd_address |= (ctei_to_tei(ctei) << LAPD_TEI_SHIFT) & LAPD_TEI_MASK; + lapd_address |= LAPD_EA2_BIT_MASK; + + osmo_store16be(lapd_address, msgb_data(msg)); + + if (length > msgb_length(msg)) { + LOGP(DL2TP, LOGL_NOTICE, "Can not parse msg as ehdlc because its to short. %d > %d.\n", length, msgb_length(msg)); + return 0; + } + + send_msg = msgb_alloc(length + 128, "lapd frame"); + memcpy(msgb_data(send_msg), msgb_data(msg), length); + msgb_pull(msg, length); + msgb_put(send_msg, length); + + l2tp_socket_enqueue(&channel->state, send_msg); + } + + if (msgb_length(msg) > 0) + LOGP(DL2TP, LOGL_NOTICE, "ehdlc_to_lapd: bytes leftover after parsing %d.\n", msgb_length(msg)); + + return 0; +} + + +/*! + * \brief rsl_oml_cb called when data arrived on the unix socket + * \param fd + * \return 0 on success + */ +int unix_rsl_oml_cb(struct osmo_fd *fd) +{ + struct msgb *msg = l2tp_msgb_alloc(); + int rc; + + struct osmo_wqueue *wqueue = container_of(fd, struct osmo_wqueue, bfd); + struct l2tp_socket_state *state = container_of(wqueue, struct l2tp_socket_state, wqueue); + struct traffic_channel *channel = container_of(state, struct traffic_channel, state); + + rc = read(fd->fd, msg->data, msg->data_len); + if (rc < 0) { + LOGP(DL2TP, LOGL_ERROR, "read failed %s\n", strerror(errno)); + return rc; + } else if (rc == 0) { + LOGP(DL2TP, LOGL_ERROR, "closing socket because read 0 bytes\n"); + l2tp_sock_cleanup(fd); + return 0; + } + if (rc > 3) { + LOGP(DL2TP, LOGL_ERROR, "read %d\n", rc); + } + msgb_put(msg, rc); + msg->dst = channel->session; + + if (!channel->session) { + LOGP(DL2TP, LOGL_NOTICE, "Drop packets.\n"); + msgb_free(msg); + return 1; + } + + rc = lapd_lapd_to_ehdlc(l2i, msg); + if (rc) { + LOGP(DL2TP, LOGL_NOTICE, "lapd_to_ehlc returned != 0: %d.\n", rc); + } + + return 0; +} + +int unix_trau_cb(struct osmo_fd *fd) +{ + return 0; +} + +int unix_pgsl_cb(struct osmo_fd *fd) +{ + return 0; +} diff --git a/siu/l2tp/l2tpd_lapd.h b/siu/l2tp/l2tpd_lapd.h new file mode 100644 index 0000000..ba2c240 --- /dev/null +++ b/siu/l2tp/l2tpd_lapd.h @@ -0,0 +1,7 @@ +#pragma once + +int lapd_lapd_to_ehdlc(struct l2tpd_instance *l2i, struct msgb *msg); +int lapd_ehdlc_to_lapd(struct l2tpd_instance *l2i, struct l2tpd_session *session, struct msgb *msg); +int unix_rsl_oml_cb(struct osmo_fd *fd); +int unix_trau_cb(struct osmo_fd *fd); +int unix_pgsl_cb(struct osmo_fd *fd); diff --git a/siu/l2tp/l2tpd_packet.c b/siu/l2tp/l2tpd_packet.c index e69dd72..3b667df 100644 --- a/siu/l2tp/l2tpd_packet.c +++ b/siu/l2tp/l2tpd_packet.c @@ -18,6 +18,7 @@ #include "l2tpd.h" #include "l2tpd_data.h" #include "l2tpd_fsm.h" +#include "l2tpd_lapd.h" #include "crc32.h" /*********************************************************************** @@ -886,10 +887,50 @@ static int l2tp_rcvmsg_control(struct msgb *msg) return -1; } -static int l2tp_rcvmsg_data(struct msgb *msg, bool ip_transport) +static int l2tp_rcvmsg_data(struct msgb *msg) { - DEBUGP(DL2TP, "rx data: %s\n", msgb_hexdump(msg)); - return 0; + struct l2tp_data_hdr *hdr; + struct l2tpd_session *l2s; + int len = msgb_length(msg); + uint32_t sequence_id = 0; + + if (len < sizeof(*hdr)) { + LOGP(DL2TP, LOGL_INFO, "Received data packet which is to small %d < %d.\n", len, 12); + return -1; + } + + hdr = (struct l2tp_data_hdr *) msgb_data(msg); + hdr->session_id = htonl(hdr->session_id); + hdr->sequence_id = htonl(hdr->sequence_id); + hdr->crc = htonl(hdr->crc); + sequence_id = hdr->sequence_id & L2TP_DATA_SEQ_ID_MASK; + msgb_pull(msg, sizeof(*hdr)); + + l2s = l2tpd_sess_find_by_l_s_id(l2i, hdr->session_id); + if (!l2s) { + LOGP(DL2TP, LOGL_INFO, "session %u: Received data packet for an unknown session.\n", hdr->session_id); + return -1; + } + + if (!(hdr->sequence_id & L2TP_DATA_SEQ_BIT)) { + LOGP(DL2TP, LOGL_INFO, "session %u: Ignoring packets because of missing seq bit.\n", hdr->session_id); + return -1; + } + + /* check sequence id */ + if (sequence_id < l2s->next_rx_seq_nr) { + LOGP(DL2TP, LOGL_DEBUG, "session %d: Received old data packet %d.\n", l2s->l_sess_id, sequence_id); + return -1; + } else if (sequence_id > l2s->next_rx_seq_nr) { + LOGP(DL2TP, LOGL_DEBUG, "session %d: Received a data packet of the future %d.\n", l2s->l_sess_id, hdr->session_id); + } else { + l2s->next_rx_seq_nr++; + } + + /* ignore crc on rx */ + struct msgb *send_msg = msgb_copy(msg, "data_to_unix"); + + return lapd_ehdlc_to_lapd(l2i, l2s, send_msg); } int l2tp_rcvmsg(struct msgb *msg) @@ -900,6 +941,7 @@ int l2tp_rcvmsg(struct msgb *msg) msgb_pull(msg, sizeof(session)); return l2tp_rcvmsg_control(msg); } else { + l2tp_rcvmsg_data(msg); LOGP(DL2TP, LOGL_ERROR, "Received session %d data.\n", session); } return -1;