diff options
Diffstat (limited to 'src')
225 files changed, 50587 insertions, 5837 deletions
diff --git a/src/Makefile.am b/src/Makefile.am index 99432811..86066466 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -1,69 +1,13 @@ -# This is _NOT_ the library release version, it's an API version. -# Please read chapter "Library interface versions" of the libtool documentation -# before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=14:0:2 - -AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -AM_CFLAGS = -Wall $(TALLOC_CFLAGS) $(PTHREAD_CFLAGS) $(LIBSCTP_CFLAGS) - -if ENABLE_PSEUDOTALLOC -AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc -endif - -lib_LTLIBRARIES = libosmocore.la - -libosmocore_la_LIBADD = $(BACKTRACE_LIB) $(TALLOC_LIBS) $(LIBRARY_RT) $(PTHREAD_LIBS) $(LIBSCTP_LIBS) -libosmocore_la_SOURCES = context.c timer.c timer_gettimeofday.c timer_clockgettime.c \ - select.c signal.c msgb.c bits.c \ - bitvec.c bitcomp.c counter.c fsm.c \ - write_queue.c utils.c socket.c \ - logging.c logging_syslog.c logging_gsmtap.c rate_ctr.c \ - gsmtap_util.c crc16.c panic.c backtrace.c \ - conv.c application.c rbtree.c strrb.c \ - loggingrb.c crc8gen.c crc16gen.c crc32gen.c crc64gen.c \ - macaddr.c stat_item.c stats.c stats_statsd.c prim.c \ - conv_acc.c conv_acc_generic.c sercomm.c prbs.c \ - isdnhdlc.c \ - tdef.c \ - sockaddr_str.c \ - use_count.c \ - $(NULL) - -if HAVE_SSSE3 -libosmocore_la_SOURCES += conv_acc_sse.c -if HAVE_SSE4_1 -conv_acc_sse.lo : AM_CFLAGS += -mssse3 -msse4.1 -else -conv_acc_sse.lo : AM_CFLAGS += -mssse3 -endif - -if HAVE_AVX2 -libosmocore_la_SOURCES += conv_acc_sse_avx.c -if HAVE_SSE4_1 -conv_acc_sse_avx.lo : AM_CFLAGS += -mssse3 -mavx2 -msse4.1 -else -conv_acc_sse_avx.lo : AM_CFLAGS += -mssse3 -mavx2 -endif -endif -endif - -BUILT_SOURCES = crc8gen.c crc16gen.c crc32gen.c crc64gen.c -EXTRA_DIST = conv_acc_sse_impl.h - -libosmocore_la_LDFLAGS = -version-info $(LIBVERSION) -no-undefined - -if ENABLE_PLUGIN -libosmocore_la_SOURCES += plugin.c -libosmocore_la_LIBADD += $(LIBRARY_DLOPEN) -endif - -if ENABLE_MSGFILE -libosmocore_la_SOURCES += msgfile.c -endif - -if ENABLE_SERIAL -libosmocore_la_SOURCES += serial.c -endif - -crc%gen.c: crcXXgen.c.tpl - $(AM_V_GEN)sed -e's/XX/$*/g' $< > $@ +SUBDIRS = \ + core \ + vty \ + isdn \ + codec \ + gsm \ + coding \ + gb \ + ctrl \ + pseudotalloc \ + sim \ + usb \ + $(NULL)
\ No newline at end of file diff --git a/src/codec/Makefile.am b/src/codec/Makefile.am index c9d7a228..bb01b9d3 100644 --- a/src/codec/Makefile.am +++ b/src/codec/Makefile.am @@ -1,10 +1,10 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=1:1:1 +LIBVERSION=4:0:0 -AM_CPPFLAGS = -I$(top_srcdir)/include $(TALLOC_CFLAGS) -AM_CFLAGS = -Wall +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) if ENABLE_PSEUDOTALLOC AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc @@ -13,6 +13,14 @@ endif lib_LTLIBRARIES = libosmocodec.la -libosmocodec_la_SOURCES = gsm610.c gsm620.c gsm660.c gsm690.c ecu.c ecu_fr.c +libosmocodec_la_SOURCES = \ + gsm610.c \ + gsm620.c \ + gsm660.c \ + gsm690.c \ + ecu.c \ + ecu_fr.c \ + ecu_fr_old.c \ + $(NULL) libosmocodec_la_LDFLAGS = -version-info $(LIBVERSION) -no-undefined -libosmocodec_la_LIBADD = $(top_builddir)/src/libosmocore.la +libosmocodec_la_LIBADD = $(top_builddir)/src/core/libosmocore.la diff --git a/src/codec/ecu.c b/src/codec/ecu.c index db7148ce..cdb3e62e 100644 --- a/src/codec/ecu.c +++ b/src/codec/ecu.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /* As the developer and copyright holder of the related code, I hereby @@ -48,7 +44,7 @@ static const struct osmo_ecu_ops *g_ecu_ops[_NUM_OSMO_ECU_CODECS]; /*! initialize an ECU instance for given codec. * \param[in] ctx talloc context from which to allocate - * \parma[in] codec codec for which to initialize/create ECU */ + * \param[in] codec codec for which to initialize/create ECU */ struct osmo_ecu_state *osmo_ecu_init(void *ctx, enum osmo_ecu_codec codec) { if (codec >= ARRAY_SIZE(g_ecu_ops)) @@ -100,6 +96,20 @@ int osmo_ecu_frame_out(struct osmo_ecu_state *st, uint8_t *frame_out) return g_ecu_ops[st->codec]->frame_out(st, frame_out); } +/*! check if the current state of this ECU is a DTX pause. + * \param[in] st ECU state/instance on which to operate + * \return true if DTX pause, false otherwise */ +bool osmo_ecu_is_dtx_pause(struct osmo_ecu_state *st) +{ + if (st->codec >= ARRAY_SIZE(g_ecu_ops)) + return false; + if (!g_ecu_ops[st->codec]) + return false; + if (!g_ecu_ops[st->codec]->is_dtx_pause) + return false; + return g_ecu_ops[st->codec]->is_dtx_pause(st); +} + /*********************************************************************** * low-level API for ECU implementations ***********************************************************************/ diff --git a/src/codec/ecu_fr.c b/src/codec/ecu_fr.c index 4545172a..45ba0cc8 100644 --- a/src/codec/ecu_fr.c +++ b/src/codec/ecu_fr.c @@ -1,9 +1,15 @@ /* * (C) 2017 by sysmocom - s.f.m.c. GmbH * (C) 2017 by Philipp Maier <pmaier@sysmocom.de> - * * All Rights Reserved * + * Significantly reworked in 2023 by Mother + * Mychaela N. Falconia <falcon@freecalypso.org> - however, + * Mother Mychaela's contributions are NOT subject to copyright. + * No rights reserved, all rights relinquished. + * Portions of this code are based on Themyscira libgsmfrp, + * a public domain library by the same author. + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -14,10 +20,46 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + * The present ECU implementation for GSM-FR is closely based on the + * TS 46.011 spec from 3GPP; more specifically, it is based on the + * Example solution presented in Chapter 6 of that spec, adapted for + * libosmocodec ECU architecture, and comes as close to fulfilling + * the spec's officially stated requirements (Chapter 5) as is + * possible within this Osmocom-imposed architecture. Please note + * the following areas where the present implementation fails to + * fulfill the original intent of GSM spec authors: + * + * - The "lost SID" criterion, defined in GSM 06.31, is based on the + * TAF bit from the Radio Subsystem. However, libosmocodec ECU API + * does not include this flag, thus spec requirements related to + * lost SID conditions cannot be implemented in a strictly compliant + * manner. The present implementation improvises its own "lost SID" + * detector (not strictly spec-compliant) by counting frame_out() + * calls in between good traffic frame inputs via frame_in(). + * + * - In the architecture envisioned and assumed in the GSM specs, + * the ECU function of GSM 06.11 was never intended to be a fully + * modular component with its own bona fide I/O interfaces - this + * approach appears to be an Osmocom invention - instead this ECU + * function was intended to be subsumed in the Rx DTX handler + * component of GSM 06.31, also incorporating the comfort noise + * generator of GSM 06.12 - and unlike the narrower-scope ECU, + * this slightly-larger-scope Rx DTX handler is a modular component + * with well-defined I/O interfaces. In the case of BFI conditions + * following a SID, GSM 06.11 spec was written with the assumption + * that the ECU controls the comfort noise generator via internal + * signals, as opposed to emitting "corrected" SID frames on a + * modular interface going to a CN generator located somewhere else. + * Thus the "correct" behavior for a fully modularized ECU is unclear, + * and an argument can be made that the very existence of such a + * fully modularized ECU is incorrect in itself. The present + * implementation re-emits a "rejuvenated" form of the last saved + * SID frame during BFI conditions following a SID within the + * permitted window of 48 frames, then starts emitting muted SIDs + * with Xmaxc decreasing by 4 on each frame, and finally switches + * to emitting non-SID silence frames (Table 1 of TS 46.011) + * once Xmaxc reaches 0. */ #include <stdbool.h> @@ -25,144 +67,216 @@ #include <stdint.h> #include <errno.h> -#include <osmocom/core/bitvec.h> +#include <osmocom/core/prbs.h> -#include <osmocom/codec/gsm610_bits.h> #include <osmocom/codec/codec.h> #include <osmocom/codec/ecu.h> +#include <osmocom/core/linuxlist.h> + +/* See TS 46.011, Chapter 6 Example solution */ +#define GSM611_XMAXC_REDUCE 4 + +/* The first 5 bytes of RTP encoding neatly contain the magic nibble + * and LARc parameters, which also happens to be the part of SID frames + * that needs to be passed through as-is. */ +#define SID_PREFIX_LEN 5 + +enum ecu_principal_state { + STATE_NO_DATA, + STATE_SPEECH, + STATE_SP_MUTING, + STATE_SID, + STATE_SID_MUTING, +}; -/* See also GSM 06.11, chapter 6 Example solution */ -#define GSM610_XMAXC_REDUCE 4 -#define GSM610_XMAXC_LEN 6 +struct fr_ecu_state { + struct osmo_ecu_state ecu_state; + enum ecu_principal_state pr_state; + uint8_t speech_frame[GSM_FR_BYTES]; + uint8_t sid_prefix[SID_PREFIX_LEN]; + uint8_t sid_xmaxc; + uint8_t sid_reemit_count; + struct osmo_prbs_state prng; + bool last_input_was_sid; +}; -/** - * Reduce the XMAXC field. When the XMAXC field reaches - * zero the function will return true. +/* This function is the frame input to the ECU - all inputs to this + * function have been received by the Radio Subsystem as good traffic + * frames in the GSM 06.31 definition. */ -static bool reduce_xmaxcr(struct bitvec *frame_bitvec, - const unsigned int index) +static void fr_ecu_input(struct fr_ecu_state *fr, const uint8_t *frame) { - unsigned int field_index; - uint64_t field; - - field_index = index; - field = bitvec_read_field(frame_bitvec, &field_index, GSM610_XMAXC_LEN); - if (field > GSM610_XMAXC_REDUCE) - field -= GSM610_XMAXC_REDUCE; - else - field = 0; - - field_index = index; - bitvec_write_field(frame_bitvec, &field_index, field, GSM610_XMAXC_LEN); - - return field == 0; + enum osmo_gsm631_sid_class sidc; + + sidc = osmo_fr_sid_classify(frame); + switch (sidc) { + case OSMO_GSM631_SID_CLASS_SPEECH: + memcpy(fr->speech_frame, frame, GSM_FR_BYTES); + fr->pr_state = STATE_SPEECH; + fr->last_input_was_sid = false; + return; + case OSMO_GSM631_SID_CLASS_INVALID: + /* GSM 06.31 section 6.1.2 says: "an invalid SID frame + * shall be substituted by the last valid SID frame + * and the procedure for valid SID frames be applied." + * However, libosmocodec ECU architecture prevents us + * from doing what the spec says: the frame_in() method + * gets a const frame that can't be modified, and + * frame_out() will never get called when BFI=0, even + * when the "good traffic frame" (in the BFI=0 sense) + * is an invalid SID by the bit-counting rule. + * Thus there is no place where we can re-emit a cached + * copy of the last valid SID upon receiving an invalid SID. + * + * In the standard GSM architecture this problem never + * arises because the ECU is not a separate component + * but is coupled with the CN generator, thus the output + * from the Rx DTX handler block will be a CN frame, + * for both valid-SID and invalid-SID inputs to the block. + * But what can we do within the constraints of libosmocodec + * ECU framework? We treat the invalid SID almost like a + * BFI, doing almost nothing in the frame_in() method, + * but we reset sid_reemit_count because by the rules of + * GSM 06.31 an invalid SID is still an accepted SID frame + * for the purpose of "lost SID" logic. */ + fr->sid_reemit_count = 0; + fr->last_input_was_sid = true; + return; + case OSMO_GSM631_SID_CLASS_VALID: + /* save LARc part */ + memcpy(fr->sid_prefix, frame, SID_PREFIX_LEN); + /* save Xmaxc from the last subframe */ + fr->sid_xmaxc = ((frame[27] & 0x1F) << 1) | (frame[28] >> 7); + fr->pr_state = STATE_SID; + fr->sid_reemit_count = 0; + fr->last_input_was_sid = true; + return; + default: + /* There are only 3 possible SID classifications per GSM 06.31 + * section 6.1.1, thus any other return value is a grave error + * in the code. */ + OSMO_ASSERT(0); + } } -/** - * Reduce all XMAXC fields in the frame. When all XMAXC fields - * reach zero, then the function will return true. +/* Reduce all 4 Xmaxc fields in the frame. When all 4 Xmaxc fields + * reach 0, the function will return true for "mute". */ -static bool reduce_xmaxcr_all(struct bitvec *frame_bitvec) +static bool reduce_xmaxc(uint8_t *frame) { - bool silent = true; - - silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC00); - silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC10); - silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC20); - silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC30); - - return silent; + bool mute_flag = true; + uint8_t sub, xmaxc; + + for (sub = 0; sub < 4; sub++) { + xmaxc = ((frame[sub*7+6] & 0x1F) << 1) | (frame[sub*7+7] >> 7); + if (xmaxc > GSM611_XMAXC_REDUCE) { + xmaxc -= GSM611_XMAXC_REDUCE; + mute_flag = false; + } else + xmaxc = 0; + frame[sub*7+6] &= 0xE0; + frame[sub*7+6] |= xmaxc >> 1; + frame[sub*7+7] &= 0x7F; + frame[sub*7+7] |= (xmaxc & 1) << 7; + } + return mute_flag; } -/* Use certain modifications to conceal the errors in a full rate frame */ -static int conceal_frame(uint8_t *frame) +/* TS 46.011 chapter 6, paragraph 4, last sentence: "The grid position + * parameters are chosen randomly between 0 and 3 during this time." + * (The "during this time" qualifier refers to the speech muting state.) + * This sentence in the spec must have been overlooked by previous ECU + * implementors, as this aspect of the muting logic was missing. + */ +static void random_grid_pos(struct fr_ecu_state *fr, uint8_t *frame) { - struct bitvec *frame_bitvec; - unsigned int len; - bool silent; - int rc = 0; - - /* In case we already deal with a silent frame, - * there is nothing to, we just abort immediately */ - if (osmo_fr_check_sid(frame, GSM_FR_BYTES)) - return 0; - - /* Attempt to allocate memory for bitvec */ - frame_bitvec = bitvec_alloc(GSM_FR_BYTES, NULL); - if (!frame_bitvec) - return -ENOMEM; + uint8_t sub; - /* Convert a frame to bitvec */ - len = bitvec_unpack(frame_bitvec, frame); - if (len != GSM_FR_BYTES) { - rc = -EIO; - goto leave; - } - - /* Fudge frame parameters */ - silent = reduce_xmaxcr_all(frame_bitvec); - - /* If we reached silence level, mute the frame - * completely, this also means that we can - * save the bitvec_pack operation */ - if (silent) { - memset(frame, 0x00, GSM_FR_BYTES); - frame[0] = 0xd0; - goto leave; + for (sub = 0; sub < 4; sub++) { + frame[sub*7+6] &= 0x9F; + frame[sub*7+6] |= osmo_prbs_get_ubit(&fr->prng) << 6; + frame[sub*7+6] |= osmo_prbs_get_ubit(&fr->prng) << 5; } +} - /* Convert back to packed byte form */ - len = bitvec_pack(frame_bitvec, frame); - if (len != GSM_FR_BYTES) { - rc = -EIO; - goto leave; +/* Like reduce_xmaxc() above, but for comfort noise rather than speech. */ +static bool reduce_xmaxc_sid(struct fr_ecu_state *fr) +{ + if (fr->sid_xmaxc > GSM611_XMAXC_REDUCE) { + fr->sid_xmaxc -= GSM611_XMAXC_REDUCE; + return false; } - -leave: - bitvec_free(frame_bitvec); - return rc; + fr->sid_xmaxc = 0; + return true; } -/*! - * To be called when a good frame is received. - * This function will then create a backup of the frame - * and reset the internal state. - * \param[in] state The state object for the ECU - * \param[out] frame The valid frame (GSM_FR_BYTES bytes in RTP payload format) +/* This function implements the part which is peculiar to the present + * "standalone" packaging of GSM-FR ECU, without a directly coupled + * comfort noise generator - it re-emits synthetic SID frames during + * DTX pauses, initially unchanged from the saved SID and later muted. */ -void osmo_ecu_fr_reset(struct osmo_ecu_fr_state *state, const uint8_t *frame) +static void reemit_sid(struct fr_ecu_state *fr, uint8_t *frame) { - state->subsequent_lost_frame = false; - memcpy(state->frame_backup, frame, GSM_FR_BYTES); + uint8_t *p, sub; + + memcpy(frame, fr->sid_prefix, SID_PREFIX_LEN); + p = frame + SID_PREFIX_LEN; + for (sub = 0; sub < 4; sub++) { + *p++ = 0; + *p++ = fr->sid_xmaxc >> 1; + *p++ = (fr->sid_xmaxc & 1) << 7; + *p++ = 0; + *p++ = 0; + *p++ = 0; + *p++ = 0; + } } -/*! - * To be called when a bad frame is received. - * This function will then generate a replacement frame - * that can be used to conceal the dropout. - * \param[in] state The state object for the ECU - * \param[out] frame The buffer to fill with GSM_FR_BYTES of replacement frame - * \returns 0 if the frame was sucessfully filled +/* This function is responsible for generating the ECU's output + * in the event that the Radio Subsystem does not have a good + * traffic frame - conditions corresponding to BFI=1 in the specs. */ -int osmo_ecu_fr_conceal(struct osmo_ecu_fr_state *state, uint8_t *frame) +static void fr_ecu_output(struct fr_ecu_state *fr, uint8_t *frame) { - int rc; - - /* For subsequent frames we run the error concealment - * functions on the backed up frame before we restore - * the backup */ - if (state->subsequent_lost_frame) { - rc = conceal_frame(state->frame_backup); - if (rc) - return rc; + bool mute; + + switch (fr->pr_state) { + case STATE_NO_DATA: + memcpy(frame, osmo_gsm611_silence_frame, GSM_FR_BYTES); + return; + case STATE_SPEECH: + /* TS 46.011 chapter 6: "The first lost speech frame is + * replaced at the speech decoder input by the previous + * good speech frame." */ + memcpy(frame, fr->speech_frame, GSM_FR_BYTES); + fr->pr_state = STATE_SP_MUTING; + return; + case STATE_SP_MUTING: + mute = reduce_xmaxc(fr->speech_frame); + memcpy(frame, fr->speech_frame, GSM_FR_BYTES); + random_grid_pos(fr, frame); + if (mute) + fr->pr_state = STATE_NO_DATA; + return; + case STATE_SID: + fr->sid_reemit_count++; + if (fr->sid_reemit_count >= 48) { + fr->pr_state = STATE_SID_MUTING; + reduce_xmaxc_sid(fr); + } + reemit_sid(fr, frame); + return; + case STATE_SID_MUTING: + if (reduce_xmaxc_sid(fr)) { + fr->pr_state = STATE_NO_DATA; + memcpy(frame, osmo_gsm611_silence_frame, GSM_FR_BYTES); + } else + reemit_sid(fr, frame); + return; + default: + /* a severe bug in the state machine! */ + OSMO_ASSERT(0); } - - /* Restore the backed up frame and set flag in case - * we receive even more bad frames */ - memcpy(frame, state->frame_backup, GSM_FR_BYTES); - state->subsequent_lost_frame = true; - - return 0; } /*********************************************************************** @@ -171,44 +285,57 @@ int osmo_ecu_fr_conceal(struct osmo_ecu_fr_state *state, uint8_t *frame) static struct osmo_ecu_state *ecu_fr_init(void *ctx, enum osmo_ecu_codec codec) { - struct osmo_ecu_state *st; - size_t size = sizeof(*st) + sizeof(struct osmo_ecu_fr_state); + struct fr_ecu_state *fr; - st = talloc_named_const(ctx, size, "ecu_state_FR"); - if (!st) - return NULL; + fr = talloc_zero(ctx, struct fr_ecu_state); + fr->ecu_state.codec = codec; + fr->pr_state = STATE_NO_DATA; + osmo_prbs_state_init(&fr->prng, &osmo_prbs15); - memset(st, 0, size); - st->codec = codec; + return (struct osmo_ecu_state *) fr; +} - return st; +static inline struct fr_ecu_state *_osmo_ecu_state_get_fr(struct osmo_ecu_state *st) +{ + return (struct fr_ecu_state *)container_of(st, struct fr_ecu_state, ecu_state); } static int ecu_fr_frame_in(struct osmo_ecu_state *st, bool bfi, const uint8_t *frame, unsigned int frame_bytes) { - struct osmo_ecu_fr_state *fr = (struct osmo_ecu_fr_state *) &st->data; + struct fr_ecu_state *fr = _osmo_ecu_state_get_fr(st); + if (bfi) return 0; + if (frame_bytes != GSM_FR_BYTES) + return 0; + if ((frame[0] & 0xF0) != 0xD0) + return 0; - osmo_ecu_fr_reset(fr, frame); + fr_ecu_input(fr, frame); return 0; } static int ecu_fr_frame_out(struct osmo_ecu_state *st, uint8_t *frame_out) { - struct osmo_ecu_fr_state *fr = (struct osmo_ecu_fr_state *) &st->data; + struct fr_ecu_state *fr = _osmo_ecu_state_get_fr(st); + + fr_ecu_output(fr, frame_out); + return GSM_FR_BYTES; +} + +static bool ecu_fr_is_dtx_pause(struct osmo_ecu_state *st) +{ + struct fr_ecu_state *fr = _osmo_ecu_state_get_fr(st); - if (osmo_ecu_fr_conceal(fr, frame_out) == 0) - return GSM_FR_BYTES; - else - return -1; + return fr->last_input_was_sid; } static const struct osmo_ecu_ops osmo_ecu_ops_fr = { .init = ecu_fr_init, .frame_in = ecu_fr_frame_in, .frame_out = ecu_fr_frame_out, + .is_dtx_pause = ecu_fr_is_dtx_pause, }; static __attribute__((constructor)) void on_dso_load_ecu_fr(void) diff --git a/src/codec/ecu_fr_old.c b/src/codec/ecu_fr_old.c new file mode 100644 index 00000000..cfefce14 --- /dev/null +++ b/src/codec/ecu_fr_old.c @@ -0,0 +1,166 @@ +/* + * (C) 2017 by sysmocom - s.f.m.c. GmbH + * (C) 2017 by Philipp Maier <pmaier@sysmocom.de> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * + * This module implements legacy, deprecated osmo_ecu_fr_reset() and + * osmo_ecu_fr_conceal() functions only - see ecu_fr.c for the new + * GSM-FR ECU implementation. + */ + +#include <stdbool.h> +#include <string.h> +#include <stdint.h> +#include <errno.h> + +#include <osmocom/core/bitvec.h> + +#include <osmocom/codec/gsm610_bits.h> +#include <osmocom/codec/codec.h> +#include <osmocom/codec/ecu.h> + +/* See also GSM 06.11, chapter 6 Example solution */ +#define GSM610_XMAXC_REDUCE 4 +#define GSM610_XMAXC_LEN 6 + +/** + * Reduce the XMAXC field. When the XMAXC field reaches + * zero the function will return true. + */ +static bool reduce_xmaxcr(struct bitvec *frame_bitvec, + const unsigned int index) +{ + unsigned int field_index; + uint64_t field; + + field_index = index; + field = bitvec_read_field(frame_bitvec, &field_index, GSM610_XMAXC_LEN); + if (field > GSM610_XMAXC_REDUCE) + field -= GSM610_XMAXC_REDUCE; + else + field = 0; + + field_index = index; + bitvec_write_field(frame_bitvec, &field_index, field, GSM610_XMAXC_LEN); + + return field == 0; +} + +/** + * Reduce all XMAXC fields in the frame. When all XMAXC fields + * reach zero, then the function will return true. + */ +static bool reduce_xmaxcr_all(struct bitvec *frame_bitvec) +{ + bool silent = true; + + silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC00); + silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC10); + silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC20); + silent &= reduce_xmaxcr(frame_bitvec, GSM610_RTP_XMAXC30); + + return silent; +} + +/* Use certain modifications to conceal the errors in a full rate frame */ +static int conceal_frame(uint8_t *frame) +{ + struct bitvec *frame_bitvec; + unsigned int len; + bool silent; + int rc = 0; + + /* In case we already deal with a silent frame, + * there is nothing to, we just abort immediately */ + if (osmo_fr_check_sid(frame, GSM_FR_BYTES)) + return 0; + + /* Attempt to allocate memory for bitvec */ + frame_bitvec = bitvec_alloc(GSM_FR_BYTES, NULL); + if (!frame_bitvec) + return -ENOMEM; + + /* Convert a frame to bitvec */ + len = bitvec_unpack(frame_bitvec, frame); + if (len != GSM_FR_BYTES) { + rc = -EIO; + goto leave; + } + + /* Fudge frame parameters */ + silent = reduce_xmaxcr_all(frame_bitvec); + + /* If we reached silence level, mute the frame + * completely, this also means that we can + * save the bitvec_pack operation */ + if (silent) { + memset(frame, 0x00, GSM_FR_BYTES); + frame[0] = 0xd0; + goto leave; + } + + /* Convert back to packed byte form */ + len = bitvec_pack(frame_bitvec, frame); + if (len != GSM_FR_BYTES) { + rc = -EIO; + goto leave; + } + +leave: + bitvec_free(frame_bitvec); + return rc; +} + +/*! + * To be called when a good frame is received. + * This function will then create a backup of the frame + * and reset the internal state. + * \param[in] state The state object for the ECU + * \param[out] frame The valid frame (GSM_FR_BYTES bytes in RTP payload format) + */ +void osmo_ecu_fr_reset(struct osmo_ecu_fr_state *state, const uint8_t *frame) +{ + state->subsequent_lost_frame = false; + memcpy(state->frame_backup, frame, GSM_FR_BYTES); +} + +/*! + * To be called when a bad frame is received. + * This function will then generate a replacement frame + * that can be used to conceal the dropout. + * \param[in] state The state object for the ECU + * \param[out] frame The buffer to fill with GSM_FR_BYTES of replacement frame + * \returns 0 if the frame was successfully filled + */ +int osmo_ecu_fr_conceal(struct osmo_ecu_fr_state *state, uint8_t *frame) +{ + int rc; + + /* For subsequent frames we run the error concealment + * functions on the backed up frame before we restore + * the backup */ + if (state->subsequent_lost_frame) { + rc = conceal_frame(state->frame_backup); + if (rc) + return rc; + } + + /* Restore the backed up frame and set flag in case + * we receive even more bad frames */ + memcpy(frame, state->frame_backup, GSM_FR_BYTES); + state->subsequent_lost_frame = true; + + return 0; +} diff --git a/src/codec/gsm610.c b/src/codec/gsm610.c index a05eabab..5cc4f148 100644 --- a/src/codec/gsm610.c +++ b/src/codec/gsm610.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> @@ -300,6 +296,35 @@ const uint16_t gsm610_bitorder[260] = { 29, /* LARc5:0 */ }; +/* + * Table 1 in section 6 of 3GPP TS 46.011 (substitution and muting of lost + * frames) specifies a silence frame in the form of GSM 06.10 parameters; + * the following const datum is this GSM 06.11 silence frame in GSM-FR + * RTP encoding. + */ +const uint8_t osmo_gsm611_silence_frame[GSM_FR_BYTES] = { + 0xDA, 0xA7, 0xAA, 0xA5, 0x1A, + 0x50, 0x20, 0x38, 0xE4, 0x6D, 0xB9, 0x1B, + 0x50, 0x20, 0x38, 0xE4, 0x6D, 0xB9, 0x1B, + 0x50, 0x20, 0x38, 0xE4, 0x6D, 0xB9, 0x1B, + 0x50, 0x20, 0x38, 0xE4, 0x6D, 0xB9, 0x1B, +}; + +static const uint16_t sid_code_word_bits[95] = { + /* bit numbers are relative to the RTP frame beginning, + * with signature bits included in the count. */ + 57, 58, 60, 61, 63, 64, 66, 67, 69, 70, 72, 73, + 75, 76, 78, 79, 81, 82, 84, 85, 87, 88, 90, 91, + 93, 94, 113, 114, 116, 117, 119, 120, 122, 123, + 125, 126, 128, 129, 131, 132, 134, 135, 137, + 138, 140, 141, 143, 144, 146, 147, 149, 150, + 169, 170, 172, 173, 175, 176, 178, 179, 181, + 182, 184, 185, 187, 188, 190, 191, 193, 194, + 196, 197, 199, 200, 202, 203, 205, 206, 225, + 226, 228, 229, 231, 232, 234, 235, 237, 240, + 243, 246, 249, 252, 255, 258, 261 +}; + /*! Check whether RTP frame contains FR SID code word according to * TS 101 318 §5.1.2 * \param[in] rtp_payload Buffer with RTP payload @@ -309,16 +334,7 @@ const uint16_t gsm610_bitorder[260] = { bool osmo_fr_check_sid(const uint8_t *rtp_payload, size_t payload_len) { struct bitvec bv; - uint16_t i, z_bits[] = { 57, 58, 60, 61, 63, 64, 66, 67, 69, 70, 72, 73, - 75, 76, 78, 79, 81, 82, 84, 85, 87, 88, 90, 91, - 93, 94, 113, 114, 116, 117, 119, 120, 122, 123, - 125, 126, 128, 129, 131, 132, 134, 135, 137, - 138, 140, 141, 143, 144, 146, 147, 149, 150, - 169, 170, 172, 173, 175, 176, 178, 179, 181, - 182, 184, 185, 187, 188, 190, 191, 193, 194, - 196, 197, 199, 200, 202, 203, 205, 206, 225, - 226, 228, 229, 231, 232, 234, 235, 237, 240, - 243, 246, 249, 252, 255, 258, 261 }; + uint16_t i; /* signature does not match Full Rate SID */ if ((rtp_payload[0] >> 4) != 0xD) @@ -327,10 +343,120 @@ bool osmo_fr_check_sid(const uint8_t *rtp_payload, size_t payload_len) bv.data = (uint8_t *) rtp_payload; bv.data_len = payload_len; - /* code word is all 0 at given bits, numbered from 1 */ - for (i = 0; i < ARRAY_SIZE(z_bits); i++) - if (bitvec_get_bit_pos(&bv, z_bits[i]) != ZERO) + /* code word is all 0 at given bits */ + for (i = 0; i < ARRAY_SIZE(sid_code_word_bits); i++) { + if (bitvec_get_bit_pos(&bv, sid_code_word_bits[i]) != ZERO) return false; + } return true; } + +/*! Classify potentially-SID FR codec frame in RTP format according + * to the rules of GSM 06.31 §6.1.1 + * \param[in] rtp_payload Buffer with RTP payload + * \returns enum osmo_gsm631_sid_class, with symbolic values + * OSMO_GSM631_SID_CLASS_SPEECH, OSMO_GSM631_SID_CLASS_INVALID or + * OSMO_GSM631_SID_CLASS_VALID corresponding to the 3 possible bit-counting + * classifications prescribed by the spec. + * + * Differences between the more familiar osmo_fr_check_sid() and the present + * function are: + * + * 1. osmo_fr_check_sid() returns true only if the SID frame is absolutely + * perfect, with all 95 bits of the SID code word zeroed. However, the + * rules of GSM 06.31 §6.1.1 allow up to one bit to be in error, + * and the frame is still accepted as valid SID. + * + * 2. The third possible state of invalid SID is not handled at all by the + * simpler osmo_fr_check_sid() function. + * + * 3. osmo_fr_check_sid() includes a check for 0xD RTP signature, and returns + * false if that signature nibble is wrong. That check is not included + * in the present version because there is no place for it in the + * ETSI-prescribed classification, it is neither speech nor SID. The + * assumption is that this function is used to classify the bit content + * of received codec frames, not their RTP encoding - the latter needs + * to be validated beforehand. + * + * Which function should one use? The answer depends on the specific + * circumstances, and needs to be addressed on a case-by-case basis. + */ +enum osmo_gsm631_sid_class osmo_fr_sid_classify(const uint8_t *rtp_payload) +{ + struct bitvec bv; + uint16_t i, n; + + bv.data = (uint8_t *) rtp_payload; + bv.data_len = GSM_FR_BYTES; + + /* count not-SID-matching bits per the spec */ + n = 0; + for (i = 0; i < ARRAY_SIZE(sid_code_word_bits); i++) { + if (bitvec_get_bit_pos(&bv, sid_code_word_bits[i]) != ZERO) + n++; + if (n >= 16) + return OSMO_GSM631_SID_CLASS_SPEECH; + } + if (n >= 2) + return OSMO_GSM631_SID_CLASS_INVALID; + else + return OSMO_GSM631_SID_CLASS_VALID; +} + +/*! Reset the SID field and the unused bits of a potentially corrupted, + * but still valid GSM-FR SID frame in RTP encoding to their pristine state. + * \param[in] rtp_payload Buffer with RTP payload - must be writable! + * + * Per GSM 06.12 section 5.2, a freshly minted SID frame carries 60 bits + * of comfort noise parameters (LARc and 4 times Xmaxc), while the remaining + * 200 bits are all zeros; the latter 200 all-0 bits further break down into + * 95 bits of SID field (checked by receivers to detect SID) and 105 unused + * bits which receivers are told to ignore. Network elements that receive + * SID frames from call leg A uplink and need to retransmit them on leg B + * downlink should "rejuvenate" received SID frames prior to retransmission; + * this function does the job. + */ +void osmo_fr_sid_reset(uint8_t *rtp_payload) +{ + uint8_t *p, sub; + + p = rtp_payload + 5; /* skip magic+LARc */ + for (sub = 0; sub < 4; sub++) { + *p++ = 0; + *p++ &= 0x1F; /* upper 5 bits of Xmaxc field */ + *p++ &= 0x80; /* and the lsb spilling into the next byte */ + *p++ = 0; + *p++ = 0; + *p++ = 0; + *p++ = 0; + } +} + +/*! Preen potentially-SID FR codec frame in RTP format, ensuring that it is + * either a speech frame or a valid SID, and if the latter, making it a + * perfect, error-free SID frame. + * \param[in] rtp_payload Buffer with RTP payload - must be writable! + * \returns true if the frame is good, false otherwise + */ +bool osmo_fr_sid_preen(uint8_t *rtp_payload) +{ + enum osmo_gsm631_sid_class sidc; + + sidc = osmo_fr_sid_classify(rtp_payload); + switch (sidc) { + case OSMO_GSM631_SID_CLASS_SPEECH: + return true; + case OSMO_GSM631_SID_CLASS_INVALID: + return false; + case OSMO_GSM631_SID_CLASS_VALID: + /* "Rejuvenate" this SID frame, correcting any errors */ + osmo_fr_sid_reset(rtp_payload); + return true; + default: + /* There are only 3 possible SID classifications per GSM 06.31 + * section 6.1.1, thus any other return value is a grave error + * in the code. */ + OSMO_ASSERT(0); + } +} diff --git a/src/codec/gsm620.c b/src/codec/gsm620.c index 282781fd..ef1d3b9b 100644 --- a/src/codec/gsm620.c +++ b/src/codec/gsm620.c @@ -17,14 +17,11 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> #include <stdbool.h> +#include <string.h> #include <osmocom/core/bitvec.h> #include <osmocom/core/utils.h> @@ -268,12 +265,6 @@ const uint16_t gsm620_voiced_bitorder[112] = { 81, /* Code 3:7 */ }; -static inline uint16_t mask(const uint8_t msb) -{ - const uint16_t m = (uint16_t)1 << (msb - 1); - return (m - 1) ^ m; -} - /*! Check whether RTP frame contains HR SID code word according to * TS 101 318 §5.2.2 * \param[in] rtp_payload Buffer with RTP payload @@ -282,16 +273,44 @@ static inline uint16_t mask(const uint8_t msb) */ bool osmo_hr_check_sid(const uint8_t *rtp_payload, size_t payload_len) { - uint8_t i, bits[] = { 1, 2, 8, 9, 5, 4, 9, 5, 4, 9, 5, 4, 9, 5 }; - struct bitvec bv; - bv.data = (uint8_t *) rtp_payload; - bv.data_len = payload_len; - bv.cur_bit = 33; + struct bitvec bv = { + .data = (uint8_t *)rtp_payload, + .data_len = payload_len, + }; - /* code word is all 1 at given bits, numbered from 1, MODE is always 3 */ - for (i = 0; i < ARRAY_SIZE(bits); i++) - if (bitvec_get_uint(&bv, bits[i]) != mask(bits[i])) + /* A SID frame is identified by a SID codeword consisting of 79 bits which are all 1, + * so we basically check if all bits in range r34..r112 (inclusive) are 1. */ + for (bv.cur_bit = 33; bv.cur_bit < bv.data_len * 8; bv.cur_bit++) + if (bitvec_get_bit_pos(&bv, bv.cur_bit) != ONE) return false; return true; } + +/*! Reset the SID field of a potentially corrupted, but still valid GSM-HR + * SID frame in TS 101 318 format to its pristine state (full SID codeword). + * \param[in] rtp_payload Buffer with RTP payload - must be writable! + * + * Per GSM 06.22 section 5.3, a freshly minted SID frame consists of 33 bits + * of comfort noise parameters and 79 bits of SID codeword (all 1s). Network + * elements that receive SID frames from call leg A uplink and need to + * retransmit them on leg B downlink should "rejuvenate" received SID frames + * prior to retransmission by resetting the SID field to its pristine state + * of all 1s; this function does the job. + * + * Important note: because of HR-specific quirks (lack of exact bit counting + * rules in GSM 06.41 spec compared to 06.31 & 06.81, plus the fact that such + * bit counting can only be done efficiently in the GSM 05.03 channel decoder + * prior to bit reordering based on voiced or unvoiced mode), a generic + * (usable from any network element) SID classification function similar to + * osmo_{fr,efr}_sid_classify() unfortunately cannot exist for HR. Therefore, + * the triggering condition for invoking this SID rejuvenation/reset function + * can only be an out-of-band SID indication, as in GSM 08.61 TRAU frames + * or RFC 5993 ToC octet. + */ +void osmo_hr_sid_reset(uint8_t *rtp_payload) +{ + /* set all 79 SID codeword bits to 1 */ + rtp_payload[4] |= 0x7F; + memset(rtp_payload + 5, 0xFF, 9); +} diff --git a/src/codec/gsm660.c b/src/codec/gsm660.c index 4f7bb099..b15bdf37 100644 --- a/src/codec/gsm660.c +++ b/src/codec/gsm660.c @@ -17,13 +17,13 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> +#include <stdbool.h> + +#include <osmocom/core/bitvec.h> +#include <osmocom/core/utils.h> #include <osmocom/codec/codec.h> /* GSM EFR - subjective importance bit ordering */ @@ -257,3 +257,161 @@ const uint16_t gsm660_bitorder[260] = { 243, /* 258 -> PULSE 4_9: b0 */ 246, /* 259 -> PULSE 4_10: b0 */ }; + +static const uint8_t sid_code_word_bits[95] = { + /* bit numbers are relative to "pure" EFR frame beginning, + * not counting the signature bits. */ + 45, 46, 48, 49, 50, 51, 52, 53, 54, 55, + 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, + 66, 67, 68, 94, 95, 96, 98, 99, 100, 101, + 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, + 112, 113, 114, 115, 116, 117, 118, 148, 149, 150, + 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, + 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, + 171, 196, 197, 198, 199, 200, 201, 202, 203, 204, + 205, 206, 207, 208, 209, 212, 213, 214, 215, 216, + 217, 218, 219, 220, 221 +}; + +/*! Check whether RTP frame contains EFR SID code word according to + * TS 101 318 §5.3.2 + * \param[in] rtp_payload Buffer with RTP payload + * \param[in] payload_len Length of payload + * \returns true if code word is found, false otherwise + */ +bool osmo_efr_check_sid(const uint8_t *rtp_payload, size_t payload_len) +{ + struct bitvec bv; + uint16_t i; + + /* signature does not match Enhanced Full Rate SID */ + if ((rtp_payload[0] >> 4) != 0xC) + return false; + + bv.data = (uint8_t *) rtp_payload; + bv.data_len = payload_len; + + /* code word is all 1 at given bits */ + for (i = 0; i < ARRAY_SIZE(sid_code_word_bits); i++) { + if (bitvec_get_bit_pos(&bv, sid_code_word_bits[i]+4) != ONE) + return false; + } + + return true; +} + +/*! Classify potentially-SID EFR codec frame in RTP format according + * to the rules of GSM 06.81 §6.1.1 + * \param[in] rtp_payload Buffer with RTP payload + * \returns enum osmo_gsm631_sid_class, with symbolic values + * OSMO_GSM631_SID_CLASS_SPEECH, OSMO_GSM631_SID_CLASS_INVALID or + * OSMO_GSM631_SID_CLASS_VALID corresponding to the 3 possible bit-counting + * classifications prescribed by the spec. + * + * Differences between the more familiar osmo_efr_check_sid() and the present + * function are: + * + * 1. osmo_efr_check_sid() returns true only if the SID frame is absolutely + * perfect, with all 95 bits of the SID code word set. However, the + * rules of GSM 06.81 §6.1.1 allow up to one bit to be in error, + * and the frame is still accepted as valid SID. + * + * 2. The third possible state of invalid SID is not handled at all by the + * simpler osmo_efr_check_sid() function. + * + * 3. osmo_efr_check_sid() includes a check for 0xC RTP signature, and returns + * false if that signature nibble is wrong. That check is not included + * in the present version because there is no place for it in the + * ETSI-prescribed classification, it is neither speech nor SID. The + * assumption is that this function is used to classify the bit content + * of received codec frames, not their RTP encoding - the latter needs + * to be validated beforehand. + * + * Which function should one use? The answer depends on the specific + * circumstances, and needs to be addressed on a case-by-case basis. + */ +enum osmo_gsm631_sid_class osmo_efr_sid_classify(const uint8_t *rtp_payload) +{ + struct bitvec bv; + uint16_t i, n; + + bv.data = (uint8_t *) rtp_payload; + bv.data_len = GSM_EFR_BYTES; + + /* count not-SID-matching bits per the spec */ + n = 0; + for (i = 0; i < ARRAY_SIZE(sid_code_word_bits); i++) { + if (bitvec_get_bit_pos(&bv, sid_code_word_bits[i]+4) != ONE) + n++; + if (n >= 16) + return OSMO_GSM631_SID_CLASS_SPEECH; + } + if (n >= 2) + return OSMO_GSM631_SID_CLASS_INVALID; + else + return OSMO_GSM631_SID_CLASS_VALID; +} + +/*! Reset the SID field of a potentially corrupted, but still valid GSM-EFR + * SID frame in RTP encoding to its pristine state (full SID code word). + * \param[in] rtp_payload Buffer with RTP payload - must be writable! + * + * Per GSM 06.62 section 5.3, a freshly minted SID frame consists of 58 bits + * of comfort noise parameters (LSF and 4 times fixed codebook gain), 95 bits + * of SID code word (all 1s) and 91 unused bits (all 0s). Network elements + * that receive SID frames from call leg A uplink and need to retransmit them + * on leg B downlink should "rejuvenate" received SID frames prior to + * retransmission by resetting the SID field to its pristine state of all 1s; + * this function does the job. + * + * Potential TODO: it would be nice to also zero out the remaining 91 bits + * which the spec leaves as reserved, clearing out leg A radio bit errors - + * but do we really need to? + */ +void osmo_efr_sid_reset(uint8_t *rtp_payload) +{ + /* set all 95 SID code word bits to 1 */ + rtp_payload[6] |= 0x6F; + rtp_payload[7] = 0xFF; + rtp_payload[8] = 0xFF; + rtp_payload[9] |= 0x80; + rtp_payload[12] |= 0x3B; + rtp_payload[13] = 0xFF; + rtp_payload[14] = 0xFF; + rtp_payload[15] |= 0xE0; + rtp_payload[19] = 0xFF; + rtp_payload[20] = 0xFF; + rtp_payload[21] = 0xFF; + rtp_payload[25] = 0xFF; + rtp_payload[26] |= 0xFC; + rtp_payload[27] = 0xFF; + rtp_payload[28] |= 0xC0; +} + +/*! Preen potentially-SID EFR codec frame in RTP format, ensuring that it is + * either a speech frame or a valid SID, and if the latter, making it a + * perfect, error-free SID frame. + * \param[in] rtp_payload Buffer with RTP payload - must be writable! + * \returns true if the frame is good, false otherwise + */ +bool osmo_efr_sid_preen(uint8_t *rtp_payload) +{ + enum osmo_gsm631_sid_class sidc; + + sidc = osmo_efr_sid_classify(rtp_payload); + switch (sidc) { + case OSMO_GSM631_SID_CLASS_SPEECH: + return true; + case OSMO_GSM631_SID_CLASS_INVALID: + return false; + case OSMO_GSM631_SID_CLASS_VALID: + /* "Rejuvenate" this SID frame, correcting any errors */ + osmo_efr_sid_reset(rtp_payload); + return true; + default: + /* There are only 3 possible SID classifications per GSM 06.81 + * section 6.1.1, thus any other return value is a grave error + * in the code. */ + OSMO_ASSERT(0); + } +} diff --git a/src/codec/gsm690.c b/src/codec/gsm690.c index 19557164..3b2e6694 100644 --- a/src/codec/gsm690.c +++ b/src/codec/gsm690.c @@ -2,6 +2,7 @@ * GSM 06.90 - GSM AMR Codec. */ /* * (C) 2010 Sylvain Munaut <tnt@246tNt.com> + * (C) 2020 Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -17,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> @@ -29,6 +26,7 @@ #include <stdlib.h> #include <osmocom/core/utils.h> +#include <osmocom/core/bits.h> #include <osmocom/codec/codec.h> /* * These table map between the raw encoder parameter output and @@ -216,8 +214,117 @@ const uint16_t gsm690_4_75_bitorder[95] = { 92, 31, 52, 65, 86, }; +/*! These constants refer to the length of one "AMR core frame" as per + * TS 26.101 Section 4.2.2 / Table 2. */ +const uint8_t gsm690_bitlength[AMR_NO_DATA+1] = { + [AMR_4_75] = 95, + [AMR_5_15] = 103, + [AMR_5_90] = 118, + [AMR_6_70] = 134, + [AMR_7_40] = 148, + [AMR_7_95] = 159, + [AMR_10_2] = 204, + [AMR_12_2] = 244, + [AMR_SID] = 39, +}; + +struct ts26101_reorder_table { + /*! Table as per TS 26.101 Annex B to compute d-bits from s-bits */ + const uint16_t *s_to_d; + /*! size of table */ + uint8_t len; +}; + +static const struct ts26101_reorder_table ts26101_reorder_tables[8] = { + [AMR_4_75] = { + .s_to_d = gsm690_4_75_bitorder, + .len = ARRAY_SIZE(gsm690_4_75_bitorder), + }, + [AMR_5_15] = { + .s_to_d = gsm690_5_15_bitorder, + .len = ARRAY_SIZE(gsm690_5_15_bitorder), + }, + [AMR_5_90] = { + .s_to_d = gsm690_5_9_bitorder, + .len = ARRAY_SIZE(gsm690_5_9_bitorder), + }, + [AMR_6_70] = { + .s_to_d = gsm690_6_7_bitorder, + .len = ARRAY_SIZE(gsm690_6_7_bitorder), + }, + [AMR_7_40] = { + .s_to_d = gsm690_7_4_bitorder, + .len = ARRAY_SIZE(gsm690_7_4_bitorder), + }, + [AMR_7_95] = { + .s_to_d = gsm690_7_95_bitorder, + .len = ARRAY_SIZE(gsm690_7_95_bitorder), + }, + [AMR_10_2] = { + .s_to_d = gsm690_10_2_bitorder, + .len = ARRAY_SIZE(gsm690_10_2_bitorder), + }, + [AMR_12_2] = { + .s_to_d = gsm690_12_2_bitorder, + .len = ARRAY_SIZE(gsm690_12_2_bitorder), + }, +}; + +/*! Convert from S-bits (codec output) to d-bits. + * \param[out] out user-provided output buffer for generated unpacked d-bits + * \param[in] in input buffer for unpacked s-bits + * \param[in] n_bits number of bits (in both in and out) + * \param[in] AMR mode (0..7) */ +int osmo_amr_s_to_d(ubit_t *out, const ubit_t *in, uint16_t n_bits, enum osmo_amr_type amr_mode) +{ + const struct ts26101_reorder_table *tbl; + int i; + + if (amr_mode >= ARRAY_SIZE(ts26101_reorder_tables)) + return -ENODEV; + + tbl = &ts26101_reorder_tables[amr_mode]; + + if (n_bits > tbl->len) + return -EINVAL; + + for (i = 0; i < n_bits; i++) { + uint16_t n = tbl->s_to_d[i]; + out[i] = in[n]; + } + + return n_bits; +} + +/*! Convert from d-bits to s-bits (codec input). + * \param[out] out user-provided output buffer for generated unpacked s-bits + * \param[in] in input buffer for unpacked d-bits + * \param[in] n_bits number of bits (in both in and out) + * \param[in] AMR mode (0..7) */ +int osmo_amr_d_to_s(ubit_t *out, const ubit_t *in, uint16_t n_bits, enum osmo_amr_type amr_mode) +{ + const struct ts26101_reorder_table *tbl; + int i; + + if (amr_mode >= ARRAY_SIZE(ts26101_reorder_tables)) + return -ENODEV; + + tbl = &ts26101_reorder_tables[amr_mode]; + + if (n_bits > tbl->len) + return -EINVAL; + + for (i = 0; i < n_bits; i++) { + uint16_t n = tbl->s_to_d[i]; + out[n] = in[i]; + } + + return n_bits; +} + +/* See also RFC 4867 §3.6, Table 1, Column "Total speech bits" */ static const uint8_t amr_len_by_ft[16] = { - 12, 13, 15, 17, 19, 20, 26, 31, 7, 0, 0, 0, 0, 0, 0, 0 + 12, 13, 15, 17, 19, 20, 26, 31, 5, 0, 0, 0, 0, 0, 0, 0 }; const struct value_string osmo_amr_type_names[] = { diff --git a/src/coding/Makefile.am b/src/coding/Makefile.am index f47fe457..0cab66ff 100644 --- a/src/coding/Makefile.am +++ b/src/coding/Makefile.am @@ -1,13 +1,14 @@ # This is _NOT_ the library release version, it's an API version. # Please read Chapter 6 "Library interface versions" of the libtool # documentation before making any modification -LIBVERSION=1:1:1 +LIBVERSION=3:0:3 AM_CPPFLAGS = \ -I"$(top_srcdir)/include" \ -I"$(top_builddir)/include" \ - $(TALLOC_CFLAGS) -AM_CFLAGS = -Wall + -I"$(top_builddir)" \ + $(NULL) +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) if ENABLE_PSEUDOTALLOC AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc @@ -20,16 +21,19 @@ libosmocoding_la_SOURCES = \ gsm0503_mapping.c \ gsm0503_tables.c \ gsm0503_parity.c \ - gsm0503_coding.c + gsm0503_coding.c \ + gsm0503_amr_dtx.c libosmocoding_la_LDFLAGS = \ $(LTLDFLAGS_OSMOCODING) \ - -version-info \ - $(LIBVERSION) \ + -version-info $(LIBVERSION) \ -no-undefined \ - $(TALLOC_LIBS) + $(NULL) + libosmocoding_la_LIBADD = \ - ../libosmocore.la \ - ../gsm/libosmogsm.la \ - ../codec/libosmocodec.la + $(top_builddir)/src/core/libosmocore.la \ + $(top_builddir)/src/gsm/libosmogsm.la \ + $(top_builddir)/src/codec/libosmocodec.la \ + $(NULL) EXTRA_DIST = libosmocoding.map +EXTRA_libosmocoding_la_DEPENDENCIES = libosmocoding.map diff --git a/src/coding/gsm0503_amr_dtx.c b/src/coding/gsm0503_amr_dtx.c new file mode 100644 index 00000000..7de3b281 --- /dev/null +++ b/src/coding/gsm0503_amr_dtx.c @@ -0,0 +1,355 @@ +/* + * (C) 2020-2022 by sysmocom - s.f.m.c. GmbH, Author: Philipp Maier + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <stdio.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> + +#include <osmocom/core/bits.h> +#include <osmocom/core/conv.h> +#include <osmocom/core/utils.h> +#include <osmocom/coding/gsm0503_amr_dtx.h> +#include <osmocom/coding/gsm0503_parity.h> +#include <osmocom/gsm/gsm0503.h> + +#define S2U(b) ((b) < 0) + +/* See also: 3GPP TS 05.03, chapter 3.10.1.3, 3.10.5.2 Identification marker */ +static const ubit_t id_marker_1[] = { 1, 0, 1, 1, 0, 0, 0, 0, 1 }; + +/* See also: 3GPP TS 05.03, chapter 3.9.1.3, 3.10.2.2, 3.10.2.2 Identification marker */ +static const ubit_t id_marker_0[] = { 0, 1, 0, 0, 1, 1, 1, 1, 0 }; + +/* See also: 3GPP TS 05.03, chapter 3.9 Adaptive multi rate speech channel at full rate (TCH/AFS) */ +static const ubit_t codec_mode_1_sid[] = { 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 1, 0 }; +static const ubit_t codec_mode_2_sid[] = { 0, 0, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0 }; +static const ubit_t codec_mode_3_sid[] = { 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 }; +static const ubit_t codec_mode_4_sid[] = { 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 1, 1, 1 }; + +const struct value_string gsm0503_amr_dtx_frame_names[] = { + { AMR_OTHER, "AMR_OTHER (audio)" }, + { AFS_SID_FIRST, "AFS_SID_FIRST" }, + { AFS_SID_UPDATE, "AFS_SID_UPDATE (marker)" }, + { AFS_SID_UPDATE_CN, "AFS_SID_UPDATE_CN (audio)" }, + { AFS_ONSET, "AFS_ONSET" }, + { AHS_SID_UPDATE, "AHS_SID_UPDATE (marker)" }, + { AHS_SID_UPDATE_CN, "AHS_SID_UPDATE_CN (audio)" }, + { AHS_SID_FIRST_P1, "AHS_SID_FIRST_P1" }, + { AHS_SID_FIRST_P2, "AHS_SID_FIRST_P2" }, + { AHS_ONSET, "AHS_ONSET" }, + { AHS_SID_FIRST_INH, "AHS_SID_FIRST_INH" }, + { AHS_SID_UPDATE_INH, "AHS_SID_UPDATE_INH" }, + { 0, NULL } +}; + +static bool detect_afs_id_marker(int *n_errors, int *n_bits_total, const sbit_t *sbits, uint8_t offset, uint8_t count, + const ubit_t * id_marker, uint8_t id_marker_len) +{ + unsigned int i, k; + unsigned int id_bit_nr = 0; + int errors = 0; + int bits = 0; + + /* Override coded in-band data */ + sbits += offset; + + /* Check for identification marker bits */ + for (i = 0; i < count; i++) { + for (k = 0; k < 4; k++) { + if (*sbits == 0 || id_marker[id_bit_nr % id_marker_len] != S2U(*sbits)) + errors++; + id_bit_nr++; + sbits++; + bits++; + } + + /* Jump to the next block of 4 bits */ + sbits += 4; + } + + *n_errors = errors; + *n_bits_total = bits; + + /* Tolerate up to 1/8 errornous bits */ + return *n_errors < *n_bits_total / 8; +} + +static bool detect_ahs_id_marker(int *n_errors, int *n_bits_total, const sbit_t *sbits, const ubit_t *id_marker) +{ + unsigned int i, k; + int errors = 0; + int bits = 0; + + /* Override coded in-band data */ + sbits += 16; + + /* Check first identification marker bits (23*9 bits) */ + for (i = 0; i < 23; i++) { + for (k = 0; k < 9; k++) { + if (*sbits == 0 || id_marker[k] != S2U(*sbits)) + errors++; + sbits++; + bits++; + } + } + + /* Check remaining identification marker bits (5 bits) */ + for (k = 0; k < 5; k++) { + if (*sbits == 0 || id_marker[k] != S2U(*sbits)) + errors++; + sbits++; + bits++; + } + + *n_errors = errors; + *n_bits_total = bits; + + /* Tolerate up to 1/8 errornous bits */ + return *n_errors < *n_bits_total / 8; +} + +static bool detect_interleaved_ahs_id_marker(int *n_errors, int *n_bits_total, const sbit_t *sbits, uint8_t offset, + uint8_t n_bits, const ubit_t * id_marker, uint8_t id_marker_len) +{ + unsigned int i, k; + int errors = 0; + int bits = 0; + uint8_t full_rounds = n_bits / id_marker_len; + uint8_t remainder = n_bits % id_marker_len; + + /* Override coded in-band data */ + sbits += offset; + + /* Check first identification marker bits (23*9 bits) */ + for (i = 0; i < full_rounds; i++) { + for (k = 0; k < id_marker_len; k++) { + if (*sbits == 0 || id_marker[k] != S2U(*sbits)) + errors++; + sbits += 2; + bits++; + } + } + + /* Check remaining identification marker bits (5 bits) */ + for (k = 0; k < remainder; k++) { + if (*sbits == 0 || id_marker[k] != S2U(*sbits)) + errors++; + sbits += 2; + bits++; + } + + *n_errors = errors; + *n_bits_total = bits; + + /* Tolerate up to 1/8 errornous bits */ + return *n_errors < *n_bits_total / 8; +} + +/* Detect an FR AMR SID_FIRST frame by its identifcation marker */ +static bool detect_afs_sid_first(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_afs_id_marker(n_errors, n_bits_total, sbits, 32, 53, id_marker_0, 9); +} + +/* Detect an FR AMR SID_UPDATE frame by its identification marker */ +static bool detect_afs_sid_update(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_afs_id_marker(n_errors, n_bits_total, sbits, 36, 53, id_marker_0, 9); +} + +/* Detect an FR AMR ONSET frame by its repeating coded inband data */ +static int detect_afs_onset(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + bool rc; + + rc = detect_afs_id_marker(n_errors, n_bits_total, sbits, 4, 57, codec_mode_1_sid, 16); + if (rc) + return 0; + + rc = detect_afs_id_marker(n_errors, n_bits_total, sbits, 4, 57, codec_mode_2_sid, 16); + if (rc) + return 1; + + rc = detect_afs_id_marker(n_errors, n_bits_total, sbits, 4, 57, codec_mode_3_sid, 16); + if (rc) + return 2; + + rc = detect_afs_id_marker(n_errors, n_bits_total, sbits, 4, 57, codec_mode_4_sid, 16); + if (rc) + return 3; + + return -1; +} + +/* Detect an HR AMR SID UPDATE frame by its identification marker */ +static bool detect_ahs_sid_update(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_ahs_id_marker(n_errors, n_bits_total, sbits, id_marker_1); +} + +/* Detect an HR AMR SID FIRST (part 1) frame by its identification marker */ +static bool detect_ahs_sid_first_p1(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_ahs_id_marker(n_errors, n_bits_total, sbits, id_marker_0); +} + +/* Detect an HR AMR SID FIRST (part 2) frame by its repeating coded inband data */ +static int detect_ahs_sid_first_p2(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + bool rc; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 0, 114, codec_mode_1_sid, 16); + if (rc) + return 0; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 0, 114, codec_mode_2_sid, 16); + if (rc) + return 1; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 0, 114, codec_mode_3_sid, 16); + if (rc) + return 2; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 0, 114, codec_mode_4_sid, 16); + if (rc) + return 3; + + return -1; +} + +/* Detect an HR AMR ONSET frame by its repeating coded inband data */ +static int detect_ahs_onset(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + bool rc; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 1, 114, codec_mode_1_sid, 16); + if (rc) + return 0; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 1, 114, codec_mode_2_sid, 16); + if (rc) + return 1; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 1, 114, codec_mode_3_sid, 16); + if (rc) + return 2; + + rc = detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 1, 114, codec_mode_4_sid, 16); + if (rc) + return 3; + + return -1; +} + +/* Detect an HR AMR SID FIRST INHIBIT frame by its identification marker */ +static bool detect_ahs_sid_first_inh(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 33, 212, id_marker_1, 9); +} + +/* Detect an HR AMR SID UPDATE INHIBIT frame by its identification marker */ +static bool detect_ahs_sid_update_inh(int *n_errors, int *n_bits_total, const sbit_t *sbits) +{ + return detect_interleaved_ahs_id_marker(n_errors, n_bits_total, sbits, 33, 212, id_marker_0, 9); +} + +/*! Detect FR AMR DTX frame in unmapped, deinterleaved frame bits. + * \param[out] n_errors number of errornous bits. + * \param[out] n_bits_total number of checked bits. + * \param[out] mode_id codec mode ID (0..3 or -1). + * \param[in] sbits input soft-bits (456 bit). + * \returns dtx frame type. */ +enum gsm0503_amr_dtx_frames gsm0503_detect_afs_dtx_frame2(int *n_errors, int *n_bits_total, + int *mode_id, const sbit_t *sbits) +{ + if (detect_afs_sid_first(n_errors, n_bits_total, sbits)) + return AFS_SID_FIRST; + if (detect_afs_sid_update(n_errors, n_bits_total, sbits)) + return AFS_SID_UPDATE; + if ((*mode_id = detect_afs_onset(n_errors, n_bits_total, sbits)) != -1) + return AFS_ONSET; + + *n_errors = 0; + *n_bits_total = 0; + return AMR_OTHER; +} + +/*! Detect FR AMR DTX frame in unmapped, deinterleaved frame bits. + * DEPRECATED: use gsm0503_detect_afs_dtx_frame2() instead. + * \param[out] n_errors number of errornous bits. + * \param[out] n_bits_total number of checked bits. + * \param[in] ubits input bits (456 bit). + * \returns dtx frame type. */ +enum gsm0503_amr_dtx_frames gsm0503_detect_afs_dtx_frame(int *n_errors, int *n_bits_total, + const ubit_t *ubits) +{ + int mode_id; /* unused */ + sbit_t sbits[456]; + + osmo_ubit2sbit(&sbits[0], &ubits[0], sizeof(sbits)); + return gsm0503_detect_afs_dtx_frame2(n_errors, n_bits_total, &mode_id, &sbits[0]); +} + +/*! Detect HR AMR DTX frame in unmapped, deinterleaved frame bits. + * \param[out] n_errors number of errornous bits. + * \param[out] n_bits_total number of checked bits. + * \param[out] mode_id codec ID (CMI or CMC/CMR). + * \param[out] mode_id codec mode ID (0..3 or -1). + * \param[in] sbits input soft-bits (456 bit). + * \returns dtx frame type, */ +enum gsm0503_amr_dtx_frames gsm0503_detect_ahs_dtx_frame2(int *n_errors, int *n_bits_total, + int *mode_id, const sbit_t *sbits) +{ + if (detect_ahs_sid_update(n_errors, n_bits_total, sbits)) + return AHS_SID_UPDATE; + if (detect_ahs_sid_first_inh(n_errors, n_bits_total, sbits)) + return AHS_SID_FIRST_INH; + if (detect_ahs_sid_update_inh(n_errors, n_bits_total, sbits)) + return AHS_SID_UPDATE_INH; + if (detect_ahs_sid_first_p1(n_errors, n_bits_total, sbits)) + return AHS_SID_FIRST_P1; + if ((*mode_id = detect_ahs_sid_first_p2(n_errors, n_bits_total, sbits)) != -1) + return AHS_SID_FIRST_P2; + if ((*mode_id = detect_ahs_onset(n_errors, n_bits_total, sbits)) != -1) + return AHS_ONSET; + + *n_errors = 0; + *n_bits_total = 0; + return AMR_OTHER; +} + +/*! Detect HR AMR DTX frame in unmapped, deinterleaved frame bits. + * DEPRECATED: use gsm0503_detect_ahs_dtx_frame2() instead. + * \param[out] n_errors number of errornous bits. + * \param[out] n_bits_total number of checked bits. + * \param[in] ubits input bits (456 bit). + * \returns dtx frame type, */ +enum gsm0503_amr_dtx_frames gsm0503_detect_ahs_dtx_frame(int *n_errors, int *n_bits_total, + const ubit_t *ubits) +{ + int mode_id; /* unused */ + sbit_t sbits[456]; + + osmo_ubit2sbit(&sbits[0], &ubits[0], sizeof(sbits)); + return gsm0503_detect_ahs_dtx_frame2(n_errors, n_bits_total, &mode_id, &sbits[0]); +} diff --git a/src/coding/gsm0503_coding.c b/src/coding/gsm0503_coding.c index 7385d233..022f4656 100644 --- a/src/coding/gsm0503_coding.c +++ b/src/coding/gsm0503_coding.c @@ -17,10 +17,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdio.h> @@ -35,9 +31,7 @@ #include <osmocom/core/crcgen.h> #include <osmocom/core/endian.h> -#include <osmocom/gprs/protocol/gsm_04_60.h> -#include <osmocom/gprs/gprs_rlc.h> - +#include <osmocom/gsm/protocol/gsm_44_060.h> #include <osmocom/gsm/protocol/gsm_04_08.h> #include <osmocom/gsm/gsm0503.h> #include <osmocom/codec/codec.h> @@ -47,6 +41,7 @@ #include <osmocom/coding/gsm0503_tables.h> #include <osmocom/coding/gsm0503_coding.h> #include <osmocom/coding/gsm0503_parity.h> +#include <osmocom/coding/gsm0503_amr_dtx.h> /*! \mainpage libosmocoding Documentation * @@ -541,24 +536,29 @@ static int osmo_conv_decode_ber_punctured(const struct osmo_conv_code *code, int *n_errors, int *n_bits_total, const uint8_t *data_punc) { - int res, i, coded_len; + int res, coded_len; ubit_t recoded[EGPRS_DATA_C_MAX]; res = osmo_conv_decode(code, input, output); - if (n_bits_total || n_errors) { - coded_len = osmo_conv_encode(code, output, recoded); - OSMO_ASSERT(sizeof(recoded) / sizeof(recoded[0]) >= coded_len); - } + if (!n_bits_total && !n_errors) + return res; + + coded_len = osmo_conv_encode(code, output, recoded); + OSMO_ASSERT(ARRAY_SIZE(recoded) >= coded_len); /* Count bit errors */ if (n_errors) { *n_errors = 0; - for (i = 0; i < coded_len; i++) { - if (((!data_punc) || (data_punc && !data_punc[i])) && - !((recoded[i] && input[i] < 0) || - (!recoded[i] && input[i] > 0)) ) - *n_errors += 1; + for (unsigned int i = 0; i < coded_len; i++) { + /* punctured bits do not count as bit errors */ + if (data_punc != NULL && data_punc[i]) + continue; + if (recoded[i] == 1 && input[i] < 0) + continue; + if (recoded[i] == 0 && input[i] > 0) + continue; + *n_errors += 1; } } @@ -610,7 +610,7 @@ static int _xcch_decode_cB(uint8_t *l2_data, const sbit_t *cB, /*! convenience wrapper for encoding to coded bits * \param[out] cB caller-allocated buffer for 456 coded bits as per TS 05.03 4.1.3 - * \param[out] l2_data to-be-encoded L2 Frame + * \param[in] l2_data to-be-encoded L2 Frame * \returns 0 */ static int _xcch_encode_cB(ubit_t *cB, const uint8_t *l2_data) { @@ -925,10 +925,10 @@ static int egprs_decode_data(uint8_t *l2_data, const sbit_t *c, * \param[out] l2_data caller-allocated buffer for L2 Frame * \param[in] bursts burst input data as soft unpacked bits * \param[in] nbits number of bits in \a bursts - * \param usf_p unused argument ?!? + * \param usf_p Uplink State Flag, FIXME: not implemented * \param[out] n_errors number of detected bit-errors * \param[out] n_bits_total total number of decoded bits - * \returns 0 on success; negative on error */ + * \returns number of bytes decoded; negative on error */ int gsm0503_pdtch_egprs_decode(uint8_t *l2_data, const sbit_t *bursts, uint16_t nbits, uint8_t *usf_p, int *n_errors, int *n_bits_total) { @@ -1014,10 +1014,10 @@ int gsm0503_pdtch_egprs_decode(uint8_t *l2_data, const sbit_t *bursts, uint16_t /*! Decode GPRS PDTCH * \param[out] l2_data caller-allocated buffer for L2 Frame * \param[in] bursts burst input data as soft unpacked bits - * \param[out] usf_p uplink stealing flag + * \param[out] usf_p Uplink State Flag, only relevant for DL blocks * \param[out] n_errors number of detected bit-errors - * \param[out] n_bits_total total number of dcoded bits - * \returns 0 on success; negative on error */ + * \param[out] n_bits_total total number of decoded bits + * \returns number of bytes decoded; negative on error */ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, int *n_errors, int *n_bits_total) { @@ -1046,6 +1046,10 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, osmo_conv_decode_ber(&gsm0503_xcch, cB, conv, n_errors, n_bits_total); + /* the three USF bits d(0),d(1),d(2) are *not* precoded */ + if (usf_p) + *usf_p = (conv[0] << 2) | (conv[1] << 1) | (conv[2] << 0); + rv = osmo_crc64gen_check_bits(&gsm0503_fire_crc40, conv, 184, conv + 184); if (rv) @@ -1055,6 +1059,7 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, return 23; case 2: + /* reorder, set punctured bits to 0 (unknown state) */ for (i = 587, j = 455; i >= 0; i--) { if (!gsm0503_puncture_cs2[i]) cB[i] = cB[j--]; @@ -1062,9 +1067,15 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, cB[i] = 0; } - osmo_conv_decode_ber(&gsm0503_cs2_np, cB, - conv, n_errors, n_bits_total); + /* decode as if puncturing was not employed (note '_np') */ + osmo_conv_decode_ber_punctured(&gsm0503_cs2_np, cB, conv, + n_errors, NULL, + gsm0503_puncture_cs2); + /* indicate the actual amount of coded bits (excluding punctured ones) */ + if (n_bits_total != NULL) + *n_bits_total = 456; + /* 5.1.2.2 a) the three USF bits d(0),d(1),d(2) are precoded into six bits */ for (i = 0; i < 8; i++) { for (j = 0, k = 0; j < 6; j++) k += abs(((int)gsm0503_usf2six[i][j]) - ((int)conv[j])); @@ -1090,6 +1101,7 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, return 34; case 3: + /* reorder, set punctured bits to 0 (unknown state) */ for (i = 675, j = 455; i >= 0; i--) { if (!gsm0503_puncture_cs3[i]) cB[i] = cB[j--]; @@ -1097,9 +1109,15 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, cB[i] = 0; } - osmo_conv_decode_ber(&gsm0503_cs3_np, cB, - conv, n_errors, n_bits_total); + /* decode as if puncturing was not employed (note '_np') */ + osmo_conv_decode_ber_punctured(&gsm0503_cs3_np, cB, conv, + n_errors, NULL, + gsm0503_puncture_cs3); + /* indicate the actual amount of coded bits (excluding punctured ones) */ + if (n_bits_total != NULL) + *n_bits_total = 456; + /* 5.1.3.2 a) the three USF bits d(0),d(1),d(2) are precoded into six bits */ for (i = 0; i < 8; i++) { for (j = 0, k = 0; j < 6; j++) k += abs(((int)gsm0503_usf2six[i][j]) - ((int)conv[j])); @@ -1128,6 +1146,7 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, for (i = 12; i < 456; i++) conv[i] = (cB[i] < 0) ? 1 : 0; + /* 5.1.4.2 a) the three USF bits d(0),d(1),d(2) are precoded into twelve bits */ for (i = 0; i < 8; i++) { for (j = 0, k = 0; j < 12; j++) k += abs(((int)gsm0503_usf2twelve_sbit[i][j]) - ((int)cB[j])); @@ -1168,7 +1187,7 @@ int gsm0503_pdtch_decode(uint8_t *l2_data, const sbit_t *bursts, uint8_t *usf_p, } /* - * EGPRS PDTCH UL block encoding + * EGPRS PDTCH DL block encoding */ static int egprs_type3_map(ubit_t *bursts, const ubit_t *hc, const ubit_t *dc, int usf) { @@ -1176,7 +1195,7 @@ static int egprs_type3_map(ubit_t *bursts, const ubit_t *hc, const ubit_t *dc, i ubit_t iB[456]; const ubit_t *hl_hn = gsm0503_pdtch_hl_hn_ubit[3]; - gsm0503_mcs1_dl_interleave(gsm0503_usf2six[usf], hc, dc, iB); + gsm0503_mcs1_dl_interleave(gsm0503_usf2twelve_ubit[usf], hc, dc, iB); for (i = 0; i < 4; i++) { gsm0503_xcch_burst_map(&iB[i * 114], &bursts[i * 116], @@ -1332,7 +1351,7 @@ static int egprs_parse_dl_cps(struct egprs_cps *cps, * \param[out] bursts caller-allocated buffer for unpacked burst bits * \param[in] l2_data L2 (MAC) block to be encoded * \param[in] l2_len length of l2_data in bytes, used to determine MCS - * \returns 0 on success; negative on error */ + * \returns number of bits encoded; negative on error */ int gsm0503_pdtch_egprs_encode(ubit_t *bursts, const uint8_t *l2_data, uint8_t l2_len) { @@ -1427,7 +1446,7 @@ bad_header: * \param[out] bursts caller-allocated buffer for unpacked burst bits * \param[in] l2_data L2 (MAC) block to be encoded * \param[in] l2_len length of l2_data in bytes, used to determine CS - * \returns 0 on success; negative on error */ + * \returns number of bits encoded; negative on error */ int gsm0503_pdtch_encode(ubit_t *bursts, const uint8_t *l2_data, uint8_t l2_len) { ubit_t iB[456], cB[676]; @@ -1576,24 +1595,22 @@ static void tch_fr_disassemble(ubit_t *b_bits, } } -/* assemble a HR codec frame in format as used inside RTP */ +/* assemble a HR codec frame in the canonical format of ETSI TS 101 318 */ static void tch_hr_reassemble(uint8_t *tch_data, const ubit_t *b_bits) { - int i, j; - - tch_data[0] = 0x00; /* F = 0, FT = 000 */ - memset(tch_data + 1, 0, 14); + int i; - for (i = 0, j = 8; i < 112; i++, j++) - tch_data[j >> 3] |= (b_bits[i] << (7 - (j & 7))); + memset(tch_data, 0, GSM_HR_BYTES); + for (i = 0; i < 112; i++) + tch_data[i >> 3] |= (b_bits[i] << (7 - (i & 7))); } static void tch_hr_disassemble(ubit_t *b_bits, const uint8_t *tch_data) { - int i, j; + int i; - for (i = 0, j = 8; i < 112; i++, j++) - b_bits[i] = (tch_data[j >> 3] >> (7 - (j & 7))) & 1; + for (i = 0; i < 112; i++) + b_bits[i] = (tch_data[i >> 3] >> (7 - (i & 7))) & 1; } /* assemble a EFR codec frame in format as used inside RTP */ @@ -1635,6 +1652,39 @@ static void tch_amr_disassemble(ubit_t *d_bits, const uint8_t *tch_data, int len d_bits[i] = (tch_data[j >> 3] >> (7 - (j & 7))) & 1; } +/* Append STI and MI bits to the SID_UPDATE frame, see also + * 3GPP TS 26.101, chapter 4.2.3 AMR Core Frame with comfort noise bits */ +static void tch_amr_sid_update_append(ubit_t *sid_update, uint8_t sti, uint8_t mi) +{ + /* Zero out the space that had been used by the CRC14 */ + memset(sid_update + 35, 0, 14); + + /* Append STI and MI parameters */ + sid_update[35] = sti & 1; + sid_update[36] = mi & 1; + sid_update[37] = mi >> 1 & 1; + sid_update[38] = mi >> 2 & 1; +} + +/* Extract a SID UPDATE fram the sbits of an FR AMR frame */ +static void extract_afs_sid_update(sbit_t *sid_update, const sbit_t *sbits) +{ + + unsigned int i; + + sbits += 32; + + for (i = 0; i < 53; i++) { + sid_update[0] = sbits[0]; + sid_update[1] = sbits[1]; + sid_update[2] = sbits[2]; + sid_update[3] = sbits[3]; + sid_update += 4; + sbits += 8; + } + +} + /* re-arrange according to TS 05.03 Table 2 (receiver) */ static void tch_fr_d_to_b(ubit_t *b_bits, const ubit_t *d_bits) { @@ -1775,7 +1825,7 @@ static void tch_efr_unreorder(ubit_t *s, ubit_t *p, const ubit_t *w) s[119] = (sum >= 2); memcpy(s + 121, w + 125, 53); sum = s[172] + w[178] + w[179]; - s[172] = (sum > 2); + s[172] = (sum >= 2); memcpy(s + 174, w + 180, 50); sum = s[222] + w[230] + w[231]; s[222] = (sum >= 2); @@ -1831,7 +1881,7 @@ int gsm0503_tch_fr_decode(uint8_t *tch_data, const sbit_t *bursts, return -1; } - return 23; + return GSM_MACBLOCK_LEN; } osmo_conv_decode_ber(&gsm0503_tch_fr, cB, conv, n_errors, n_bits_total); @@ -1898,40 +1948,39 @@ int gsm0503_tch_fr_encode(ubit_t *bursts, const uint8_t *tch_data, switch (len) { case GSM_EFR_BYTES: /* TCH EFR */ - tch_efr_disassemble(s, tch_data); - tch_efr_protected(s, b); - osmo_crc8gen_set_bits(&gsm0503_tch_efr_crc8, b, 65, p); - tch_efr_reorder(w, s, p); - tch_efr_w_to_d(d, w); - goto coding_efr_fr; case GSM_FR_BYTES: /* TCH FR */ tch_fr_disassemble(w, tch_data, net_order); - tch_fr_b_to_d(d, w); - coding_efr_fr: osmo_crc8gen_set_bits(&gsm0503_tch_fr_crc3, d, 50, p); - tch_fr_reorder(conv, d, p); - memcpy(cB + 378, d + 182, 78); - osmo_conv_encode(&gsm0503_tch_fr, conv, cB); - h = 0; - + break; + case 0: /* no data, induce BFI in the receiver */ + /* Do the same thing that sysmoBTS PHY does when fed a 0-length + * payload for DL: set all u(k) bits to 0, and do the same + * with all class 2 bits. This operation is NOT the same as + * an FR codec frame of all zero bits: with all-zeros d(k) input + * the CRC3 function will produce 111 output, whereas we + * transmit 000 in those parity bits too. The result will be + * an induced BFI (bad frame indication) condition in the + * receiver, for both TCH/FS and TCH/EFS decoders. */ + memset(conv, 0, sizeof(conv)); + memset(cB + 378, 0, 78); + osmo_conv_encode(&gsm0503_tch_fr, conv, cB); + h = 0; break; case GSM_MACBLOCK_LEN: /* FACCH */ _xcch_encode_cB(cB, tch_data); - h = 1; - break; default: return -1; @@ -1948,13 +1997,13 @@ coding_efr_fr: } /*! Perform channel decoding of a HR(v1) channel according TS 05.03 - * \param[out] tch_data Codec frame in RTP payload format - * \param[in] bursts buffer containing the symbols of 8 bursts + * \param[out] tch_data Codec frame in TS 101 318 canonical format + * \param[in] bursts buffer containing the symbols of 6 bursts * \param[in] odd Odd (1) or even (0) frame number * \param[out] n_errors Number of detected bit errors * \param[out] n_bits_total Total number of bits * \returns length of bytes used in \a tch_data output buffer; negative on error */ -int gsm0503_tch_hr_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, +int gsm0503_tch_hr_decode2(uint8_t *tch_data, const sbit_t *bursts, int odd, int *n_errors, int *n_bits_total) { sbit_t iB[912], cB[456], h; @@ -1968,7 +2017,7 @@ int gsm0503_tch_hr_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, steal -= h; } - for (i = 2; i < 5; i++) { + for (i = 2; i < 6; i++) { gsm0503_tch_burst_unmap(NULL, &bursts[i * 116], &h, 1); steal -= h; } @@ -2021,7 +2070,35 @@ int gsm0503_tch_hr_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, tch_hr_reassemble(tch_data, b); - return 15; + return GSM_HR_BYTES; +} + +/*! Perform channel decoding of a HR(v1) channel according TS 05.03, + * deprecated legacy API. + * \param[out] tch_data Codec frame in pseudo-RFC5993 format + * \param[in] bursts buffer containing the symbols of 6 bursts + * \param[in] odd Odd (1) or even (0) frame number + * \param[out] n_errors Number of detected bit errors + * \param[out] n_bits_total Total number of bits + * \returns length of bytes used in \a tch_data output buffer; negative on error + * + * The HR1 codec frame format returned by this function is pseudo-RFC5993, + * not true RFC 5993, as there is no SID classification being done + * and the FT bits in the ToC octet are always set to 0 - but this + * arguably-bogus format is the legacy public API. + */ +int gsm0503_tch_hr_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, + int *n_errors, int *n_bits_total) +{ + int rc; + + rc = gsm0503_tch_hr_decode2(tch_data, bursts, odd, n_errors, + n_bits_total); + if (rc != GSM_HR_BYTES) + return rc; + memmove(tch_data + 1, tch_data, GSM_HR_BYTES); + tch_data[0] = 0x00; /* FT=0, note absence of SID classification */ + return GSM_HR_BYTES_RTP_RFC5993; } /*! Perform channel encoding on a TCH/HS channel according to TS 05.03 @@ -2036,46 +2113,41 @@ int gsm0503_tch_hr_encode(ubit_t *bursts, const uint8_t *tch_data, int len) int i; switch (len) { - case 15: /* TCH HR */ + case GSM_HR_BYTES_RTP_RFC5993: /* TCH HR with RFC 5993 prefix */ + tch_data++; + /* fall-through */ + case GSM_HR_BYTES: /* TCH HR in "pure" form */ tch_hr_disassemble(b, tch_data); - tch_hr_b_to_d(d, b); - osmo_crc8gen_set_bits(&gsm0503_tch_fr_crc3, d + 73, 22, p); - tch_hr_reorder(conv, d, p); - - osmo_conv_encode(&gsm0503_tch_hr, conv, cB); - memcpy(cB + 211, d + 95, 17); - +hr_conv_coding: + osmo_conv_encode(&gsm0503_tch_hr, conv, cB); h = 0; - gsm0503_tch_hr_interleave(cB, iB); - for (i = 0; i < 4; i++) { gsm0503_tch_burst_map(&iB[i * 114], &bursts[i * 116], &h, i >> 1); } - break; + case 0: /* no data, induce BFI in the receiver */ + /* see comments in gsm0503_tch_fr_encode() - same deal here */ + memset(conv, 0, sizeof(conv)); + memset(cB + 211, 0, 17); + goto hr_conv_coding; case GSM_MACBLOCK_LEN: /* FACCH */ _xcch_encode_cB(cB, tch_data); - h = 1; - gsm0503_tch_fr_interleave(cB, iB); - for (i = 0; i < 6; i++) { gsm0503_tch_burst_map(&iB[i * 114], &bursts[i * 116], &h, i >> 2); } - for (i = 2; i < 4; i++) { gsm0503_tch_burst_map(&iB[i * 114 + 456], &bursts[i * 116], &h, 1); } - break; default: return -1; @@ -2084,6 +2156,26 @@ int gsm0503_tch_hr_encode(ubit_t *bursts, const uint8_t *tch_data, int len) return 0; } +/* TCH/AFS: parse codec ID (CMI or CMC/CMR) from coded in-band data (16 bit) */ +static uint8_t gsm0503_tch_afs_decode_inband(const sbit_t *cB) +{ + unsigned int id = 0, best = 0; + unsigned int i, j, k; + + for (i = 0; i < 4; i++) { + /* FIXME: why not using remaining (16 - 8) soft-bits here? */ + for (j = 0, k = 0; j < 8; j++) + k += abs(((int)gsm0503_afs_ic_sbit[i][j]) - ((int)cB[j])); + + if (i == 0 || k < best) { + best = k; + id = i; + } + } + + return id; +} + /*! Perform channel decoding of a TCH/AFS channel according TS 05.03 * \param[out] tch_data Codec frame in RTP payload format * \param[in] bursts buffer containing the symbols of 8 bursts @@ -2101,10 +2193,35 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, int codec_mode_req, uint8_t *codec, int codecs, uint8_t *ft, uint8_t *cmr, int *n_errors, int *n_bits_total) { + return gsm0503_tch_afs_decode_dtx(tch_data, bursts, codec_mode_req, + codec, codecs, ft, cmr, n_errors, + n_bits_total, NULL); +} + +/*! Perform channel decoding of a TCH/AFS channel according TS 05.03 + * \param[out] tch_data Codec frame in RTP payload format + * \param[in] bursts buffer containing the symbols of 8 bursts + * \param[in] codec_mode_req is this CMR (1) or CMC (0) + * \param[in] codec array of active codecs (active codec set) + * \param[in] codecs number of codecs in \a codec + * \param ft Frame Type; Input if \a codec_mode_req = 1, Output * otherwise + * \param[out] cmr Output in \a codec_mode_req = 1 + * \param[out] n_errors Number of detected bit errors + * \param[out] n_bits_total Total number of bits + * \param[inout] dtx DTX frame type output, previous DTX frame type input + * \returns (>=4) length of bytes used in \a tch_data output buffer; ([0,3]) + * codec out of range; negative on error + */ +int gsm0503_tch_afs_decode_dtx(uint8_t *tch_data, const sbit_t *bursts, + int codec_mode_req, uint8_t *codec, int codecs, uint8_t *ft, + uint8_t *cmr, int *n_errors, int *n_bits_total, uint8_t *dtx) +{ sbit_t iB[912], cB[456], h; ubit_t d[244], p[6], conv[250]; - int i, j, k, best = 0, rv, len, steal = 0, id = 0; + int i, rv, len, steal = 0, id = -1; *n_errors = 0; *n_bits_total = 0; + static ubit_t sid_first_dummy[64] = { 0 }; + sbit_t sid_update_enc[256]; for (i=0; i<8; i++) { gsm0503_tch_burst_unmap(&iB[i * 114], &bursts[i * 116], &h, i >> 2); @@ -2114,6 +2231,12 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, gsm0503_tch_fr_deinterleave(cB, iB); if (steal > 0) { + /* If not NULL, dtx indicates type of previously decoded TCH/AFS frame. + * It's normally updated by gsm0503_detect_afs_dtx_frame2(), which is not + * reached in case of FACCH. Reset it here to avoid FACCH/F frames being + * misinterpreted as AMR's special DTX frames. */ + if (dtx != NULL) + *dtx = AMR_OTHER; rv = _xcch_decode_cB(tch_data, cB, n_errors, n_bits_total); if (rv) { /* Error decoding FACCH frame */ @@ -2123,18 +2246,59 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, return GSM_MACBLOCK_LEN; } - for (i = 0; i < 4; i++) { - for (j = 0, k = 0; j < 8; j++) - k += abs(((int)gsm0503_afs_ic_sbit[i][j]) - ((int)cB[j])); + /* Determine the DTX frame type (SID_UPDATE, ONSET etc...) */ + if (dtx) { + const enum gsm0503_amr_dtx_frames dtx_prev = *dtx; + + *dtx = gsm0503_detect_afs_dtx_frame2(n_errors, n_bits_total, &id, cB); + + switch (*dtx) { + case AMR_OTHER: + /* NOTE: The AFS_SID_UPDATE frame is splitted into + * two half rate frames. If the id marker frame + * (AFS_SID_UPDATE) is detected the following frame + * contains the actual comfort noised data part of + * (AFS_SID_UPDATE_CN). */ + if (dtx_prev != AFS_SID_UPDATE) + break; + /* TODO: parse CMI _and_ CMC/CMR (16 + 16 bit) */ + *dtx = AFS_SID_UPDATE_CN; + + extract_afs_sid_update(sid_update_enc, cB); + osmo_conv_decode_ber(&gsm0503_tch_axs_sid_update, + sid_update_enc, conv, n_errors, + n_bits_total); + rv = osmo_crc16gen_check_bits(&gsm0503_amr_crc14, conv, + 35, conv + 35); + if (rv != 0) { + /* Error checking CRC14 for an AMR SID_UPDATE frame */ + return -1; + } - if (i == 0 || k < best) { - best = k; - id = i; + tch_amr_sid_update_append(conv, 1, + (codec_mode_req) ? codec[*ft] + : codec[id > 0 ? id : 0]); + tch_amr_reassemble(tch_data, conv, 39); + len = 5; + goto out; + case AFS_SID_FIRST: /* TODO: parse CMI or CMC/CMR (16 bit) */ + tch_amr_sid_update_append(sid_first_dummy, 0, + (codec_mode_req) ? codec[*ft] + : codec[id > 0 ? id : 0]); + tch_amr_reassemble(tch_data, sid_first_dummy, 39); + len = 5; + goto out; + case AFS_SID_UPDATE: /* TODO: parse CMI _and_ CMC/CMR (16 + 16 bit) */ + case AFS_ONSET: + len = 0; + goto out; + default: + break; } } - /* Check if indicated codec fits into range of codecs */ - if (id >= codecs) { + /* Parse codec ID (CMI or CMC/CMR) and check if it fits into range of codecs */ + if ((id = gsm0503_tch_afs_decode_inband(&cB[0])) >= codecs) { /* Codec mode out of range, return id */ return id; } @@ -2283,11 +2447,14 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, return -1; } +out: /* Change codec request / indication, if frame is valid */ - if (codec_mode_req) - *cmr = id; - else - *ft = id; + if (id != -1) { + if (codec_mode_req) + *cmr = id; + else + *ft = id; + } return len; } @@ -2295,7 +2462,7 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, /*! Perform channel encoding on a TCH/AFS channel according to TS 05.03 * \param[out] bursts caller-allocated output buffer for bursts bits * \param[in] tch_data Codec input data in RTP payload format - * \param[in] len Length of \a tch_data in bytes + * \param[in] len Length of \a tch_data in bytes or 0 to generate a bad frame * \param[in] codec_mode_req Use CMR (1) or FT (0) * \param[in] codec Array of codecs (active codec set) * \param[in] codecs Number of entries in \a codec @@ -2303,7 +2470,7 @@ int gsm0503_tch_afs_decode(uint8_t *tch_data, const sbit_t *bursts, * \param[in] cmr Codec Mode Request (used in codec_mode_req = 1 only) * \returns 0 in case of success; negative on error */ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, - int codec_mode_req, uint8_t *codec, int codecs, uint8_t ft, + int codec_mode_req, const uint8_t *codec, int codecs, uint8_t ft, uint8_t cmr) { ubit_t iB[912], cB[456], h; @@ -2321,28 +2488,27 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, h = 0; - if (codec_mode_req) { - if (cmr >= codecs) { - /* FIXME: CMR ID is not in codec list! */ - return -1; - } - id = cmr; - } else { - if (ft >= codecs) { - /* FIXME: FT ID is not in codec list! */ - return -1; - } - id = ft; - } + id = codec_mode_req ? cmr : ft; + if (OSMO_UNLIKELY(id >= ARRAY_SIZE(gsm0503_afs_ic_ubit))) + return -1; + if (OSMO_UNLIKELY(ft >= codecs)) + return -1; switch (codec[ft]) { case 7: /* TCH/AFS12.2 */ - if (len != 31) - goto invalid_length; - - tch_amr_disassemble(d, tch_data, 244); - - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 81, p); + if (!len) { + /* No data, induce BFI in the receiver by inverted CRC bits. + * The data bit are all 0, so the correct parity bits would be 111111. */ + memset(d, 0, 244); + memset(p, 0, 6); + } else { + if (len != 31) + goto invalid_length; + + tch_amr_disassemble(d, tch_data, 244); + + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 81, p); + } tch_amr_merge(conv, d, p, 244, 81); @@ -2350,12 +2516,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 6: /* TCH/AFS10.2 */ - if (len != 26) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 204); + memset(p, 0, 6); + } else { + if (len != 26) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 204); + tch_amr_disassemble(d, tch_data, 204); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 65, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 65, p); + } tch_amr_merge(conv, d, p, 204, 65); @@ -2363,12 +2535,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 5: /* TCH/AFS7.95 */ - if (len != 20) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 159); + memset(p, 0, 6); + } else { + if (len != 20) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 159); + tch_amr_disassemble(d, tch_data, 159); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 75, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 75, p); + } tch_amr_merge(conv, d, p, 159, 75); @@ -2376,12 +2554,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 4: /* TCH/AFS7.4 */ - if (len != 19) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 148); + memset(p, 0, 6); + } else { + if (len != 19) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 148); + tch_amr_disassemble(d, tch_data, 148); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 61, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 61, p); + } tch_amr_merge(conv, d, p, 148, 61); @@ -2389,12 +2573,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 3: /* TCH/AFS6.7 */ - if (len != 17) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 134); + memset(p, 0, 6); + } else { + if (len != 17) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 134); + tch_amr_disassemble(d, tch_data, 134); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + } tch_amr_merge(conv, d, p, 134, 55); @@ -2402,12 +2592,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 2: /* TCH/AFS5.9 */ - if (len != 15) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 118); + memset(p, 0, 6); + } else { + if (len != 15) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 118); + tch_amr_disassemble(d, tch_data, 118); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + } tch_amr_merge(conv, d, p, 118, 55); @@ -2415,12 +2611,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 1: /* TCH/AFS5.15 */ - if (len != 13) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 103); + memset(p, 0, 6); + } else { + if (len != 13) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 103); + tch_amr_disassemble(d, tch_data, 103); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 49, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 49, p); + } tch_amr_merge(conv, d, p, 103, 49); @@ -2428,12 +2630,18 @@ int gsm0503_tch_afs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 0: /* TCH/AFS4.75 */ - if (len != 12) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 95); + memset(p, 0, 6); + } else { + if (len != 12) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 95); + tch_amr_disassemble(d, tch_data, 95); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 39, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 39, p); + } tch_amr_merge(conv, d, p, 95, 39); @@ -2462,9 +2670,29 @@ invalid_length: return -1; } -/*! Perform channel decoding of a TCH/AFS channel according TS 05.03 +/* TCH/AHS: parse codec ID (CMI or CMC/CMR) from coded in-band data (16 bit) */ +static uint8_t gsm0503_tch_ahs_decode_inband(const sbit_t *cB) +{ + unsigned int id = 0, best = 0; + unsigned int i, j, k; + + for (i = 0, k = 0; i < 4; i++) { + /* FIXME: why not using remaining (16 - 4) soft-bits here? */ + for (j = 0, k = 0; j < 4; j++) + k += abs(((int)gsm0503_ahs_ic_sbit[i][j]) - ((int)cB[j])); + + if (i == 0 || k < best) { + best = k; + id = i; + } + } + + return id; +} + +/*! Perform channel decoding of a TCH/AHS channel according TS 05.03 * \param[out] tch_data Codec frame in RTP payload format - * \param[in] bursts buffer containing the symbols of 8 bursts + * \param[in] bursts buffer containing the symbols of 6 bursts * \param[in] odd Is this an odd (1) or even (0) frame number? * \param[in] codec_mode_req is this CMR (1) or CMC (0) * \param[in] codec array of active codecs (active codec set) @@ -2480,9 +2708,34 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, int codec_mode_req, uint8_t *codec, int codecs, uint8_t *ft, uint8_t *cmr, int *n_errors, int *n_bits_total) { + return gsm0503_tch_ahs_decode_dtx(tch_data, bursts, odd, codec_mode_req, + codec, codecs, ft, cmr, n_errors, + n_bits_total, NULL); +} + +/*! Perform channel decoding of a TCH/AHS channel according TS 05.03 + * \param[out] tch_data Codec frame in RTP payload format + * \param[in] bursts buffer containing the symbols of 6 bursts + * \param[in] odd Is this an odd (1) or even (0) frame number? + * \param[in] codec_mode_req is this CMR (1) or CMC (0) + * \param[in] codec array of active codecs (active codec set) + * \param[in] codecs number of codecs in \a codec + * \param ft Frame Type; Input if \a codec_mode_req = 1, Output * otherwise + * \param[out] cmr Output in \a codec_mode_req = 1 + * \param[out] n_errors Number of detected bit errors + * \param[out] n_bits_total Total number of bits + * \param[inout] dtx DTX frame type output, previous DTX frame type input + * \returns (>=4) length of bytes used in \a tch_data output buffer; ([0,3]) + * codec out of range; negative on error + */ +int gsm0503_tch_ahs_decode_dtx(uint8_t *tch_data, const sbit_t *bursts, int odd, + int codec_mode_req, uint8_t *codec, int codecs, uint8_t *ft, + uint8_t *cmr, int *n_errors, int *n_bits_total, uint8_t *dtx) +{ sbit_t iB[912], cB[456], h; ubit_t d[244], p[6], conv[135]; - int i, j, k, best = 0, rv, len, steal = 0, id = 0; + int i, rv, len, steal = 0, id = -1; + static ubit_t sid_first_dummy[64] = { 0 }; /* only unmap the stealing bits */ if (!odd) { @@ -2498,6 +2751,13 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, /* if we found a stole FACCH, but only at correct alignment */ if (steal > 0) { + /* If not NULL, dtx indicates type of previously decoded TCH/AHS frame. + * It's normally updated by gsm0503_detect_ahs_dtx_frame2(), which is not + * reached in case of FACCH. Reset it here to avoid FACCH/H frames being + * misinterpreted as AMR's special DTX frames. */ + if (dtx != NULL) + *dtx = AMR_OTHER; + for (i = 0; i < 6; i++) { gsm0503_tch_burst_unmap(&iB[i * 114], &bursts[i * 116], NULL, i >> 2); @@ -2526,18 +2786,72 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, gsm0503_tch_hr_deinterleave(cB, iB); - for (i = 0; i < 4; i++) { - for (j = 0, k = 0; j < 4; j++) - k += abs(((int)gsm0503_ahs_ic_sbit[i][j]) - ((int)cB[j])); + /* Determine the DTX frame type (SID_UPDATE, ONSET etc...) */ + if (dtx) { + int n_bits_total_sid; + int n_errors_sid; + + *dtx = gsm0503_detect_ahs_dtx_frame2(n_errors, n_bits_total, &id, cB); + /* TODO: detect and handle AHS_SID_UPDATE + AHS_SID_UPDATE_INH */ + + switch (*dtx) { + case AHS_SID_UPDATE: /* TODO: parse CMI _and_ CMC/CMR (16 + 16 bit) */ + /* cB[] contains 16 bits of coded in-band data and 212 bits containing + * the identification marker. We need to unmap/deinterleave 114 odd + * bits from the last two blocks, 114 even bits from the first two + * blocks and combine them together. */ + gsm0503_tch_burst_unmap(&iB[0 * 114], &bursts[2 * 116], NULL, 0); + gsm0503_tch_burst_unmap(&iB[1 * 114], &bursts[3 * 116], NULL, 0); + gsm0503_tch_burst_unmap(&iB[2 * 114], &bursts[0 * 116], NULL, 1); + gsm0503_tch_burst_unmap(&iB[3 * 114], &bursts[1 * 116], NULL, 1); + gsm0503_tch_hr_deinterleave(cB, iB); + + /* cB[] is expected to contain 16 bits of coded in-band data and + * 212 bits containing the coded data (53 bits coded at 1/4 rate). */ + *dtx = AHS_SID_UPDATE_CN; + + osmo_conv_decode_ber(&gsm0503_tch_axs_sid_update, + cB + 16, conv, &n_errors_sid, + &n_bits_total_sid); + /* gsm0503_detect_ahs_dtx_frame2() calculates BER for the marker, + * osmo_conv_decode_ber() calculates BER for the coded data. */ + if (n_errors != NULL) + *n_errors += n_errors_sid; + if (n_bits_total != NULL) + *n_bits_total += n_bits_total_sid; + rv = osmo_crc16gen_check_bits(&gsm0503_amr_crc14, conv, + 35, conv + 35); + if (rv != 0) { + /* Error checking CRC14 for an AMR SID_UPDATE frame */ + return -1; + } - if (i == 0 || k < best) { - best = k; - id = i; + tch_amr_sid_update_append(conv, 1, + (codec_mode_req) ? codec[*ft] + : codec[id > 0 ? id : 0]); + tch_amr_reassemble(tch_data, conv, 39); + len = 5; + goto out; + case AHS_SID_FIRST_P2: + tch_amr_sid_update_append(sid_first_dummy, 0, + (codec_mode_req) ? codec[*ft] + : codec[id > 0 ? id : 0]); + tch_amr_reassemble(tch_data, sid_first_dummy, 39); + len = 5; + goto out; + case AHS_ONSET: + case AHS_SID_FIRST_INH: /* TODO: parse CMI or CMC/CMR (16 bit) */ + case AHS_SID_UPDATE_INH: /* TODO: parse CMI or CMC/CMR (16 bit) */ + case AHS_SID_FIRST_P1: /* TODO: parse CMI or CMC/CMR (16 bit) */ + len = 0; + goto out; + default: + break; } } - /* Check if indicated codec fits into range of codecs */ - if (id >= codecs) { + /* Parse codec ID (CMI or CMC/CMR) and check if it fits into range of codecs */ + if ((id = gsm0503_tch_ahs_decode_inband(&cB[0])) >= codecs) { /* Codec mode out of range, return id */ return id; } @@ -2670,11 +2984,14 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, return -1; } +out: /* Change codec request / indication, if frame is valid */ - if (codec_mode_req) - *cmr = id; - else - *ft = id; + if (id != -1) { + if (codec_mode_req) + *cmr = id; + else + *ft = id; + } return len; } @@ -2682,7 +2999,7 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, /*! Perform channel encoding on a TCH/AHS channel according to TS 05.03 * \param[out] bursts caller-allocated output buffer for bursts bits * \param[in] tch_data Codec input data in RTP payload format - * \param[in] len Length of \a tch_data in bytes + * \param[in] len Length of \a tch_data in bytes or 0 to generate a bad frame * \param[in] codec_mode_req Use CMR (1) or FT (0) * \param[in] codec Array of codecs (active codec set) * \param[in] codecs Number of entries in \a codec @@ -2690,7 +3007,7 @@ int gsm0503_tch_ahs_decode(uint8_t *tch_data, const sbit_t *bursts, int odd, * \param[in] cmr Codec Mode Request (used in codec_mode_req = 1 only) * \returns 0 in case of success; negative on error */ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, - int codec_mode_req, uint8_t *codec, int codecs, uint8_t ft, + int codec_mode_req, const uint8_t *codec, int codecs, uint8_t ft, uint8_t cmr) { ubit_t iB[912], cB[456], h; @@ -2717,28 +3034,27 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, h = 0; - if (codec_mode_req) { - if (cmr >= codecs) { - /* FIXME: CMR ID %d not in codec list */ - return -1; - } - id = cmr; - } else { - if (ft >= codecs) { - /* FIXME: FT ID %d not in codec list */ - return -1; - } - id = ft; - } + id = codec_mode_req ? cmr : ft; + if (OSMO_UNLIKELY(id >= ARRAY_SIZE(gsm0503_ahs_ic_ubit))) + return -1; + if (OSMO_UNLIKELY(ft >= codecs)) + return -1; switch (codec[ft]) { case 5: /* TCH/AHS7.95 */ - if (len != 20) - goto invalid_length; - - tch_amr_disassemble(d, tch_data, 159); - - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 67, p); + if (!len) { + /* No data, induce BFI in the receiver by inverted CRC bits. + * The data bit are all 0, so the correct parity bits would be 111111. */ + memset(d, 0, 159); + memset(p, 0, 6); + } else { + if (len != 20) + goto invalid_length; + + tch_amr_disassemble(d, tch_data, 159); + + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 67, p); + } tch_amr_merge(conv, d, p, 123, 67); @@ -2748,12 +3064,18 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 4: /* TCH/AHS7.4 */ - if (len != 19) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 148); + memset(p, 0, 6); + } else { + if (len != 19) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 148); + tch_amr_disassemble(d, tch_data, 148); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 61, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 61, p); + } tch_amr_merge(conv, d, p, 120, 61); @@ -2763,12 +3085,18 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 3: /* TCH/AHS6.7 */ - if (len != 17) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 134); + memset(p, 0, 6); + } else { + if (len != 17) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 134); + tch_amr_disassemble(d, tch_data, 134); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + } tch_amr_merge(conv, d, p, 110, 55); @@ -2778,12 +3106,18 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 2: /* TCH/AHS5.9 */ - if (len != 15) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 118); + memset(p, 0, 6); + } else { + if (len != 15) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 118); + tch_amr_disassemble(d, tch_data, 118); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 55, p); + } tch_amr_merge(conv, d, p, 102, 55); @@ -2793,12 +3127,18 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 1: /* TCH/AHS5.15 */ - if (len != 13) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 103); + memset(p, 0, 6); + } else { + if (len != 13) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 103); + tch_amr_disassemble(d, tch_data, 103); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 49, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 49, p); + } tch_amr_merge(conv, d, p, 91, 49); @@ -2808,12 +3148,18 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, break; case 0: /* TCH/AHS4.75 */ - if (len != 12) - goto invalid_length; + if (!len) { + /* See comment above. */ + memset(d, 0, 95); + memset(p, 0, 6); + } else { + if (len != 12) + goto invalid_length; - tch_amr_disassemble(d, tch_data, 95); + tch_amr_disassemble(d, tch_data, 95); - osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 39, p); + osmo_crc8gen_set_bits(&gsm0503_amr_crc6, d, 39, p); + } tch_amr_merge(conv, d, p, 83, 39); @@ -2827,7 +3173,7 @@ int gsm0503_tch_ahs_encode(ubit_t *bursts, const uint8_t *tch_data, int len, return -1; } - memcpy(cB, gsm0503_afs_ic_ubit[id], 4); + memcpy(cB, gsm0503_ahs_ic_ubit[id], 4); gsm0503_tch_hr_interleave(cB, iB); @@ -2879,7 +3225,7 @@ static inline int16_t rach_decode_ber(const sbit_t *burst, uint8_t bsic, bool is osmo_ubit2pbit_ext(ra, 0, conv, 0, nbits, 1); - return is_11bit ? osmo_load16le(ra) : ra[0]; + return is_11bit ? ((ra[0] << 3) | (ra[1] & 0x07)) : ra[0]; } /*! Decode the Extended (11-bit) RACH according to 3GPP TS 45.003 @@ -2974,7 +3320,8 @@ int gsm0503_rach_ext_encode(ubit_t *burst, uint16_t ra11, uint8_t bsic, bool is_ uint8_t ra[2] = { 0 }, nbits = 8; if (is_11bit) { - osmo_store16le(ra11, ra); + ra[0] = (uint8_t) (ra11 >> 3); + ra[1] = (uint8_t) (ra11 & 0x07); nbits = 11; } else ra[0] = (uint8_t)ra11; @@ -3031,4 +3378,491 @@ int gsm0503_sch_encode(ubit_t *burst, const uint8_t *sb_info) return 0; } +/* + * GSM CSD transcoding + */ + +static inline void _tch_csd_burst_map(ubit_t *burst, const ubit_t *iB) +{ + unsigned int i; + + /* hu(B): copy *even* numbered bits if not stolen by FACCH */ + if (burst[58] == 0) { + for (i = 0; i < 57; i += 2) + burst[i] |= iB[i]; + for (i = 58; i < 114; i += 2) + burst[i + 2] |= iB[i]; + } + + /* hl(B): copy *odd* numbered bits if not stolen by FACCH */ + if (burst[57] == 0) { + for (i = 1; i < 57; i += 2) + burst[i] |= iB[i]; + for (i = 57; i < 114; i += 2) + burst[i + 2] |= iB[i]; + } +} + +/*! Perform channel encoding of a TCH/F9.6 channel as per section 3.3. + * \param[out] bursts Caller-allocated buffer for symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[in] data Data to be encoded (240 unpacked bits). + * \returns 0 in case of success; negative on error. */ +int gsm0503_tch_fr96_encode(ubit_t *bursts, const ubit_t *data) +{ + ubit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[4 * 60 + 4]; + + /* 3.3.2 Block code: b1(60) + b2(60) + b3(60) + b4(60) + pad(4) */ + memcpy(&conv[0], &data[0], 4 * 60); + /* pad(4) is set to 0 by osmo_conv_encode() below */ + + /* 3.3.3 Convolutional encoder */ + osmo_conv_encode(&gsm0503_tch_f96, &conv[0], &cB[0]); + + /* 3.3.4 Interleaving */ + memset(&iB[0], 0, sizeof(iB)); + gsm0503_tch_f96_interleave(&cB[0], &iB[0]); + + /* 3.3.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) + _tch_csd_burst_map(&bursts[i * 116], &iB[i * 114]); + + return 0; +} + +/*! Perform channel decoding of a TCH/F9.6 channel as per section 3.3. + * \param[out] data Caller-allocated buffer for decoded data (240 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_fr96_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[4 * 60 + 4]; + + /* 3.3.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) { + memcpy(&iB[i * 114], &bursts[i * 116], 57); + memcpy(&iB[i * 114 + 57], &bursts[i * 116 + 59], 57); + } + + /* 3.3.4 Interleaving */ + gsm0503_tch_f96_deinterleave(&cB[0], &iB[0]); + + /* 3.3.3 Convolutional encoder */ + osmo_conv_decode_ber(&gsm0503_tch_f96, &cB[0], &conv[0], n_errors, n_bits_total); + + /* 3.3.2 Block code: b1(60) + b2(60) + b3(60) + b4(60) + pad(4) */ + memcpy(&data[0], &conv[0], 4 * 60); + + return 4 * 60; +} + +/*! Perform channel encoding of a TCH/F4.8 channel as per section 3.4. + * \param[out] bursts Caller-allocated buffer for symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[in] data Data to be encoded (120 unpacked bits). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_fr48_encode(ubit_t *bursts, const ubit_t *data) +{ + ubit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[2 * 60 + 32]; + + /* 3.4.2 Block code: + * + * Sixteen bits equal to 0 are added to the 60 information bits, the result + * being a block of 76 bits, {u(0),u(1),...,u(75)}, with: + * + * u(19k+p) = d(15k+p) for k = 0,1,2,3 and p = 0,1,...,14; + * u(19k+p) = 0 for k = 0,1,2,3 and p = 15,16,17,18. + * + * Two such blocks forming a block of 152 bits: u1 + u2. */ + for (unsigned int k = 0; k < 2 * 4; k++) { + memcpy(&conv[19 * k], &data[15 * k], 15); + memset(&conv[19 * k + 15], 0, 4); + } + + /* 3.4.3 Convolutional encoder */ + osmo_conv_encode(&gsm0503_tch_f48, &conv[0], &cB[0]); + + /* 3.4.4 Interleaving: as specified for the TCH/F9.6 in subclause 3.3.4 */ + memset(&iB[0], 0, sizeof(iB)); + gsm0503_tch_f96_interleave(&cB[0], &iB[0]); + + /* 3.4.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) + _tch_csd_burst_map(&bursts[i * 116], &iB[i * 114]); + + return 0; +} + +/*! Perform channel decoding of a TCH/F4.8 channel as per section 3.4. + * \param[out] data Caller-allocated buffer for decoded data (120 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_fr48_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[2 * 60 + 32]; + + /* 3.4.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) { + memcpy(&iB[i * 114], &bursts[i * 116], 57); + memcpy(&iB[i * 114 + 57], &bursts[i * 116 + 59], 57); + } + + /* 3.4.4 Interleaving: as specified for the TCH/F9.6 in subclause 3.3.4 */ + gsm0503_tch_f96_deinterleave(&cB[0], &iB[0]); + + /* 3.4.3 Convolutional encoder */ + osmo_conv_decode_ber(&gsm0503_tch_f48, &cB[0], &conv[0], n_errors, n_bits_total); + + /* 3.4.2 Block code: + * + * Sixteen bits equal to 0 are added to the 60 information bits, the result + * being a block of 76 bits, {u(0),u(1),...,u(75)}, with: + * + * u(19k+p) = d(15k+p) for k = 0,1,2,3 and p = 0,1,...,14; + * u(19k+p) = 0 for k = 0,1,2,3 and p = 15,16,17,18. + * + * Two such blocks forming a block of 152 bits: u1 + u2. */ + for (unsigned int k = 0; k < 2 * 4; k++) + memcpy(&data[15 * k], &conv[19 * k], 15); + + return 2 * 60; +} + +/*! Perform channel encoding of a TCH/H4.8 channel as per section 3.5. + * The algorithm is identical to TCH/F9.6, so it's just a wrapper. + * \param[out] bursts Caller-allocated buffer for symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[in] data Data to be encoded (240 unpacked bits). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_hr48_encode(ubit_t *bursts, const ubit_t *data) +{ + return gsm0503_tch_fr96_encode(bursts, data); +} + +/*! Perform channel decoding of a TCH/H4.8 channel as per section 3.5. + * The algorithm is identical to TCH/F9.6, so it's just a wrapper. + * \param[out] data Caller-allocated buffer for decoded data (240 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_hr48_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + return gsm0503_tch_fr96_decode(data, bursts, n_errors, n_bits_total); +} + +/*! Perform channel encoding of a TCH/F2.4 channel as per section 3.6. + * \param[out] bursts Caller-allocated buffer for symbols of 8 bursts, + * 8 * 2 * 58 == 928 bits total. + * \param[in] data Data to be encoded (72 unpacked bits). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_fr24_encode(ubit_t *bursts, const ubit_t *data) +{ + ubit_t iB[8 * 114], cB[4 * 114]; + const ubit_t h = 0; + + /* 3.6.{1-3} Block code and Convolutional encoder */ + osmo_conv_encode(&gsm0503_tch_f24, &data[0], &cB[0]); + + /* 3.6.4 Interleaving: as specified for the TCH/FS in subclause 3.1.3 */ + gsm0503_tch_fr_interleave(&cB[0], &iB[0]); + + /* 3.6.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 8; i++) + gsm0503_tch_burst_map(&iB[i * 114], &bursts[i * 116], &h, i >> 2); + + return 0; +} + +/*! Perform channel decoding of a TCH/F2.4 channel as per section 3.6. + * \param[out] data Caller-allocated buffer for decoded data (72 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 8 bursts, + * 8 * 2 * 58 == 928 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_fr24_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[8 * 114], cB[4 * 114]; + + /* 3.6.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 8; i++) + gsm0503_tch_burst_unmap(&iB[i * 114], &bursts[i * 116], NULL, i >> 2); + + /* 3.6.4 Interleaving: as specified for the TCH/FS in subclause 3.1.3 */ + gsm0503_tch_fr_deinterleave(&cB[0], &iB[0]); + + /* 3.6.{1-3} Block code and Convolutional encoder */ + osmo_conv_decode_ber(&gsm0503_tch_f24, &cB[0], &data[0], n_errors, n_bits_total); + + return 72; +} + +/*! Perform channel encoding of a TCH/H2.4 channel as per section 3.7. + * \param[out] bursts Caller-allocated buffer for symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[in] data Data to be encoded (144 unpacked bits). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_hr24_encode(ubit_t *bursts, const ubit_t *data) +{ + ubit_t iB[22 * 114], cB[4 * 114]; + + /* 3.7.{1-3} Block code and Convolutional encoder */ + osmo_conv_encode(&gsm0503_tch_h24, &data[ 0], &cB[ 0]); + osmo_conv_encode(&gsm0503_tch_h24, &data[72], &cB[228]); + + /* 3.7.4 Interleaving: as specified for the TCH/F9.6 in subclause 3.3.4 */ + memset(&iB[0], 0, sizeof(iB)); + gsm0503_tch_f96_interleave(&cB[0], &iB[0]); + + /* 3.7.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) + _tch_csd_burst_map(&bursts[i * 116], &iB[i * 114]); + + return 0; +} + +/*! Perform channel decoding of a TCH/H2.4 channel as per section 3.7. + * \param[out] data Caller-allocated buffer for decoded data (144 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_hr24_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + int n_errors_l[2], n_bits_total_l[2]; + sbit_t iB[22 * 114], cB[4 * 114]; + + /* 3.7.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) { + memcpy(&iB[i * 114], &bursts[i * 116], 57); + memcpy(&iB[i * 114 + 57], &bursts[i * 116 + 59], 57); + } + + /* 3.7.4 Interleaving: as specified for the TCH/F9.6 in subclause 3.3.4 */ + gsm0503_tch_f96_deinterleave(&cB[0], &iB[0]); + + /* 3.7.{1-3} Block code and Convolutional encoder */ + osmo_conv_decode_ber(&gsm0503_tch_h24, &cB[ 0], &data[ 0], &n_errors_l[0], &n_bits_total_l[0]); + osmo_conv_decode_ber(&gsm0503_tch_h24, &cB[228], &data[72], &n_errors_l[1], &n_bits_total_l[1]); + + if (n_errors) + *n_errors = n_errors_l[0] + n_errors_l[1]; + + if (n_bits_total) + *n_bits_total = n_bits_total_l[0] + n_bits_total_l[1]; + + return 2 * 72; +} + +/*! Perform channel encoding of a TCH/F14.4 channel as per section 3.8. + * \param[out] bursts Caller-allocated buffer for symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[in] data Data to be encoded (290 unpacked bits). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_fr144_encode(ubit_t *bursts, const ubit_t *data) +{ + ubit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[290 + 4]; + + /* 3.8.2 Block code: b(290) + pad(4) */ + memcpy(&conv[0], &data[0], 290); + /* pad(4) is set to 0 by osmo_conv_encode() below */ + + /* 3.8.3 Convolutional encoder */ + osmo_conv_encode(&gsm0503_tch_f144, &conv[0], &cB[0]); + + /* 3.8.4 Interleaving */ + memset(&iB[0], 0, sizeof(iB)); + gsm0503_tch_f96_interleave(&cB[0], &iB[0]); + + /* 3.8.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) + _tch_csd_burst_map(&bursts[i * 116], &iB[i * 114]); + + return 0; +} + +/*! Perform channel decoding of a TCH/14.4 channel as per section 3.8. + * \param[out] data Caller-allocated buffer for decoded data (290 unpacked bits). + * \param[in] bursts Buffer containing the symbols of 22 bursts, + * 22 * 2 * 58 == 2552 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of unpacked bits used in the output buffer; negative on error. */ +int gsm0503_tch_fr144_decode(ubit_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[22 * 114], cB[4 * 114]; + ubit_t conv[294]; + + /* 3.8.5 Mapping on a burst: as specified for TCH/FS in subclause 3.1.4 */ + for (unsigned int i = 0; i < 22; i++) { + memcpy(&iB[i * 114], &bursts[i * 116], 57); + memcpy(&iB[i * 114 + 57], &bursts[i * 116 + 59], 57); + } + + /* 3.8.4 Interleaving: as specified for the TCH/F9.6 in subclause 3.3.4 */ + gsm0503_tch_f96_deinterleave(&cB[0], &iB[0]); + + /* 3.8.3 Convolutional encoder */ + osmo_conv_decode_ber(&gsm0503_tch_f144, &cB[0], &conv[0], n_errors, n_bits_total); + + /* 3.8.2 Block code: b(290) + pad(4) */ + memcpy(&data[0], &conv[0], 290); + + return 290; +} + +/* + * FACCH/[FH] transcoding + */ + +/*! Perform channel encoding of a FACCH/F data as per section 4.2. + * \param[out] bursts Caller-allocated buffer for symbols of 8 bursts, + * 8 * 2 * 58 == 928 bits total. + * \param[in] data FACCH MAC block to be encoded (GSM_MACBLOCK_LEN). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_fr_facch_encode(ubit_t *bursts, const uint8_t *data) +{ + ubit_t iB[8 * 114], cB[4 * 114]; + const ubit_t h = 1; + + /* 4.2.1-3 as specified for the SACCH in 4.1.1-3 */ + _xcch_encode_cB(&cB[0], &data[0]); + + /* 4.2.4 Interleaving: as specified for the TCH/FS in subclause 3.1.3 */ + gsm0503_tch_fr_interleave(&cB[0], &iB[0]); + + /* 4.2.5 Mapping on a Burst: + * - hu(B)=1 the even numbered bits in the first 4 bursts and + * - hl(B)=1 the odd numbered bits of the last 4 bursts are stolen. */ + for (unsigned int i = 0; i < 8; i++) + gsm0503_tch_burst_map(&iB[i * 114], &bursts[i * 116], &h, i >> 2); + + return 0; +} + +/*! Perform channel decoding of a FACCH/F data as per section 4.2. + * \param[out] data Caller-allocated buffer for decoded FACCH (GSM_MACBLOCK_LEN). + * \param[in] bursts Buffer containing the symbols of 8 bursts, + * 8 * 2 * 58 == 928 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of bytes used in the output buffer; negative on error. */ +int gsm0503_tch_fr_facch_decode(uint8_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[8 * 114], cB[4 * 114]; + int steal = 0; + + /* FACCH decision: sum of 4 first hu(B) and 4 last hl(B) soft-bits */ + for (unsigned int i = 0; i < 4; i++) + steal -= bursts[i * 116 + 58]; /* hu(B) */ + for (unsigned int i = 4; i < 8; i++) + steal -= bursts[i * 116 + 57]; /* hl(B) */ + if (steal <= 0) + return -1; + + /* 4.2.5 Mapping on a Burst: + * - hu(B)=1 the even numbered bits in the first 4 bursts and + * - hl(B)=1 the odd numbered bits of the last 4 bursts are stolen. */ + for (unsigned int i = 0; i < 8; i++) + gsm0503_tch_burst_unmap(&iB[i * 114], &bursts[i * 116], NULL, i >> 2); + + /* 4.2.4 Interleaving: as specified for the TCH/FS in subclause 3.1.3 */ + gsm0503_tch_fr_deinterleave(&cB[0], &iB[0]); + + /* 4.2.1-3 as specified for the SACCH in 4.1.1-3 */ + if (_xcch_decode_cB(&data[0], &cB[0], n_errors, n_bits_total) != 0) + return -1; + + return GSM_MACBLOCK_LEN; +} + +/*! Perform channel encoding of a FACCH/H data as per section 4.3. + * \param[out] bursts Caller-allocated buffer for symbols of 6 bursts, + * 6 * 2 * 58 == 696 bits total. + * \param[in] data FACCH MAC block to be encoded (GSM_MACBLOCK_LEN). + * \returns 0 in case of success; negative on error */ +int gsm0503_tch_hr_facch_encode(ubit_t *bursts, const uint8_t *data) +{ + ubit_t iB[8 * 114], cB[4 * 114]; + const ubit_t h = 1; + + /* 4.3.1-3 as specified for the SACCH in 4.1.1-3 */ + _xcch_encode_cB(&cB[0], &data[0]); + + /* 4.3.4 Interleaving */ + gsm0503_tch_fr_interleave(&cB[0], &iB[0]); + + /* 4.3.5 Mapping on a Burst: + * - hu(B)=1 the even numbered bits of the first 2 bursts, + * - hu(B)=1 & hl(B)=1 all bits of the middle 2 bursts and + * - hl(B)=1 the odd numbered bits of the last 2 bursts are stolen. */ + for (unsigned int i = 0; i < 6; i++) + gsm0503_tch_burst_map(&iB[i * 114], &bursts[i * 116], &h, i >> 2); + for (unsigned int i = 2; i < 4; i++) + gsm0503_tch_burst_map(&iB[i * 114 + 456], &bursts[i * 116], &h, 1); + + return 0; +} + +/*! Perform channel decoding of a FACCH/H data as per section 4.3. + * \param[out] data Caller-allocated buffer for decoded FACCH (GSM_MACBLOCK_LEN). + * \param[in] bursts Buffer containing the symbols of 6 bursts, + * 6 * 2 * 58 == 696 bits total. + * \param[out] n_errors Number of detected bit errors. + * \param[out] n_bits_total Total number of bits. + * \returns Number of bytes used in the output buffer; negative on error. */ +int gsm0503_tch_hr_facch_decode(uint8_t *data, const sbit_t *bursts, + int *n_errors, int *n_bits_total) +{ + sbit_t iB[8 * 114], cB[4 * 114]; + int steal = 0; + + /* FACCH decision: sum of 4 first hu(B) and 4 last hl(B) soft-bits */ + for (unsigned int i = 0; i < 4; i++) + steal -= bursts[i * 116 + 58]; /* hu(B) */ + for (unsigned int i = 2; i < 6; i++) + steal -= bursts[i * 116 + 57]; /* hl(B) */ + if (steal <= 0) + return -1; + + /* 4.3.5 Mapping on a Burst: + * - hu(B)=1 the even numbered bits of the first 2 bursts, + * - hu(B)=1 & hl(B)=1 all bits of the middle 2 bursts and + * - hl(B)=1 the odd numbered bits of the last 2 bursts are stolen. */ + for (unsigned int i = 0; i < 6; i++) + gsm0503_tch_burst_unmap(&iB[i * 114], &bursts[i * 116], NULL, i >> 2); + for (unsigned int i = 2; i < 4; i++) + gsm0503_tch_burst_unmap(&iB[i * 114 + 456], &bursts[i * 116], NULL, 1); + + /* 4.3.4 Interleaving */ + gsm0503_tch_fr_deinterleave(&cB[0], &iB[0]); + + /* 4.3.1-3 as specified for the SACCH in 4.1.1-3 */ + if (_xcch_decode_cB(&data[0], &cB[0], n_errors, n_bits_total) != 0) + return -1; + + return GSM_MACBLOCK_LEN; +} + /*! @} */ diff --git a/src/coding/gsm0503_interleaving.c b/src/coding/gsm0503_interleaving.c index d5008d07..570d65aa 100644 --- a/src/coding/gsm0503_interleaving.c +++ b/src/coding/gsm0503_interleaving.c @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> @@ -682,4 +678,56 @@ void gsm0503_tch_hr_interleave(const ubit_t *cB, ubit_t *iB) } } +/* 3GPP TS 45.003 Section 3.3.4 + * The coded bits are reordered and interleaved according to the following rule: + * i(B,j) = c(n,k) for k = 0,1,...,455 + * n = 0,1,...,N,N + 1,... + * B = B0 +4n + (k mod 19) + (k div 114) + * j = (k mod 19) + 19 (k mod 6) + * + * The result of the interleaving is a distribution of the reordered 114 + * bit of a given data block, n = N, over 19 blocks, 6 bits equally + * distributed in each block, in a diagonal way over consecutive blocks. + * + * Or in other words the interleaving is a distribution of the encoded, + * reordered 456 bits from four given input data blocks, which taken + * together give n = N, over 22 bursts, 6 bits equally distributed in + * the first and 22 nd bursts, 12 bits distributed in the second and 21 + * st bursts, 18 bits distributed in the third and 20 th bursts and 24 + * bits distributed in the other 16 bursts. + * + * The block of coded data is interleaved "diagonal", where a new block + * of coded data starts with every fourth burst and is distributed over + * 22 bursts. + * + * Also used for TCH/F4.8, TCH/H4.8, and TCH/H2.4 and TCH/F14.4 */ +void gsm0503_tch_f96_interleave(const ubit_t *cB, ubit_t *iB) +{ + int j, k, B; + + for (k = 0; k < 456; k++) { + /* upper bound for B: 4*n + 18 + 4 = 4*n + 22 */ + B = /* B0 + 4n + */ (k % 19) + (k / 114); + /* upper bound for j: 18 + 19*5 = 113 */ + j = (k % 19) + 19 * (k % 6); + /* upper iB index: 4*n+23*114-1 */ + iB[B * 114 + j] = cB[k]; + } +} + +void gsm0503_tch_f96_deinterleave(sbit_t *cB, const sbit_t *iB) +{ + int j, k, B; + + for (k = 0; k < 456; k++) { + /* upper bound for B: 4*n + 18 + 4 = 4*n + 22 */ + B = /* B0 + 4n + */ (k % 19) + (k / 114); + /* upper bound for j: 18 + 19*5 = 113 */ + j = (k % 19) + 19 * (k % 6); + /* upper iB index: 4*n+23*114-1 */ + cB[k] = iB[B * 114 + j]; + } +} + + /*! @} */ diff --git a/src/coding/gsm0503_mapping.c b/src/coding/gsm0503_mapping.c index f7532eb2..04acfd07 100644 --- a/src/coding/gsm0503_mapping.c +++ b/src/coding/gsm0503_mapping.c @@ -15,10 +15,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> diff --git a/src/coding/gsm0503_parity.c b/src/coding/gsm0503_parity.c index 874114ff..ef71d351 100644 --- a/src/coding/gsm0503_parity.c +++ b/src/coding/gsm0503_parity.c @@ -15,10 +15,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> @@ -134,4 +130,15 @@ const struct osmo_crc8gen_code gsm0503_amr_crc6 = { .remainder = 0x3f, }; +/*! GSM AMR parity (SID_UPDATE) + * + * g(x) = x^14 + x^13 + x^5 + x^3 + x^2 + 1 + */ +const struct osmo_crc16gen_code gsm0503_amr_crc14 = { + .bits = 14, + .poly = 0x202d, + .init = 0x0000, + .remainder = 0x3fff, +}; + /*! @} */ diff --git a/src/coding/gsm0503_tables.c b/src/coding/gsm0503_tables.c index 5fe634bf..25ea2fa3 100644 --- a/src/coding/gsm0503_tables.c +++ b/src/coding/gsm0503_tables.c @@ -15,10 +15,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> @@ -63,6 +59,9 @@ const sbit_t gsm0503_pdtch_edge_hl_hn_sbit[3][8] = { { -127,-127, -127, 127, 127,-127, -127,-127 }, }; +/* + * 3GPP TS 05.03 sec 5.1.2.2 "Block code". Rows re-ordered to be indxed by USF in host bit order. + */ const ubit_t gsm0503_usf2six[8][6] = { { 0,0,0, 0,0,0 }, { 1,0,0, 1,0,1 }, @@ -74,6 +73,9 @@ const ubit_t gsm0503_usf2six[8][6] = { { 1,1,1, 0,0,0 }, }; +/* + * 3GPP TS 05.03 sec 5.1.4.2 "Block code". Rows re-ordered to be indxed by USF in host bit order. + */ const ubit_t gsm0503_usf2twelve_ubit[8][12] = { { 0,0,0, 0,0,0, 0,0,0, 0,0,0 }, { 1,1,0, 1,0,0, 0,0,1, 0,1,1 }, diff --git a/src/coding/libosmocoding.map b/src/coding/libosmocoding.map index 87b38864..0444690e 100644 --- a/src/coding/libosmocoding.map +++ b/src/coding/libosmocoding.map @@ -56,6 +56,7 @@ gsm0503_sch_crc10; gsm0503_tch_fr_crc3; gsm0503_tch_efr_crc8; gsm0503_amr_crc6; +gsm0503_amr_crc14; gsm0503_xcch_burst_unmap; gsm0503_xcch_burst_map; @@ -75,6 +76,8 @@ gsm0503_xcch_deinterleave; gsm0503_xcch_interleave; gsm0503_tch_fr_deinterleave; gsm0503_tch_fr_interleave; +gsm0503_tch_f96_deinterleave; +gsm0503_tch_f96_interleave; gsm0503_tch_hr_deinterleave; gsm0503_tch_hr_interleave; gsm0503_mcs1_ul_deinterleave; @@ -104,10 +107,13 @@ gsm0503_tch_fr_encode; gsm0503_tch_fr_decode; gsm0503_tch_hr_encode; gsm0503_tch_hr_decode; +gsm0503_tch_hr_decode2; gsm0503_tch_afs_encode; gsm0503_tch_afs_decode; +gsm0503_tch_afs_decode_dtx; gsm0503_tch_ahs_encode; gsm0503_tch_ahs_decode; +gsm0503_tch_ahs_decode_dtx; gsm0503_rach_ext_encode; gsm0503_rach_ext_decode; gsm0503_rach_ext_decode_ber; @@ -116,6 +122,30 @@ gsm0503_rach_decode; gsm0503_rach_decode_ber; gsm0503_sch_encode; gsm0503_sch_decode; +gsm0503_amr_dtx_frame_names; +gsm0503_amr_dtx_frame_name; +gsm0503_detect_afs_dtx_frame; +gsm0503_detect_ahs_dtx_frame; +gsm0503_detect_afs_dtx_frame2; +gsm0503_detect_ahs_dtx_frame2; + +gsm0503_tch_fr96_encode; +gsm0503_tch_fr96_decode; +gsm0503_tch_fr48_encode; +gsm0503_tch_fr48_decode; +gsm0503_tch_hr48_encode; +gsm0503_tch_hr48_decode; +gsm0503_tch_fr24_encode; +gsm0503_tch_fr24_decode; +gsm0503_tch_hr24_encode; +gsm0503_tch_hr24_decode; +gsm0503_tch_fr144_encode; +gsm0503_tch_fr144_decode; + +gsm0503_tch_fr_facch_encode; +gsm0503_tch_fr_facch_decode; +gsm0503_tch_hr_facch_encode; +gsm0503_tch_hr_facch_decode; local: *; }; diff --git a/src/core/Makefile.am b/src/core/Makefile.am new file mode 100644 index 00000000..2efebd8d --- /dev/null +++ b/src/core/Makefile.am @@ -0,0 +1,166 @@ +# This is _NOT_ the library release version, it's an API version. +# Please read chapter "Library interface versions" of the libtool documentation +# before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html +LIBVERSION=21:0:0 + +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) $(PTHREAD_CFLAGS) $(LIBSCTP_CFLAGS) $(LIBMNL_CFLAGS) $(URING_CFLAGS) + +if ENABLE_PSEUDOTALLOC +AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc +endif + +lib_LTLIBRARIES = libosmocore.la + +libosmocore_la_LIBADD = \ + $(BACKTRACE_LIB) \ + $(TALLOC_LIBS) \ + $(LIBRARY_RT) \ + $(PTHREAD_LIBS) \ + $(LIBSCTP_LIBS) \ + $(URING_LIBS) \ + $(NULL) + +libosmocore_la_SOURCES = \ + application.c \ + backtrace.c \ + base64.c \ + bits.c \ + bitvec.c \ + bitcomp.c \ + context.c \ + conv.c \ + conv_acc.c \ + conv_acc_generic.c \ + counter.c \ + crc16.c \ + crc8gen.c \ + crc16gen.c \ + crc32gen.c \ + crc64gen.c \ + exec.c \ + fsm.c \ + gsmtap_util.c \ + isdnhdlc.c \ + it_q.c \ + logging.c \ + logging_syslog.c \ + logging_gsmtap.c \ + loggingrb.c \ + macaddr.c \ + msgb.c \ + netdev.c \ + netns.c \ + osmo_io.c \ + osmo_io_poll.c \ + panic.c \ + prbs.c \ + prim.c \ + rate_ctr.c \ + rbtree.c \ + select.c \ + sercomm.c \ + signal.c \ + sockaddr_str.c \ + socket.c \ + soft_uart.c \ + stat_item.c \ + stats.c \ + stats_statsd.c \ + stats_tcp.c \ + strrb.c \ + tdef.c \ + thread.c \ + time_cc.c \ + timer.c \ + timer_gettimeofday.c \ + timer_clockgettime.c \ + tun.c \ + use_count.c \ + utils.c \ + write_queue.c \ + probes.d \ + $(NULL) + +if HAVE_SSSE3 +libosmocore_la_SOURCES += conv_acc_sse.c +if HAVE_SSE4_1 +conv_acc_sse.lo : AM_CFLAGS += -mssse3 -msse4.1 +else +conv_acc_sse.lo : AM_CFLAGS += -mssse3 +endif + +if HAVE_AVX2 +libosmocore_la_SOURCES += conv_acc_sse_avx.c +if HAVE_SSE4_1 +conv_acc_sse_avx.lo : AM_CFLAGS += -mssse3 -mavx2 -msse4.1 +else +conv_acc_sse_avx.lo : AM_CFLAGS += -mssse3 -mavx2 +endif +endif +endif + +if HAVE_NEON +libosmocore_la_SOURCES += conv_acc_neon.c +# conv_acc_neon.lo : AM_CFLAGS += -mfpu=neon no, could as well be vfp with neon +endif + +BUILT_SOURCES = crc8gen.c crc16gen.c crc32gen.c crc64gen.c + +EXTRA_DIST = \ + conv_acc_sse_impl.h \ + conv_acc_neon_impl.h \ + crcXXgen.c.tpl \ + osmo_io_internal.h \ + stat_item_internal.h \ + libosmocore.map \ + $(NULL) + +EXTRA_libosmocore_la_DEPENDENCIES = libosmocore.map + +libosmocore_la_LDFLAGS = \ + $(LTLDFLAGS_OSMOCORE) \ + -version-info \ + $(LIBVERSION) \ + -no-undefined + +if ENABLE_PLUGIN +libosmocore_la_SOURCES += plugin.c +libosmocore_la_LIBADD += $(LIBRARY_DLOPEN) +endif + +if ENABLE_MSGFILE +libosmocore_la_SOURCES += msgfile.c +endif + +if ENABLE_SERIAL +libosmocore_la_SOURCES += serial.c +endif + +if ENABLE_SYSTEMD_LOGGING +libosmocore_la_SOURCES += logging_systemd.c +libosmocore_la_LIBADD += $(SYSTEMD_LIBS) +endif + +if ENABLE_LIBMNL +libosmocore_la_SOURCES += mnl.c +libosmocore_la_LIBADD += $(LIBMNL_LIBS) +endif + +if ENABLE_SYSTEMTAP +probes.h: probes.d + $(DTRACE) -C -h -s $< -o $@ + +probes.lo: probes.d + $(LIBTOOL) --mode=compile $(AM_V_lt) --tag=CC env CFLAGS="$(CFLAGS)" $(DTRACE) -C -G -s $< -o $@ + +BUILT_SOURCES += probes.h probes.lo +libosmocore_la_LIBADD += probes.lo +endif + +if ENABLE_URING +libosmocore_la_SOURCES += osmo_io_uring.c +endif + +crc%gen.c: crcXXgen.c.tpl + $(AM_V_GEN)sed -e's/XX/$*/g' $< > $@ diff --git a/src/application.c b/src/core/application.c index 7fd62808..f7e5816f 100644 --- a/src/application.c +++ b/src/core/application.c @@ -18,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \mainpage libosmocore Documentation diff --git a/src/backtrace.c b/src/core/backtrace.c index a18bde02..60bd2381 100644 --- a/src/backtrace.c +++ b/src/core/backtrace.c @@ -18,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdio.h> diff --git a/src/core/base64.c b/src/core/base64.c new file mode 100644 index 00000000..0c161ceb --- /dev/null +++ b/src/core/base64.c @@ -0,0 +1,195 @@ +/* + * RFC 1521 base64 encoding/decoding + * + * Copyright (C) 2006-2015, ARM Limited, All Rights Reserved + * + * This file is part of mbed TLS (https://tls.mbed.org) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <osmocom/core/base64.h> + +#include <stdint.h> +#include <stdio.h> +#include <errno.h> + +static const unsigned char base64_enc_map[64] = { + 'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', + 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', + 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', + 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', + 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', '+', '/' +}; + +static const unsigned char base64_dec_map[128] = { + 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, + 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, + 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, + 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, + 127, 127, 127, 62, 127, 127, 127, 63, 52, 53, + 54, 55, 56, 57, 58, 59, 60, 61, 127, 127, + 127, 64, 127, 127, 127, 0, 1, 2, 3, 4, + 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, + 25, 127, 127, 127, 127, 127, 127, 26, 27, 28, + 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, + 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, + 49, 50, 51, 127, 127, 127, 127, 127 +}; + +/* + * Encode a buffer into base64 format + */ +int osmo_base64_encode(unsigned char *dst, size_t dlen, size_t *olen, + const unsigned char *src, size_t slen) +{ + size_t i, n; + int C1, C2, C3; + unsigned char *p; + + if (slen == 0) { + *olen = 0; + return 0; + } + + n = (slen << 3) / 6; + + switch ((slen << 3) - (n * 6)) { + case 2: + n += 3; + break; + case 4: + n += 2; + break; + default: + break; + } + + if (dlen < n + 1) { + *olen = n + 1; + return -ENOBUFS; + } + + n = (slen / 3) * 3; + + for (i = 0, p = dst; i < n; i += 3) { + C1 = *src++; + C2 = *src++; + C3 = *src++; + + *p++ = base64_enc_map[(C1 >> 2) & 0x3F]; + *p++ = base64_enc_map[(((C1 & 3) << 4) + (C2 >> 4)) & 0x3F]; + *p++ = base64_enc_map[(((C2 & 15) << 2) + (C3 >> 6)) & 0x3F]; + *p++ = base64_enc_map[C3 & 0x3F]; + } + + if (i < slen) { + C1 = *src++; + C2 = ((i + 1) < slen) ? *src++ : 0; + + *p++ = base64_enc_map[(C1 >> 2) & 0x3F]; + *p++ = base64_enc_map[(((C1 & 3) << 4) + (C2 >> 4)) & 0x3F]; + + if ((i + 1) < slen) + *p++ = base64_enc_map[((C2 & 15) << 2) & 0x3F]; + else + *p++ = '='; + + *p++ = '='; + } + + *olen = p - dst; + *p = 0; + + return 0; +} + +/* + * Decode a base64-formatted buffer + */ +int osmo_base64_decode(unsigned char *dst, size_t dlen, size_t *olen, + const unsigned char *src, size_t slen) +{ + size_t i, n; + uint32_t j, x; + unsigned char *p; + + /* First pass: check for validity and get output length */ + for (i = n = j = 0; i < slen; i++) { + /* Skip spaces before checking for EOL */ + x = 0; + while (i < slen && src[i] == ' ') { + ++i; + ++x; + } + + /* Spaces at end of buffer are OK */ + if (i == slen) + break; + + if ((slen - i) >= 2 && src[i] == '\r' && src[i + 1] == '\n') + continue; + + if (src[i] == '\n') + continue; + + /* Space inside a line is an error */ + if (x != 0) + return -EINVAL; + + if (src[i] == '=' && ++j > 2) + return -EINVAL; + + if (src[i] > 127 || base64_dec_map[src[i]] == 127) + return -EINVAL; + + if (base64_dec_map[src[i]] < 64 && j != 0) + return -EINVAL; + + n++; + } + + if (n == 0) + return 0; + + n = ((n * 6) + 7) >> 3; + n -= j; + + if (dst == NULL || dlen < n) { + *olen = n; + return -ENOBUFS; + } + + for (j = 3, n = x = 0, p = dst; i > 0; i--, src++) { + if (*src == '\r' || *src == '\n' || *src == ' ') + continue; + + j -= (base64_dec_map[*src] == 64); + x = (x << 6) | (base64_dec_map[*src] & 0x3F); + + if (++n == 4) { + n = 0; + if (j > 0) + *p++ = (unsigned char)(x >> 16); + if (j > 1) + *p++ = (unsigned char)(x >> 8); + if (j > 2) + *p++ = (unsigned char)(x); + } + } + + *olen = p - dst; + + return 0; +} diff --git a/src/bitcomp.c b/src/core/bitcomp.c index 6f2fb62b..5fb2cba2 100644 --- a/src/bitcomp.c +++ b/src/core/bitcomp.c @@ -18,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \defgroup bitcomp Bit compression diff --git a/src/bits.c b/src/core/bits.c index 8837c1fb..3da7d9b9 100644 --- a/src/bits.c +++ b/src/core/bits.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> @@ -185,14 +181,14 @@ int osmo_pbit2ubit(ubit_t *out, const pbit_t *in, unsigned int num_bits) * \param[in] in input buffer of unpacked bits * \param[in] in_ofs offset into input buffer * \param[in] num_bits number of bits - * \param[in] lsb_mode Encode bits in LSB orde instead of MSB + * \param[in] lsb_mode Encode bits in LSB order instead of MSB * \returns length in bytes (max written offset of output buffer + 1) */ int osmo_ubit2pbit_ext(pbit_t *out, unsigned int out_ofs, const ubit_t *in, unsigned int in_ofs, unsigned int num_bits, int lsb_mode) { - int i, op, bn; + unsigned int i, op, bn; for (i=0; i<num_bits; i++) { op = out_ofs + i; bn = lsb_mode ? (op&7) : (7-(op&7)); @@ -210,14 +206,14 @@ int osmo_ubit2pbit_ext(pbit_t *out, unsigned int out_ofs, * \param[in] in input buffer of packed bits * \param[in] in_ofs offset into input buffer * \param[in] num_bits number of bits - * \param[in] lsb_mode Encode bits in LSB orde instead of MSB + * \param[in] lsb_mode Encode bits in LSB order instead of MSB * \returns length in bytes (max written offset of output buffer + 1) */ int osmo_pbit2ubit_ext(ubit_t *out, unsigned int out_ofs, const pbit_t *in, unsigned int in_ofs, unsigned int num_bits, int lsb_mode) { - int i, ip, bn; + unsigned int i, ip, bn; for (i=0; i<num_bits; i++) { ip = in_ofs + i; bn = lsb_mode ? (ip&7) : (7-(ip&7)); @@ -226,6 +222,35 @@ int osmo_pbit2ubit_ext(ubit_t *out, unsigned int out_ofs, return out_ofs + num_bits; } +/* look-up table for bit-reversal within a byte. Generated using: + int i,k; + for (i = 0 ; i < 256 ; i++) { + uint8_t sample = 0 ; + for (k = 0; k<8; k++) { + if ( i & 1 << k ) sample |= 0x80 >> k; + } + flip_table[i] = sample; + } + */ +static const uint8_t flip_table[256] = { + 0x00, 0x80, 0x40, 0xc0, 0x20, 0xa0, 0x60, 0xe0, 0x10, 0x90, 0x50, 0xd0, 0x30, 0xb0, 0x70, 0xf0, + 0x08, 0x88, 0x48, 0xc8, 0x28, 0xa8, 0x68, 0xe8, 0x18, 0x98, 0x58, 0xd8, 0x38, 0xb8, 0x78, 0xf8, + 0x04, 0x84, 0x44, 0xc4, 0x24, 0xa4, 0x64, 0xe4, 0x14, 0x94, 0x54, 0xd4, 0x34, 0xb4, 0x74, 0xf4, + 0x0c, 0x8c, 0x4c, 0xcc, 0x2c, 0xac, 0x6c, 0xec, 0x1c, 0x9c, 0x5c, 0xdc, 0x3c, 0xbc, 0x7c, 0xfc, + 0x02, 0x82, 0x42, 0xc2, 0x22, 0xa2, 0x62, 0xe2, 0x12, 0x92, 0x52, 0xd2, 0x32, 0xb2, 0x72, 0xf2, + 0x0a, 0x8a, 0x4a, 0xca, 0x2a, 0xaa, 0x6a, 0xea, 0x1a, 0x9a, 0x5a, 0xda, 0x3a, 0xba, 0x7a, 0xfa, + 0x06, 0x86, 0x46, 0xc6, 0x26, 0xa6, 0x66, 0xe6, 0x16, 0x96, 0x56, 0xd6, 0x36, 0xb6, 0x76, 0xf6, + 0x0e, 0x8e, 0x4e, 0xce, 0x2e, 0xae, 0x6e, 0xee, 0x1e, 0x9e, 0x5e, 0xde, 0x3e, 0xbe, 0x7e, 0xfe, + 0x01, 0x81, 0x41, 0xc1, 0x21, 0xa1, 0x61, 0xe1, 0x11, 0x91, 0x51, 0xd1, 0x31, 0xb1, 0x71, 0xf1, + 0x09, 0x89, 0x49, 0xc9, 0x29, 0xa9, 0x69, 0xe9, 0x19, 0x99, 0x59, 0xd9, 0x39, 0xb9, 0x79, 0xf9, + 0x05, 0x85, 0x45, 0xc5, 0x25, 0xa5, 0x65, 0xe5, 0x15, 0x95, 0x55, 0xd5, 0x35, 0xb5, 0x75, 0xf5, + 0x0d, 0x8d, 0x4d, 0xcd, 0x2d, 0xad, 0x6d, 0xed, 0x1d, 0x9d, 0x5d, 0xdd, 0x3d, 0xbd, 0x7d, 0xfd, + 0x03, 0x83, 0x43, 0xc3, 0x23, 0xa3, 0x63, 0xe3, 0x13, 0x93, 0x53, 0xd3, 0x33, 0xb3, 0x73, 0xf3, + 0x0b, 0x8b, 0x4b, 0xcb, 0x2b, 0xab, 0x6b, 0xeb, 0x1b, 0x9b, 0x5b, 0xdb, 0x3b, 0xbb, 0x7b, 0xfb, + 0x07, 0x87, 0x47, 0xc7, 0x27, 0xa7, 0x67, 0xe7, 0x17, 0x97, 0x57, 0xd7, 0x37, 0xb7, 0x77, 0xf7, + 0x0f, 0x8f, 0x4f, 0xcf, 0x2f, 0xaf, 0x6f, 0xef, 0x1f, 0x9f, 0x5f, 0xdf, 0x3f, 0xbf, 0x7f, 0xff, +}; + /*! generalized bit reversal function * \param[in] x the 32bit value to be reversed * \param[in] k the type of reversal requested @@ -265,16 +290,10 @@ uint32_t osmo_revbytebits_32(uint32_t x) /*! reverse the bit order in a byte * \param[in] x 8bit input value * \returns 8bit value where bits order has been reversed - * - * See Chapter 7 "Hackers Delight" */ uint32_t osmo_revbytebits_8(uint8_t x) { - x = (x & 0x55) << 1 | (x & 0xAA) >> 1; - x = (x & 0x33) << 2 | (x & 0xCC) >> 2; - x = (x & 0x0F) << 4 | (x & 0xF0) >> 4; - - return x; + return flip_table[x]; } /*! reverse bit-order of each byte in a buffer @@ -285,27 +304,10 @@ uint32_t osmo_revbytebits_8(uint8_t x) */ void osmo_revbytebits_buf(uint8_t *buf, int len) { - unsigned int i; - unsigned int unaligned_cnt; - int len_remain = len; - - unaligned_cnt = ((unsigned long)buf & 3); - for (i = 0; i < unaligned_cnt; i++) { - buf[i] = osmo_revbytebits_8(buf[i]); - len_remain--; - if (len_remain <= 0) - return; - } + int i; - for (i = unaligned_cnt; i + 3 < len; i += 4) { - osmo_store32be(osmo_revbytebits_32(osmo_load32be(buf + i)), buf + i); - len_remain -= 4; - } - - for (i = len - len_remain; i < len; i++) { - buf[i] = osmo_revbytebits_8(buf[i]); - len_remain--; - } + for (i = 0; i < len; i++) + buf[i] = flip_table[buf[i]]; } /*! @} */ diff --git a/src/bitvec.c b/src/core/bitvec.c index 0c263ad6..ac702b94 100644 --- a/src/bitvec.c +++ b/src/core/bitvec.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup bitvec @@ -45,6 +41,7 @@ #include <osmocom/core/bits.h> #include <osmocom/core/bitvec.h> #include <osmocom/core/panic.h> +#include <osmocom/core/utils.h> #define BITNUM_FROM_COMP(byte, bit) ((byte*8)+bit) @@ -200,7 +197,8 @@ int bitvec_get_bit_high(struct bitvec *bv) * \return 0 on success; negative in case of error */ int bitvec_set_bits(struct bitvec *bv, const enum bit_value *bits, unsigned int count) { - int i, rc; + unsigned int i; + int rc; for (i = 0; i < count; i++) { rc = bitvec_set_bit(bv, bits[i]); @@ -263,7 +261,7 @@ int16_t bitvec_get_int16_msb(const struct bitvec *bv, unsigned int num_bits) * \return integer value retrieved from bit vector */ int bitvec_get_uint(struct bitvec *bv, unsigned int num_bits) { - int i; + unsigned int i; unsigned int ui = 0; for (i = 0; i < num_bits; i++) { @@ -271,7 +269,7 @@ int bitvec_get_uint(struct bitvec *bv, unsigned int num_bits) if (bit < 0) return bit; if (bit) - ui |= (1 << (num_bits - i - 1)); + ui |= ((unsigned)1 << (num_bits - i - 1)); bv->cur_bit++; } @@ -291,7 +289,7 @@ int bitvec_fill(struct bitvec *bv, unsigned int num_bits, enum bit_value fill) return 0; } -/*! pad all remaining bits up to num_bits +/*! pad all remaining bits up to a given bit number * \return 0 on success; negative otherwise */ int bitvec_spare_padding(struct bitvec *bv, unsigned int up_to_bit) { @@ -397,9 +395,9 @@ int bitvec_set_bytes(struct bitvec *bv, const uint8_t *bytes, unsigned int count * \param[in] size Number of bytes in the vector * \param[in] ctx Context from which to allocate * \return pointer to allocated vector; NULL in case of error */ -struct bitvec *bitvec_alloc(unsigned int size, TALLOC_CTX *ctx) +struct bitvec *bitvec_alloc(unsigned int size, void *ctx) { - struct bitvec *bv = talloc_zero(ctx, struct bitvec); + struct bitvec *bv = talloc(ctx, struct bitvec); if (!bv) return NULL; @@ -418,6 +416,8 @@ struct bitvec *bitvec_alloc(unsigned int size, TALLOC_CTX *ctx) * \param[in] bit vector to free */ void bitvec_free(struct bitvec *bv) { + if (bv == NULL) + return; talloc_free(bv->data); talloc_free(bv); } @@ -428,7 +428,7 @@ void bitvec_free(struct bitvec *bv) * \return number of bytes (= bits) copied */ unsigned int bitvec_pack(const struct bitvec *bv, uint8_t *buffer) { - unsigned int i = 0; + unsigned int i; for (i = 0; i < bv->data_len; i++) buffer[i] = bv->data[i]; @@ -441,7 +441,7 @@ unsigned int bitvec_pack(const struct bitvec *bv, uint8_t *buffer) * \return number of bytes (= bits) copied */ unsigned int bitvec_unpack(struct bitvec *bv, const uint8_t *buffer) { - unsigned int i = 0; + unsigned int i; for (i = 0; i < bv->data_len; i++) bv->data[i] = buffer[i]; @@ -455,17 +455,13 @@ unsigned int bitvec_unpack(struct bitvec *bv, const uint8_t *buffer) */ int bitvec_unhex(struct bitvec *bv, const char *src) { - unsigned i; - unsigned val; - unsigned write_index = 0; - unsigned digits = bv->data_len * 2; + int rc; - for (i = 0; i < digits; i++) { - if (sscanf(src + i, "%1x", &val) < 1) { - return 1; - } - bitvec_write_field(bv, &write_index, val, 4); - } + rc = osmo_hexparse(src, bv->data, bv->data_len); + if (rc < 0) /* turn -1 into 1 in case of error */ + return 1; + + bv->cur_bit = rc * 8; return 0; } @@ -473,19 +469,29 @@ int bitvec_unhex(struct bitvec *bv, const char *src) * \param[in] bv The boolean vector to work on * \param[in,out] read_index Where reading supposed to start in the vector * \param[in] len How many bits to read from vector - * \returns read bits or negative value on error + * \returns An integer made up of the bits read. + * + * In case of an error, errno is set to a non-zero value. Otherwise it holds 0. */ uint64_t bitvec_read_field(struct bitvec *bv, unsigned int *read_index, unsigned int len) { unsigned int i; uint64_t ui = 0; + + /* Prevent bitvec overrun due to incorrect index and/or length */ + if (len && bytenum_from_bitnum(*read_index + len - 1) >= bv->data_len) { + errno = EOVERFLOW; + return 0; + } + bv->cur_bit = *read_index; + errno = 0; for (i = 0; i < len; i++) { - int bit = bitvec_get_bit_pos((const struct bitvec *)bv, bv->cur_bit); - if (bit < 0) - return bit; - if (bit) + unsigned int bytenum = bytenum_from_bitnum(bv->cur_bit); + unsigned int bitnum = 7 - (bv->cur_bit % 8); + + if (bv->data[bytenum] & (1 << bitnum)) ui |= ((uint64_t)1 << (len - i - 1)); bv->cur_bit++; } @@ -497,7 +503,7 @@ uint64_t bitvec_read_field(struct bitvec *bv, unsigned int *read_index, unsigned * \param[in] bv The boolean vector to work on * \param[in,out] write_index Where writing supposed to start in the vector * \param[in] len How many bits to write - * \returns next write index or negative value on error + * \returns 0 on success, negative value on error */ int bitvec_write_field(struct bitvec *bv, unsigned int *write_index, uint64_t val, unsigned int len) { @@ -534,13 +540,11 @@ char bit_value_to_char(enum bit_value v) */ void bitvec_to_string_r(const struct bitvec *bv, char *str) { - unsigned i, pos = 0; char *cur = str; - for (i = 0; i < bv->cur_bit; i++) { + for (unsigned int i = 0; i < bv->cur_bit; i++) { if (0 == i % 8) *cur++ = ' '; *cur++ = bit_value_to_char(bitvec_get_bit_pos(bv, i)); - pos++; } *cur = 0; } @@ -598,7 +602,7 @@ unsigned bitvec_rl(const struct bitvec *bv, bool b) * \returns Number of consecutive bits of \p b in \p bv and cur_bit will * \go to cur_bit + number of consecutive bit */ -unsigned bitvec_rl_curbit(struct bitvec *bv, bool b, int max_bits) +unsigned bitvec_rl_curbit(struct bitvec *bv, bool b, unsigned int max_bits) { unsigned i = 0; unsigned j = 8; diff --git a/src/context.c b/src/core/context.c index bad012bd..a0b3a555 100644 --- a/src/context.c +++ b/src/core/context.c @@ -2,7 +2,7 @@ * talloc context handling. * * (C) 2019 by Harald Welte <laforge@gnumonks.org> - * All Rights Reserverd. + * All Rights Reserved. * * SPDX-License-Identifier: GPL-2.0+ * @@ -15,11 +15,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301, USA. */ #include <string.h> #include <errno.h> @@ -44,7 +39,7 @@ int osmo_ctx_init(const char *id) } /* initialize osmo_ctx on main tread */ -static __attribute__((constructor)) void on_dso_load_ctx(void) +static __attribute__((constructor(101))) void on_dso_load_ctx(void) { OSMO_ASSERT(osmo_ctx_init("main") == 0); } diff --git a/src/conv.c b/src/core/conv.c index a2c13def..8963018b 100644 --- a/src/conv.c +++ b/src/core/conv.c @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /*! \addtogroup conv @@ -36,6 +32,7 @@ #include <stdlib.h> #include <string.h> +#include <osmocom/core/utils.h> #include <osmocom/core/bits.h> #include <osmocom/core/conv.h> @@ -66,7 +63,7 @@ osmo_conv_get_output_length(const struct osmo_conv_code *code, int len) /* Count punctured bits */ if (code->puncture) { - for (pbits=0; code->puncture[pbits] >= 0; pbits++); + for (pbits = 0; code->puncture[pbits] >= 0; pbits++) {} out_len -= pbits; } @@ -84,20 +81,21 @@ osmo_conv_get_output_length(const struct osmo_conv_code *code, int len) */ void osmo_conv_encode_init(struct osmo_conv_encoder *encoder, - const struct osmo_conv_code *code) + const struct osmo_conv_code *code) { memset(encoder, 0x00, sizeof(struct osmo_conv_encoder)); + OSMO_ASSERT(code != NULL); encoder->code = code; } void osmo_conv_encode_load_state(struct osmo_conv_encoder *encoder, - const ubit_t *input) + const ubit_t *input) { int i; uint8_t state = 0; - for (i=0; i<(encoder->code->K-1); i++) + for (i = 0; i < (encoder->code->K - 1); i++) state = (state << 1) | input[i]; encoder->state = state; @@ -105,15 +103,14 @@ osmo_conv_encode_load_state(struct osmo_conv_encoder *encoder, static inline int _conv_encode_do_output(struct osmo_conv_encoder *encoder, - uint8_t out, ubit_t *output) + uint8_t out, ubit_t *output) { const struct osmo_conv_code *code = encoder->code; int o_idx = 0; int j; if (code->puncture) { - for (j=0; j<code->N; j++) - { + for (j = 0; j < code->N; j++) { int bit_no = code->N - j - 1; int r_idx = encoder->i_idx * code->N + j; @@ -123,8 +120,7 @@ _conv_encode_do_output(struct osmo_conv_encoder *encoder, output[o_idx++] = (out >> bit_no) & 1; } } else { - for (j=0; j<code->N; j++) - { + for (j = 0; j < code->N; j++) { int bit_no = code->N - j - 1; output[o_idx++] = (out >> bit_no) & 1; } @@ -135,7 +131,7 @@ _conv_encode_do_output(struct osmo_conv_encoder *encoder, int osmo_conv_encode_raw(struct osmo_conv_encoder *encoder, - const ubit_t *input, ubit_t *output, int n) + const ubit_t *input, ubit_t *output, int n) { const struct osmo_conv_code *code = encoder->code; uint8_t state; @@ -145,11 +141,11 @@ osmo_conv_encode_raw(struct osmo_conv_encoder *encoder, o_idx = 0; state = encoder->state; - for (i=0; i<n; i++) { + for (i = 0; i < n; i++) { int bit = input[i]; uint8_t out; - out = code->next_output[state][bit]; + out = code->next_output[state][bit]; state = code->next_state[state][bit]; o_idx += _conv_encode_do_output(encoder, out, &output[o_idx]); @@ -163,8 +159,7 @@ osmo_conv_encode_raw(struct osmo_conv_encoder *encoder, } int -osmo_conv_encode_flush(struct osmo_conv_encoder *encoder, - ubit_t *output) +osmo_conv_encode_flush(struct osmo_conv_encoder *encoder, ubit_t *output) { const struct osmo_conv_code *code = encoder->code; uint8_t state; @@ -177,14 +172,14 @@ osmo_conv_encode_flush(struct osmo_conv_encoder *encoder, o_idx = 0; state = encoder->state; - for (i=0; i<n; i++) { + for (i = 0; i < n; i++) { uint8_t out; if (code->next_term_output) { - out = code->next_term_output[state]; + out = code->next_term_output[state]; state = code->next_term_state[state]; } else { - out = code->next_output[state][0]; + out = code->next_output[state][0]; state = code->next_state[state][0]; } @@ -210,7 +205,7 @@ osmo_conv_encode_flush(struct osmo_conv_encoder *encoder, */ int osmo_conv_encode(const struct osmo_conv_code *code, - const ubit_t *input, ubit_t *output) + const ubit_t *input, ubit_t *output) { struct osmo_conv_encoder encoder; int l; @@ -240,17 +235,18 @@ osmo_conv_encode(const struct osmo_conv_code *code, /* Forward declaration for accerlated decoding with certain codes */ int osmo_conv_decode_acc(const struct osmo_conv_code *code, - const sbit_t *input, ubit_t *output); + const sbit_t *input, ubit_t *output); void osmo_conv_decode_init(struct osmo_conv_decoder *decoder, - const struct osmo_conv_code *code, int len, int start_state) + const struct osmo_conv_code *code, int len, + int start_state) { int n_states; /* Init */ if (len <= 0) - len = code->len; + len = code->len; n_states = 1 << (code->K - 1); @@ -261,7 +257,7 @@ osmo_conv_decode_init(struct osmo_conv_decoder *decoder, decoder->len = len; /* Allocate arrays */ - decoder->ae = malloc(sizeof(unsigned int) * n_states); + decoder->ae = malloc(sizeof(unsigned int) * n_states); decoder->ae_next = malloc(sizeof(unsigned int) * n_states); decoder->state_history = malloc(sizeof(uint8_t) * n_states * (len + decoder->code->K - 1)); @@ -285,7 +281,7 @@ osmo_conv_decode_reset(struct osmo_conv_decoder *decoder, int start_state) memset(decoder->ae, 0x00, sizeof(unsigned int) * decoder->n_states); } else { /* Fixed start state */ - for (i=0; i<decoder->n_states; i++) { + for (i = 0; i < decoder->n_states; i++) { decoder->ae[i] = (i == start_state) ? 0 : MAX_AE; } } @@ -302,12 +298,12 @@ osmo_conv_decode_rewind(struct osmo_conv_decoder *decoder) decoder->p_idx = 0; /* Initial error normalize (remove constant) */ - for (i=0; i<decoder->n_states; i++) { + for (i = 0; i < decoder->n_states; i++) { if (decoder->ae[i] < min_ae) min_ae = decoder->ae[i]; } - for (i=0; i<decoder->n_states; i++) + for (i = 0; i < decoder->n_states; i++) decoder->ae[i] -= min_ae; } @@ -323,7 +319,7 @@ osmo_conv_decode_deinit(struct osmo_conv_decoder *decoder) int osmo_conv_decode_scan(struct osmo_conv_decoder *decoder, - const sbit_t *input, int n) + const sbit_t *input, int n) { const struct osmo_conv_code *code = decoder->code; @@ -340,27 +336,25 @@ osmo_conv_decode_scan(struct osmo_conv_decoder *decoder, /* Prepare */ n_states = decoder->n_states; - ae = decoder->ae; + ae = decoder->ae; ae_next = decoder->ae_next; state_history = &decoder->state_history[n_states * decoder->o_idx]; - in_sym = alloca(sizeof(sbit_t) * code->N); + in_sym = alloca(sizeof(sbit_t) * code->N); i_idx = 0; p_idx = decoder->p_idx; /* Scan the treillis */ - for (i=0; i<n; i++) - { + for (i = 0; i < n; i++) { /* Reset next accumulated error */ - for (s=0; s<n_states; s++) { + for (s = 0; s < n_states; s++) ae_next[s] = MAX_AE; - } /* Get input */ if (code->puncture) { /* Hard way ... */ - for (j=0; j<code->N; j++) { + for (j = 0; j < code->N; j++) { int idx = ((decoder->o_idx + i) * code->N) + j; if (idx == code->puncture[p_idx]) { in_sym[j] = 0; /* Undefined */ @@ -377,23 +371,21 @@ osmo_conv_decode_scan(struct osmo_conv_decoder *decoder, } /* Scan all state */ - for (s=0; s<n_states; s++) - { + for (s = 0; s < n_states; s++) { /* Scan possible input bits */ - for (b=0; b<2; b++) - { + for (b = 0; b < 2; b++) { int nae, ov, e; uint8_t m; /* Next output and state */ - uint8_t out = code->next_output[s][b]; + uint8_t out = code->next_output[s][b]; uint8_t state = code->next_state[s][b]; /* New error for this path */ nae = ae[s]; /* start from last error */ m = 1 << (code->N - 1); /* mask for 'out' bit selection */ - for (j=0; j<code->N; j++) { + for (j = 0; j < code->N; j++) { int is = (int)in_sym[j]; if (is) { ov = (out & m) ? -127 : 127; /* sbit_t value for it */ @@ -423,8 +415,7 @@ osmo_conv_decode_scan(struct osmo_conv_decoder *decoder, } int -osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, - const sbit_t *input) +osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, const sbit_t *input) { const struct osmo_conv_code *code = decoder->code; @@ -441,27 +432,25 @@ osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, /* Prepare */ n_states = decoder->n_states; - ae = decoder->ae; + ae = decoder->ae; ae_next = decoder->ae_next; state_history = &decoder->state_history[n_states * decoder->o_idx]; - in_sym = alloca(sizeof(sbit_t) * code->N); + in_sym = alloca(sizeof(sbit_t) * code->N); i_idx = 0; p_idx = decoder->p_idx; /* Scan the treillis */ - for (i=0; i<code->K-1; i++) - { + for (i = 0; i < code->K - 1; i++) { /* Reset next accumulated error */ - for (s=0; s<n_states; s++) { + for (s = 0; s < n_states; s++) ae_next[s] = MAX_AE; - } /* Get input */ if (code->puncture) { /* Hard way ... */ - for (j=0; j<code->N; j++) { + for (j = 0; j < code->N; j++) { int idx = ((decoder->o_idx + i) * code->N) + j; if (idx == code->puncture[p_idx]) { in_sym[j] = 0; /* Undefined */ @@ -478,8 +467,7 @@ osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, } /* Scan all state */ - for (s=0; s<n_states; s++) - { + for (s = 0; s < n_states; s++) { int nae, ov, e; uint8_t m; @@ -488,10 +476,10 @@ osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, uint8_t state; if (code->next_term_output) { - out = code->next_term_output[s]; + out = code->next_term_output[s]; state = code->next_term_state[s]; } else { - out = code->next_output[s][0]; + out = code->next_output[s][0]; state = code->next_state[s][0]; } @@ -499,7 +487,7 @@ osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, nae = ae[s]; /* start from last error */ m = 1 << (code->N - 1); /* mask for 'out' bit selection */ - for (j=0; j<code->N; j++) { + for (j = 0; j < code->N; j++) { int is = (int)in_sym[j]; if (is) { ov = (out & m) ? -127 : 127; /* sbit_t value for it */ @@ -528,38 +516,86 @@ osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, } int -osmo_conv_decode_get_output(struct osmo_conv_decoder *decoder, - ubit_t *output, int has_flush, int end_state) +osmo_conv_decode_get_best_end_state(struct osmo_conv_decoder *decoder) { const struct osmo_conv_code *code = decoder->code; - int min_ae; - uint8_t min_state, cur_state; - int i, s, n; + int min_ae, min_state; + int s; - uint8_t *sh_ptr; + /* If flushed, we _know_ the end state */ + if (code->term == CONV_TERM_FLUSH) + return 0; - /* End state ? */ - if (end_state < 0) { - /* Find state with least error */ - min_ae = MAX_AE; - min_state = 0xff; + /* Search init */ + min_state = -1; + min_ae = MAX_AE; - for (s=0; s<decoder->n_states; s++) - { + /* If tail biting, we search for the minimum path metric that + * gives a circular traceback (i.e. start_state == end_state */ + if (code->term == CONV_TERM_TAIL_BITING) { + int t, n, i; + uint8_t *sh_ptr; + + for (s = 0; s < decoder->n_states; s++) { + /* Check if that state traces back to itself */ + n = decoder->o_idx; + sh_ptr = &decoder->state_history[decoder->n_states * (n-1)]; + t = s; + + for (i = n - 1; i >= 0; i--) { + t = sh_ptr[t]; + sh_ptr -= decoder->n_states; + } + + if (s != t) + continue; + + /* If it does, consider it */ if (decoder->ae[s] < min_ae) { min_ae = decoder->ae[s]; min_state = s; } } - if (min_state == 0xff) - return -1; - } else { - min_state = (uint8_t) end_state; - min_ae = decoder->ae[end_state]; + if (min_ae < MAX_AE) + return min_state; + } + + /* Finally, just the lowest path metric */ + for (s = 0; s < decoder->n_states; s++) { + /* Is it smaller ? */ + if (decoder->ae[s] < min_ae) { + min_ae = decoder->ae[s]; + min_state = s; + } } + return min_state; +} + +int +osmo_conv_decode_get_output(struct osmo_conv_decoder *decoder, + ubit_t *output, int has_flush, int end_state) +{ + const struct osmo_conv_code *code = decoder->code; + + int min_ae; + uint8_t min_state, cur_state; + int i, n; + + uint8_t *sh_ptr; + + /* End state ? */ + if (end_state < 0) + end_state = osmo_conv_decode_get_best_end_state(decoder); + + if (end_state < 0) + return -1; + + min_state = (uint8_t) end_state; + min_ae = decoder->ae[end_state]; + /* Traceback */ cur_state = min_state; @@ -569,16 +605,15 @@ osmo_conv_decode_get_output(struct osmo_conv_decoder *decoder, /* No output for the K-1 termination input bits */ if (has_flush) { - for (i=0; i<code->K-1; i++) { + for (i = 0; i < code->K - 1; i++) { cur_state = sh_ptr[cur_state]; sh_ptr -= decoder->n_states; } n -= code->K - 1; } - /* Generate output backward */ - for (i=n-1; i>=0; i--) - { + /* Generate output backward */ + for (i = n - 1; i >= 0; i--) { min_state = cur_state; cur_state = sh_ptr[cur_state]; @@ -600,12 +635,12 @@ osmo_conv_decode_get_output(struct osmo_conv_decoder *decoder, * * This is an all-in-one function, taking care of * \ref osmo_conv_decode_init, \ref osmo_conv_decode_scan, - * \ref osmo_conv_decode_flush, \ref osmo_conv_decode_get_output and - * \ref osmo_conv_decode_deinit. + * \ref osmo_conv_decode_flush, \ref osmo_conv_decode_get_best_end_state, + * \ref osmo_conv_decode_get_output and \ref osmo_conv_decode_deinit. */ int osmo_conv_decode(const struct osmo_conv_code *code, - const sbit_t *input, ubit_t *output) + const sbit_t *input, ubit_t *output) { struct osmo_conv_decoder decoder; int rv, l; @@ -628,7 +663,7 @@ osmo_conv_decode(const struct osmo_conv_code *code, rv = osmo_conv_decode_get_output(&decoder, output, code->term == CONV_TERM_FLUSH, /* has_flush */ - code->term == CONV_TERM_FLUSH ? 0 : -1 /* end_state */ + -1 /* end_state */ ); osmo_conv_decode_deinit(&decoder); diff --git a/src/conv_acc.c b/src/core/conv_acc.c index c16e4364..4bd3b076 100644 --- a/src/conv_acc.c +++ b/src/core/conv_acc.c @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdlib.h> @@ -85,6 +81,11 @@ int16_t *osmo_conv_sse_avx_vdec_malloc(size_t n); void osmo_conv_sse_avx_vdec_free(int16_t *ptr); #endif +#ifdef HAVE_NEON +int16_t *osmo_conv_neon_vdec_malloc(size_t n); +void osmo_conv_neon_vdec_free(int16_t *ptr); +#endif + /* Forward Metric Units */ void osmo_conv_gen_metrics_k5_n2(const int8_t *seq, const int16_t *out, int16_t *sums, int16_t *paths, int norm); @@ -129,6 +130,21 @@ void osmo_conv_sse_avx_metrics_k7_n4(const int8_t *seq, const int16_t *out, int16_t *sums, int16_t *paths, int norm); #endif +#if defined(HAVE_NEON) +void osmo_conv_neon_metrics_k5_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_neon_metrics_k5_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_neon_metrics_k5_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_neon_metrics_k7_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_neon_metrics_k7_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_neon_metrics_k7_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +#endif + /* Trellis State * state - Internal lshift register value * prev - Register values of previous 0 and 1 states @@ -467,10 +483,27 @@ static void _traceback_rec(struct vdecoder *dec, */ static int traceback(struct vdecoder *dec, uint8_t *out, int term, int len) { - int i, sum, max = -1; - unsigned path, state = 0; + int i, j, sum, max = -1; + unsigned path, state = 0, state_scan; - if (term != CONV_TERM_FLUSH) { + if (term == CONV_TERM_TAIL_BITING) { + for (i = 0; i < dec->trellis.num_states; i++) { + state_scan = i; + for (j = len - 1; j >= 0; j--) { + path = dec->paths[j][state_scan] + 1; + state_scan = vstate_lshift(state_scan, dec->k, path); + } + if (state_scan != i) + continue; + sum = dec->trellis.sums[i]; + if (sum > max) { + max = sum; + state = i; + } + } + } + + if ((max < 0) && (term != CONV_TERM_FLUSH)) { for (i = 0; i < dec->trellis.num_states; i++) { sum = dec->trellis.sums[i]; if (sum > max) { @@ -528,6 +561,12 @@ static int vdec_init(struct vdecoder *dec, const struct osmo_conv_code *code) if (dec->k == 5) { switch (dec->n) { case 2: +/* rach len 14 is too short for neon */ +#ifdef HAVE_NEON + if (code->len < 100) + dec->metric_func = osmo_conv_gen_metrics_k5_n2; + else +#endif dec->metric_func = osmo_conv_metrics_k5_n2; break; case 3: @@ -681,6 +720,8 @@ static void osmo_conv_init(void) } else { INIT_POINTERS(gen); } +#elif defined(HAVE_NEON) + INIT_POINTERS(neon); #else INIT_POINTERS(gen); #endif diff --git a/src/conv_acc_generic.c b/src/core/conv_acc_generic.c index 28876738..2257e6a9 100644 --- a/src/conv_acc_generic.c +++ b/src/core/conv_acc_generic.c @@ -17,10 +17,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdlib.h> diff --git a/src/core/conv_acc_neon.c b/src/core/conv_acc_neon.c new file mode 100644 index 00000000..fb180e3d --- /dev/null +++ b/src/core/conv_acc_neon.c @@ -0,0 +1,106 @@ +/*! \file conv_acc_neon.c + * Accelerated Viterbi decoder implementation + * for architectures with only NEON available. */ +/* + * (C) 2020 by sysmocom - s.f.m.c. GmbH + * Author: Eric Wild + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <stdlib.h> +#include <stdint.h> +#include <malloc.h> +#include "config.h" + +#if defined(HAVE_NEON) +#include <arm_neon.h> +#endif + +/* align req is 16 on android because google was confused, 8 on sane platforms */ +#define NEON_ALIGN 8 + +#include <conv_acc_neon_impl.h> + +/* Aligned Memory Allocator + * NEON requires 8-byte memory alignment. We store relevant trellis values + * (accumulated sums, outputs, and path decisions) as 16 bit signed integers + * so the allocated memory is casted as such. + */ +__attribute__ ((visibility("hidden"))) +int16_t *osmo_conv_neon_vdec_malloc(size_t n) +{ + return (int16_t *) memalign(NEON_ALIGN, sizeof(int16_t) * n); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_vdec_free(int16_t *ptr) +{ + free(ptr); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k5_n2(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[0], val[1] }; + + _neon_metrics_k5_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k5_n3(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[2], 0 }; + + _neon_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k5_n4(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[2], val[3] }; + + _neon_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k7_n2(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[0], val[1] }; + + _neon_metrics_k7_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k7_n3(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[2], 0 }; + + _neon_metrics_k7_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_neon_metrics_k7_n4(const int8_t *val, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + const int16_t _val[4] = { val[0], val[1], val[2], val[3] }; + + _neon_metrics_k7_n4(_val, out, sums, paths, norm); +} diff --git a/src/core/conv_acc_neon_impl.h b/src/core/conv_acc_neon_impl.h new file mode 100644 index 00000000..8a78c75b --- /dev/null +++ b/src/core/conv_acc_neon_impl.h @@ -0,0 +1,350 @@ +/*! \file conv_acc_neon_impl.h + * Accelerated Viterbi decoder implementation: + * straight port of SSE to NEON based on Tom Tsous work */ +/* + * (C) 2020 by sysmocom - s.f.m.c. GmbH + * Author: Eric Wild + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +/* Some distributions (notably Alpine Linux) for some strange reason + * don't have this #define */ +#ifndef __always_inline +#define __always_inline inline __attribute__((always_inline)) +#endif + +#define NEON_BUTTERFLY(M0,M1,M2,M3,M4) \ +{ \ + M3 = vqaddq_s16(M0, M2); \ + M4 = vqsubq_s16(M1, M2); \ + M0 = vqsubq_s16(M0, M2); \ + M1 = vqaddq_s16(M1, M2); \ + M2 = vmaxq_s16(M3, M4); \ + M3 = vreinterpretq_s16_u16(vcgtq_s16(M3, M4)); \ + M4 = vmaxq_s16(M0, M1); \ + M1 = vreinterpretq_s16_u16(vcgtq_s16(M0, M1)); \ +} + +#define NEON_DEINTERLEAVE_K5(M0,M1,M2,M3) \ +{ \ + int16x8x2_t tmp; \ + tmp = vuzpq_s16(M0, M1); \ + M2 = tmp.val[0]; \ + M3 = tmp.val[1]; \ +} + +#define NEON_DEINTERLEAVE_K7(M0,M1,M2,M3,M4,M5,M6,M7,M8,M9,M10,M11,M12,M13,M14,M15) \ +{ \ + int16x8x2_t tmp; \ + tmp = vuzpq_s16(M0, M1); \ + M8 = tmp.val[0]; M9 = tmp.val[1]; \ + tmp = vuzpq_s16(M2, M3); \ + M10 = tmp.val[0]; M11 = tmp.val[1]; \ + tmp = vuzpq_s16(M4, M5); \ + M12 = tmp.val[0]; M13 = tmp.val[1]; \ + tmp = vuzpq_s16(M6, M7); \ + M14 = tmp.val[0]; M15 = tmp.val[1]; \ +} + +#define NEON_BRANCH_METRIC_N2(M0,M1,M2,M3,M4,M6,M7) \ +{ \ + M0 = vmulq_s16(M4, M0); \ + M1 = vmulq_s16(M4, M1); \ + M2 = vmulq_s16(M4, M2); \ + M3 = vmulq_s16(M4, M3); \ + M6 = vcombine_s16(vpadd_s16(vget_low_s16(M0), vget_high_s16(M0)), vpadd_s16(vget_low_s16(M1), vget_high_s16(M1))); \ + M7 = vcombine_s16(vpadd_s16(vget_low_s16(M2), vget_high_s16(M2)), vpadd_s16(vget_low_s16(M3), vget_high_s16(M3))); \ +} + +#define NEON_BRANCH_METRIC_N4(M0,M1,M2,M3,M4,M5) \ +{ \ + M0 = vmulq_s16(M4, M0); \ + M1 = vmulq_s16(M4, M1); \ + M2 = vmulq_s16(M4, M2); \ + M3 = vmulq_s16(M4, M3); \ + int16x4_t t1 = vpadd_s16(vpadd_s16(vget_low_s16(M0), vget_high_s16(M0)), vpadd_s16(vget_low_s16(M1), vget_high_s16(M1))); \ + int16x4_t t2 = vpadd_s16(vpadd_s16(vget_low_s16(M2), vget_high_s16(M2)), vpadd_s16(vget_low_s16(M3), vget_high_s16(M3))); \ + M5 = vcombine_s16(t1, t2); \ +} + +#define NEON_NORMALIZE_K5(M0,M1,M2,M3) \ +{ \ + M2 = vminq_s16(M0, M1); \ + int16x4_t t = vpmin_s16(vget_low_s16(M2), vget_high_s16(M2)); \ + t = vpmin_s16(t, t); \ + t = vpmin_s16(t, t); \ + M2 = vdupq_lane_s16(t, 0); \ + M0 = vqsubq_s16(M0, M2); \ + M1 = vqsubq_s16(M1, M2); \ +} + +#define NEON_NORMALIZE_K7(M0,M1,M2,M3,M4,M5,M6,M7,M8,M9,M10,M11) \ +{ \ + M8 = vminq_s16(M0, M1); \ + M9 = vminq_s16(M2, M3); \ + M10 = vminq_s16(M4, M5); \ + M11 = vminq_s16(M6, M7); \ + M8 = vminq_s16(M8, M9); \ + M10 = vminq_s16(M10, M11); \ + M8 = vminq_s16(M8, M10); \ + int16x4_t t = vpmin_s16(vget_low_s16(M8), vget_high_s16(M8)); \ + t = vpmin_s16(t, t); \ + t = vpmin_s16(t, t); \ + M8 = vdupq_lane_s16(t, 0); \ + M0 = vqsubq_s16(M0, M8); \ + M1 = vqsubq_s16(M1, M8); \ + M2 = vqsubq_s16(M2, M8); \ + M3 = vqsubq_s16(M3, M8); \ + M4 = vqsubq_s16(M4, M8); \ + M5 = vqsubq_s16(M5, M8); \ + M6 = vqsubq_s16(M6, M8); \ + M7 = vqsubq_s16(M7, M8); \ +} + +__always_inline void _neon_metrics_k5_n2(const int16_t *val, const int16_t *outa, int16_t *sumsa, int16_t *paths, + int norm) +{ + int16_t *__restrict out = __builtin_assume_aligned(outa, 8); + int16_t *__restrict sums = __builtin_assume_aligned(sumsa, 8); + int16x8_t m0, m1, m2, m3, m4, m5, m6; + int16x4_t input; + + /* (BMU) Load and expand 8-bit input out to 16-bits */ + input = vld1_s16(val); + m2 = vcombine_s16(input, input); + + /* (BMU) Load and compute branch metrics */ + m0 = vld1q_s16(&out[0]); + m1 = vld1q_s16(&out[8]); + + m0 = vmulq_s16(m2, m0); + m1 = vmulq_s16(m2, m1); + m2 = vcombine_s16(vpadd_s16(vget_low_s16(m0), vget_high_s16(m0)), + vpadd_s16(vget_low_s16(m1), vget_high_s16(m1))); + + /* (PMU) Load accumulated path matrics */ + m0 = vld1q_s16(&sums[0]); + m1 = vld1q_s16(&sums[8]); + + NEON_DEINTERLEAVE_K5(m0, m1, m3, m4) + + /* (PMU) Butterflies: 0-7 */ + NEON_BUTTERFLY(m3, m4, m2, m5, m6) + + if (norm) + NEON_NORMALIZE_K5(m2, m6, m0, m1) + + vst1q_s16(&sums[0], m2); + vst1q_s16(&sums[8], m6); + vst1q_s16(&paths[0], m5); + vst1q_s16(&paths[8], m4); +} + +__always_inline void _neon_metrics_k5_n4(const int16_t *val, const int16_t *outa, int16_t *sumsa, int16_t *paths, + int norm) +{ + int16_t *__restrict out = __builtin_assume_aligned(outa, 8); + int16_t *__restrict sums = __builtin_assume_aligned(sumsa, 8); + int16x8_t m0, m1, m2, m3, m4, m5, m6; + int16x4_t input; + + /* (BMU) Load and expand 8-bit input out to 16-bits */ + input = vld1_s16(val); + m4 = vcombine_s16(input, input); + + /* (BMU) Load and compute branch metrics */ + m0 = vld1q_s16(&out[0]); + m1 = vld1q_s16(&out[8]); + m2 = vld1q_s16(&out[16]); + m3 = vld1q_s16(&out[24]); + + NEON_BRANCH_METRIC_N4(m0, m1, m2, m3, m4, m2) + + /* (PMU) Load accumulated path matrics */ + m0 = vld1q_s16(&sums[0]); + m1 = vld1q_s16(&sums[8]); + + NEON_DEINTERLEAVE_K5(m0, m1, m3, m4) + + /* (PMU) Butterflies: 0-7 */ + NEON_BUTTERFLY(m3, m4, m2, m5, m6) + + if (norm) + NEON_NORMALIZE_K5(m2, m6, m0, m1) + + vst1q_s16(&sums[0], m2); + vst1q_s16(&sums[8], m6); + vst1q_s16(&paths[0], m5); + vst1q_s16(&paths[8], m4); +} + +__always_inline static void _neon_metrics_k7_n2(const int16_t *val, const int16_t *outa, int16_t *sumsa, int16_t *paths, + int norm) +{ + int16_t *__restrict out = __builtin_assume_aligned(outa, 8); + int16_t *__restrict sums = __builtin_assume_aligned(sumsa, 8); + int16x8_t m0, m1, m2, m3, m4, m5, m6, m7; + int16x8_t m8, m9, m10, m11, m12, m13, m14, m15; + int16x4_t input; + + /* (PMU) Load accumulated path matrics */ + m0 = vld1q_s16(&sums[0]); + m1 = vld1q_s16(&sums[8]); + m2 = vld1q_s16(&sums[16]); + m3 = vld1q_s16(&sums[24]); + m4 = vld1q_s16(&sums[32]); + m5 = vld1q_s16(&sums[40]); + m6 = vld1q_s16(&sums[48]); + m7 = vld1q_s16(&sums[56]); + + /* (PMU) Deinterleave into even and odd packed registers */ + NEON_DEINTERLEAVE_K7(m0, m1, m2, m3, m4, m5, m6, m7, m8, m9, m10, m11, m12, m13, m14, m15) + + /* (BMU) Load and expand 8-bit input out to 16-bits */ + input = vld1_s16(val); + m7 = vcombine_s16(input, input); + + /* (BMU) Load and compute branch metrics */ + m0 = vld1q_s16(&out[0]); + m1 = vld1q_s16(&out[8]); + m2 = vld1q_s16(&out[16]); + m3 = vld1q_s16(&out[24]); + + NEON_BRANCH_METRIC_N2(m0, m1, m2, m3, m7, m4, m5) + + m0 = vld1q_s16(&out[32]); + m1 = vld1q_s16(&out[40]); + m2 = vld1q_s16(&out[48]); + m3 = vld1q_s16(&out[56]); + + NEON_BRANCH_METRIC_N2(m0, m1, m2, m3, m7, m6, m7) + + /* (PMU) Butterflies: 0-15 */ + NEON_BUTTERFLY(m8, m9, m4, m0, m1) + NEON_BUTTERFLY(m10, m11, m5, m2, m3) + + vst1q_s16(&paths[0], m0); + vst1q_s16(&paths[8], m2); + vst1q_s16(&paths[32], m9); + vst1q_s16(&paths[40], m11); + + /* (PMU) Butterflies: 17-31 */ + NEON_BUTTERFLY(m12, m13, m6, m0, m2) + NEON_BUTTERFLY(m14, m15, m7, m9, m11) + + vst1q_s16(&paths[16], m0); + vst1q_s16(&paths[24], m9); + vst1q_s16(&paths[48], m13); + vst1q_s16(&paths[56], m15); + + if (norm) + NEON_NORMALIZE_K7(m4, m1, m5, m3, m6, m2, m7, m11, m0, m8, m9, m10) + + vst1q_s16(&sums[0], m4); + vst1q_s16(&sums[8], m5); + vst1q_s16(&sums[16], m6); + vst1q_s16(&sums[24], m7); + vst1q_s16(&sums[32], m1); + vst1q_s16(&sums[40], m3); + vst1q_s16(&sums[48], m2); + vst1q_s16(&sums[56], m11); +} + +__always_inline static void _neon_metrics_k7_n4(const int16_t *val, const int16_t *outa, int16_t *sumsa, int16_t *paths, + int norm) +{ + int16_t *__restrict out = __builtin_assume_aligned(outa, 8); + int16_t *__restrict sums = __builtin_assume_aligned(sumsa, 8); + int16x8_t m0, m1, m2, m3, m4, m5, m6, m7; + int16x8_t m8, m9, m10, m11, m12, m13, m14, m15; + int16x4_t input; + + /* (PMU) Load accumulated path matrics */ + m0 = vld1q_s16(&sums[0]); + m1 = vld1q_s16(&sums[8]); + m2 = vld1q_s16(&sums[16]); + m3 = vld1q_s16(&sums[24]); + m4 = vld1q_s16(&sums[32]); + m5 = vld1q_s16(&sums[40]); + m6 = vld1q_s16(&sums[48]); + m7 = vld1q_s16(&sums[56]); + + /* (PMU) Deinterleave into even and odd packed registers */ + NEON_DEINTERLEAVE_K7(m0, m1, m2, m3, m4, m5, m6, m7, m8, m9, m10, m11, m12, m13, m14, m15) + + /* (BMU) Load and expand 8-bit input out to 16-bits */ + input = vld1_s16(val); + m7 = vcombine_s16(input, input); + + /* (BMU) Load and compute branch metrics */ + m0 = vld1q_s16(&out[0]); + m1 = vld1q_s16(&out[8]); + m2 = vld1q_s16(&out[16]); + m3 = vld1q_s16(&out[24]); + + NEON_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m4) + + m0 = vld1q_s16(&out[32]); + m1 = vld1q_s16(&out[40]); + m2 = vld1q_s16(&out[48]); + m3 = vld1q_s16(&out[56]); + + NEON_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m5) + + m0 = vld1q_s16(&out[64]); + m1 = vld1q_s16(&out[72]); + m2 = vld1q_s16(&out[80]); + m3 = vld1q_s16(&out[88]); + + NEON_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m6) + + m0 = vld1q_s16(&out[96]); + m1 = vld1q_s16(&out[104]); + m2 = vld1q_s16(&out[112]); + m3 = vld1q_s16(&out[120]); + + NEON_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m7) + + /* (PMU) Butterflies: 0-15 */ + NEON_BUTTERFLY(m8, m9, m4, m0, m1) + NEON_BUTTERFLY(m10, m11, m5, m2, m3) + + vst1q_s16(&paths[0], m0); + vst1q_s16(&paths[8], m2); + vst1q_s16(&paths[32], m9); + vst1q_s16(&paths[40], m11); + + /* (PMU) Butterflies: 17-31 */ + NEON_BUTTERFLY(m12, m13, m6, m0, m2) + NEON_BUTTERFLY(m14, m15, m7, m9, m11) + + vst1q_s16(&paths[16], m0); + vst1q_s16(&paths[24], m9); + vst1q_s16(&paths[48], m13); + vst1q_s16(&paths[56], m15); + + if (norm) + NEON_NORMALIZE_K7(m4, m1, m5, m3, m6, m2, m7, m11, m0, m8, m9, m10) + + vst1q_s16(&sums[0], m4); + vst1q_s16(&sums[8], m5); + vst1q_s16(&sums[16], m6); + vst1q_s16(&sums[24], m7); + vst1q_s16(&sums[32], m1); + vst1q_s16(&sums[40], m3); + vst1q_s16(&sums[48], m2); + vst1q_s16(&sums[56], m11); +} diff --git a/src/conv_acc_sse.c b/src/core/conv_acc_sse.c index 63d8722a..513ab052 100644 --- a/src/conv_acc_sse.c +++ b/src/core/conv_acc_sse.c @@ -17,10 +17,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> diff --git a/src/conv_acc_sse_avx.c b/src/core/conv_acc_sse_avx.c index 5ac3c163..82b4fa62 100644 --- a/src/conv_acc_sse_avx.c +++ b/src/core/conv_acc_sse_avx.c @@ -17,10 +17,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <stdint.h> diff --git a/src/conv_acc_sse_impl.h b/src/core/conv_acc_sse_impl.h index 9ebbfe9c..807dbe5e 100644 --- a/src/conv_acc_sse_impl.h +++ b/src/core/conv_acc_sse_impl.h @@ -18,10 +18,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /* Some distributions (notably Alpine Linux) for some strange reason diff --git a/src/counter.c b/src/core/counter.c index 0fa31661..dace15f3 100644 --- a/src/counter.c +++ b/src/core/counter.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <string.h> @@ -79,7 +75,7 @@ int osmo_counters_for_each(int (*handle_counter)(struct osmo_counter *, void *), /*! Counts the registered counter * \returns amount of counters */ -int osmo_counters_count() +int osmo_counters_count(void) { return llist_count(&counters); } diff --git a/src/crc16.c b/src/core/crc16.c index 29dace2e..29dace2e 100644 --- a/src/crc16.c +++ b/src/core/crc16.c diff --git a/src/crcXXgen.c.tpl b/src/core/crcXXgen.c.tpl index 74e6d521..154291cc 100644 --- a/src/crcXXgen.c.tpl +++ b/src/core/crcXXgen.c.tpl @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /*! \addtogroup crc diff --git a/src/core/exec.c b/src/core/exec.c new file mode 100644 index 00000000..2e33788e --- /dev/null +++ b/src/core/exec.c @@ -0,0 +1,301 @@ +/* (C) 2019 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include "config.h" +#ifndef EMBEDDED + +#define _GNU_SOURCE +#include <unistd.h> + +#include <errno.h> +#include <string.h> + +#include <stdio.h> +#include <dirent.h> +#include <sys/types.h> +#include <pwd.h> + +#include <osmocom/core/logging.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/exec.h> + +/*! suggested list of environment variables to pass (if they exist) to a sub-process/script */ +const char *osmo_environment_whitelist[] = { + "USER", "LOGNAME", "HOME", + "LANG", "LC_ALL", "LC_COLLATE", "LC_CTYPE", "LC_MESSAGES", "LC_MONETARY", "LC_NUMERIC", "LC_TIME", + "PATH", + "PWD", + "SHELL", + "TERM", + "TMPDIR", + "LD_LIBRARY_PATH", + "LD_PRELOAD", + "POSIXLY_CORRECT", + "HOSTALIASES", + "TZ", "TZDIR", + "TERMCAP", + "COLUMNS", "LINES", + NULL +}; + +static bool str_in_list(const char **list, const char *key) +{ + const char **ent; + + for (ent = list; *ent; ent++) { + if (!strcmp(*ent, key)) + return true; + } + return false; +} + +/*! filtered a process environment by whitelist; only copying pointers, no actual strings. + * + * This function is useful if you'd like to generate an environment to pass exec*e() + * functions. It will create a new environment containing only those entries whose + * keys (as per environment convention KEY=VALUE) are contained in the whitelist. The + * function will not copy the actual strings, but just create a new pointer array, pointing + * to the same memory as the input strings. + * + * Constraints: Keys up to a maximum length of 255 characters are supported. + * + * \param[out] out caller-allocated array of pointers for the generated output + * \param[in] out_len size of out (number of pointers) + * \param[in] in input environment (NULL-terminated list of pointers like **environ) + * \param[in] whitelist whitelist of permitted keys in environment (like **environ) + * \returns number of entries filled in 'out'; negtive on error */ +int osmo_environment_filter(char **out, size_t out_len, char **in, const char **whitelist) +{ + char tmp[256]; + char **ent; + size_t out_used = 0; + + /* invalid calls */ + if (!out || out_len == 0 || !whitelist) + return -EINVAL; + + /* legal, but unusual: no input to filter should generate empty, terminated out */ + if (!in) { + out[0] = NULL; + return 1; + } + + /* iterate over input entries */ + for (ent = in; *ent; ent++) { + char *eq = strchr(*ent, '='); + unsigned long eq_pos; + if (!eq) { + /* no '=' in string, skip it */ + continue; + } + eq_pos = eq - *ent; + if (eq_pos >= ARRAY_SIZE(tmp)) + continue; + strncpy(tmp, *ent, eq_pos); + tmp[eq_pos] = '\0'; + if (str_in_list(whitelist, tmp)) { + if (out_used == out_len-1) + break; + /* append to output */ + out[out_used++] = *ent; + } + } + OSMO_ASSERT(out_used < out_len); + out[out_used++] = NULL; + return out_used; +} + +/*! append one environment to another; only copying pointers, not actual strings. + * + * This function is useful if you'd like to append soem entries to an environment + * befoer passing it to exec*e() functions. + * + * It will append all entries from 'in' to the environment in 'out', as long as + * 'out' has space (determined by 'out_len'). + * + * Constraints: If the same key exists in 'out' and 'in', duplicate keys are + * generated. It is a simple append, without any duplicate checks. + * + * \param[out] out caller-allocated array of pointers for the generated output + * \param[in] out_len size of out (number of pointers) + * \param[in] in input environment (NULL-terminated list of pointers like **environ) + * \returns number of entries filled in 'out'; negative on error */ +int osmo_environment_append(char **out, size_t out_len, char **in) +{ + size_t out_used = 0; + + if (!out || out_len == 0) + return -EINVAL; + + /* seek to end of existing output */ + for (out_used = 0; out[out_used]; out_used++) {} + + if (!in) { + if (out_used == 0) + out[out_used++] = NULL; + return out_used; + } + + for (; *in && out_used < out_len-1; in++) + out[out_used++] = *in; + + OSMO_ASSERT(out_used < out_len); + out[out_used++] = NULL; + + return out_used; +} + +/* Iterate over files in /proc/self/fd and close all above lst_fd_to_keep */ +int osmo_close_all_fds_above(int last_fd_to_keep) +{ + struct dirent *ent; + DIR *dir; + int rc; + + dir = opendir("/proc/self/fd"); + if (!dir) { + LOGP(DLGLOBAL, LOGL_ERROR, "Cannot open /proc/self/fd: %s\n", strerror(errno)); + return -ENODEV; + } + + while ((ent = readdir(dir))) { + int fd = atoi(ent->d_name); + if (fd <= last_fd_to_keep) + continue; + if (fd == dirfd(dir)) + continue; + rc = close(fd); + if (rc) + LOGP(DLGLOBAL, LOGL_ERROR, "Error closing fd=%d: %s\n", fd, strerror(errno)); + } + closedir(dir); + return 0; +} + +/* Seems like POSIX has no header file for this, and even glibc + __USE_GNU doesn't help */ +extern char **environ; + +/*! call an external shell command as 'user' without waiting for it. + * + * This mimics the behavior of system(3), with the following differences: + * - it doesn't wait for completion of the child process + * - it closes all non-stdio file descriptors by iterating /proc/self/fd + * - it constructs a reduced environment where only whitelisted keys survive + * - it (optionally) appends additional variables to the environment + * - it (optionally) changes the user ID to that of 'user' (requires execution as root) + * + * \param[in] command the shell command to be executed, see system(3) + * \param[in] env_whitelist A white-list of keys for environment variables + * \param[in] addl_env any additional environment variables to be appended + * \param[in] user name of the user to which we should switch before executing the command + * \returns PID of generated child process; negative on error + */ +int osmo_system_nowait2(const char *command, const char **env_whitelist, char **addl_env, const char *user) +{ + struct passwd _pw; + struct passwd *pw = NULL; + int getpw_buflen = sysconf(_SC_GETPW_R_SIZE_MAX); + int rc; + + if (user) { + if (getpw_buflen == -1) /* Value was indeterminate */ + getpw_buflen = 16384; /* Should be more than enough */ + char buf[getpw_buflen]; + rc = getpwnam_r(user, &_pw, buf, sizeof(buf), &pw); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "getpwnam_r(\"%s\") failed: %s\n", user, strerror(-rc)); + return rc; + } + if (!pw) { + LOGP(DLGLOBAL, LOGL_ERROR, "getpwnam_r(\"%s\"): user not found!\n", user); + return -EINVAL; + } + } + + rc = fork(); + if (rc == 0) { + /* we are in the child */ + char *new_env[1024]; + + /* close all file descriptors above stdio */ + osmo_close_all_fds_above(2); + + /* man execle: "an array of pointers *must* be terminated by a null pointer" */ + new_env[0] = NULL; + + /* build the new environment */ + if (env_whitelist) { + rc = osmo_environment_filter(new_env, ARRAY_SIZE(new_env), environ, env_whitelist); + if (rc < 0) + return rc; + } + if (addl_env) { + rc = osmo_environment_append(new_env, ARRAY_SIZE(new_env), addl_env); + if (rc < 0) + return rc; + } + + /* drop privileges */ + if (pw) { + if (setresgid(pw->pw_gid, pw->pw_gid, pw->pw_gid) < 0) { + perror("setresgid() during privilege drop"); + exit(1); + } + + if (setresuid(pw->pw_uid, pw->pw_uid, pw->pw_uid) < 0) { + perror("setresuid() during privilege drop"); + exit(1); + } + + } + + /* if we want to behave like system(3), we must go via the shell */ + execle("/bin/sh", "sh", "-c", command, (char *) NULL, new_env); + /* only reached in case of error */ + LOGP(DLGLOBAL, LOGL_ERROR, "Error executing command '%s' after fork: %s\n", + command, strerror(errno)); + return -EIO; + } else { + /* we are in the parent */ + if (rc == -1) + LOGP(DLGLOBAL, LOGL_ERROR, "fork() error executing command '%s': %s\n", + command, strerror(errno)); + return rc; + } +} + +/*! call an external shell command without waiting for it. + * + * This mimics the behavior of system(3), with the following differences: + * - it doesn't wait for completion of the child process + * - it closes all non-stdio file descriptors by iterating /proc/self/fd + * - it constructs a reduced environment where only whitelisted keys survive + * - it (optionally) appends additional variables to the environment + * + * \param[in] command the shell command to be executed, see system(3) + * \param[in] env_whitelist A white-list of keys for environment variables + * \param[in] addl_env any additional environment variables to be appended + * \returns PID of generated child process; negative on error + */ +int osmo_system_nowait(const char *command, const char **env_whitelist, char **addl_env) +{ + return osmo_system_nowait2(command, env_whitelist, addl_env, NULL); +} + + +#endif /* EMBEDDED */ diff --git a/src/fsm.c b/src/core/fsm.c index 1e8909ec..9333cac5 100644 --- a/src/fsm.c +++ b/src/core/fsm.c @@ -14,11 +14,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301, USA. */ #include <errno.h> @@ -450,8 +445,8 @@ struct osmo_fsm_inst *osmo_fsm_inst_alloc(struct osmo_fsm *fsm, void *ctx, void osmo_timer_setup(&fi->timer, fsm_tmr_cb, fi); if (osmo_fsm_inst_update_id(fi, id) < 0) { - fsm_free_or_steal(fi); - return NULL; + fsm_free_or_steal(fi); + return NULL; } INIT_LLIST_HEAD(&fi->proc.children); @@ -581,7 +576,7 @@ void osmo_fsm_inst_free(struct osmo_fsm_inst *fi) * \param[in] event Event integer value * \returns string rendering of the event */ -const char *osmo_fsm_event_name(struct osmo_fsm *fsm, uint32_t event) +const char *osmo_fsm_event_name(const struct osmo_fsm *fsm, uint32_t event) { static __thread char buf[32]; if (!fsm->event_names) { @@ -595,7 +590,7 @@ const char *osmo_fsm_event_name(struct osmo_fsm *fsm, uint32_t event) * \param[in] fi FSM instance * \returns string rendering of the FSM identity */ -const char *osmo_fsm_inst_name(struct osmo_fsm_inst *fi) +const char *osmo_fsm_inst_name(const struct osmo_fsm_inst *fi) { if (!fi) return "NULL"; @@ -611,7 +606,7 @@ const char *osmo_fsm_inst_name(struct osmo_fsm_inst *fi) * \param[in] state FSM state number * \returns string rendering of the FSM state */ -const char *osmo_fsm_state_name(struct osmo_fsm *fsm, uint32_t state) +const char *osmo_fsm_state_name(const struct osmo_fsm *fsm, uint32_t state) { static __thread char buf[32]; if (state >= fsm->num_states) { @@ -690,8 +685,11 @@ static int state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, if (!keep_timer || (keep_timer && !osmo_timer_pending(&fi->timer))) { fi->T = T; - if (timeout_ms) - osmo_timer_schedule(&fi->timer, timeout_ms / 1000, timeout_ms % 1000); + if (timeout_ms) { + osmo_timer_schedule(&fi->timer, + /* seconds */ (timeout_ms / 1000), + /* microseconds */ (timeout_ms % 1000) * 1000); + } } /* Call 'onenter' last, user might terminate FSM from there */ @@ -1016,6 +1014,26 @@ void _osmo_fsm_inst_term_children(struct osmo_fsm_inst *fi, } } +/*! Broadcast an event to all the FSMs children. + * + * Iterate over all children and send them the specified event. + * + * \param[in] fi FSM instance of the parent + * \param[in] event Event to send to children of FSM instance + * \param[in] data Data to pass along with the event + * \param[in] file Calling source file (from osmo_fsm_inst_dispatch macro) + * \param[in] line Calling source line (from osmo_fsm_inst_dispatch macro) + */ +void _osmo_fsm_inst_broadcast_children(struct osmo_fsm_inst *fi, + uint32_t event, void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *child, *tmp; + llist_for_each_entry_safe(child, tmp, &fi->proc.children, proc.child) { + _osmo_fsm_inst_dispatch(child, event, data, file, line); + } +} + const struct value_string osmo_fsm_term_cause_names[] = { OSMO_VALUE_STRING(OSMO_FSM_TERM_PARENT), OSMO_VALUE_STRING(OSMO_FSM_TERM_REQUEST), diff --git a/src/gsmtap_util.c b/src/core/gsmtap_util.c index 2fb18a48..b64c7b05 100644 --- a/src/gsmtap_util.c +++ b/src/core/gsmtap_util.c @@ -17,13 +17,9 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ -#include "../config.h" +#include "config.h" #include <osmocom/core/gsmtap_util.h> #include <osmocom/core/logging.h> @@ -33,6 +29,7 @@ #include <osmocom/core/select.h> #include <osmocom/core/socket.h> #include <osmocom/core/byteswap.h> +#include <osmocom/core/utils.h> #include <osmocom/gsm/protocol/gsm_04_08.h> #include <osmocom/gsm/rsl.h> @@ -50,22 +47,68 @@ * * \file gsmtap_util.c */ +/*! one gsmtap instance + * Until gsmtap_inst_fd() is removed from the API at some point in the future, we have to keep the first member as + * 'int' and the second as 'struct osmo_wqueue' (this effectively makes sure that the struct member wq.bfd.fd maintains + * the same memory offset from the start of the struct) to ensure that inlined static 'instances' of gsmtap_inst_fd() in + * old binaries keep working the way they used to even with gsmtap_inst objects obtained from newer versions of libosmocore */ +struct gsmtap_inst { + int osmo_io_mode; /*!< Indicates whether or not to use Osmo IO mode for message output (thus enabling use of tx queues). + * This field member may not be changed or moved (backwards compatibility) */ + struct osmo_wqueue wq; /*!< the wait queue. This field member may not be changed or moved (backwards compatibility) */ + + struct osmo_io_fd *out; /*!< Used when osmo_io_mode is nonzero */ + int sink_fd; +}; + +struct _gsmtap_inst_legacy { + int ofd_wq_mode; + struct osmo_wqueue wq; + struct osmo_fd sink_ofd; +}; +osmo_static_assert(offsetof(struct gsmtap_inst, wq) == offsetof(struct _gsmtap_inst_legacy, wq), + gsmtap_inst_new_wq_offset_equals_legacy_wq_offset); + +/*! Deprecated, use gsmtap_inst_fd2() instead + * \param[in] gti GSMTAP instance + * \returns file descriptor of GSMTAP instance */ +int gsmtap_inst_fd(struct gsmtap_inst *gti) +{ + return gsmtap_inst_fd2(gti); +} + +/*! obtain the file descriptor associated with a gsmtap instance + * \param[in] gti GSMTAP instance + * \returns file descriptor of GSMTAP instance */ +int gsmtap_inst_fd2(const struct gsmtap_inst *gti) +{ + return gti->wq.bfd.fd; +} /*! convert RSL channel number to GSMTAP channel type * \param[in] rsl_chantype RSL channel type * \param[in] link_id RSL link identifier + * \param[in] user_plane Is this voice/csd user plane (1) or signaling (0) * \returns GSMTAP channel type */ -uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id) +uint8_t chantype_rsl2gsmtap2(uint8_t rsl_chantype, uint8_t link_id, bool user_plane) { uint8_t ret = GSMTAP_CHANNEL_UNKNOWN; switch (rsl_chantype) { case RSL_CHAN_Bm_ACCHs: - ret = GSMTAP_CHANNEL_TCH_F; + case RSL_CHAN_OSMO_VAMOS_Bm_ACCHs: + if (user_plane) + ret = GSMTAP_CHANNEL_VOICE_F; + else + ret = GSMTAP_CHANNEL_FACCH_F; break; case RSL_CHAN_Lm_ACCHs: - ret = GSMTAP_CHANNEL_TCH_H; + case RSL_CHAN_OSMO_VAMOS_Lm_ACCHs: + if (user_plane) + ret = GSMTAP_CHANNEL_VOICE_H; + else + ret = GSMTAP_CHANNEL_FACCH_H; break; case RSL_CHAN_SDCCH4_ACCH: ret = GSMTAP_CHANNEL_SDCCH4; @@ -86,6 +129,12 @@ uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id) case RSL_CHAN_OSMO_PDCH: ret = GSMTAP_CHANNEL_PDCH; break; + case RSL_CHAN_OSMO_CBCH4: + ret = GSMTAP_CHANNEL_CBCH51; + break; + case RSL_CHAN_OSMO_CBCH8: + ret = GSMTAP_CHANNEL_CBCH52; + break; } if (link_id & 0x40) @@ -94,6 +143,16 @@ uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id) return ret; } +/*! convert RSL channel number to GSMTAP channel type + * \param[in] rsl_chantype RSL channel type + * \param[in] link_id RSL link identifier + * \returns GSMTAP channel type + */ +uint8_t chantype_rsl2gsmtap(uint8_t rsl_chantype, uint8_t link_id) +{ + return chantype_rsl2gsmtap2(rsl_chantype, link_id, false); +} + /*! convert GSMTAP channel type to RSL channel number + Link ID * \param[in] gsmtap_chantype GSMTAP channel type * \param[out] rsl_chantype RSL channel mumber @@ -103,10 +162,12 @@ void chantype_gsmtap2rsl(uint8_t gsmtap_chantype, uint8_t *rsl_chantype, uint8_t *link_id) { switch (gsmtap_chantype & ~GSMTAP_CHANNEL_ACCH & 0xff) { - case GSMTAP_CHANNEL_TCH_F: // TCH/F, FACCH/F + case GSMTAP_CHANNEL_FACCH_F: + case GSMTAP_CHANNEL_VOICE_F: // TCH/F *rsl_chantype = RSL_CHAN_Bm_ACCHs; break; - case GSMTAP_CHANNEL_TCH_H: // TCH/H, FACCH/H + case GSMTAP_CHANNEL_FACCH_H: + case GSMTAP_CHANNEL_VOICE_H: // TCH/H *rsl_chantype = RSL_CHAN_Lm_ACCHs; break; case GSMTAP_CHANNEL_SDCCH4: // SDCCH/4 @@ -151,7 +212,7 @@ void chantype_gsmtap2rsl(uint8_t gsmtap_chantype, uint8_t *rsl_chantype, */ struct msgb *gsmtap_makemsg_ex(uint8_t type, uint16_t arfcn, uint8_t ts, uint8_t chan_type, uint8_t ss, uint32_t fn, int8_t signal_dbm, - uint8_t snr, const uint8_t *data, unsigned int len) + int8_t snr, const uint8_t *data, unsigned int len) { struct msgb *msg; struct gsmtap_hdr *gh; @@ -198,7 +259,7 @@ struct msgb *gsmtap_makemsg_ex(uint8_t type, uint16_t arfcn, uint8_t ts, uint8_t */ struct msgb *gsmtap_makemsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type, uint8_t ss, uint32_t fn, int8_t signal_dbm, - uint8_t snr, const uint8_t *data, unsigned int len) + int8_t snr, const uint8_t *data, unsigned int len) { return gsmtap_makemsg_ex(GSMTAP_TYPE_UM, arfcn, ts, chan_type, ss, fn, signal_dbm, snr, data, len); @@ -208,13 +269,14 @@ struct msgb *gsmtap_makemsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type, #include <sys/socket.h> #include <netinet/in.h> +#include <osmocom/core/osmo_io.h> -/*! Create a new (sending) GSMTAP source socket +/*! Create a new (sending) GSMTAP source socket * \param[in] host host name or IP address in string format * \param[in] port UDP port number in host byte order * \return file descriptor of the new socket * - * Opens a GSMTAP source (sending) socket, conncet it to host/port and + * Opens a GSMTAP source (sending) socket, connect it to host/port and * return resulting fd. If \a host is NULL, the destination address * will be localhost. If \a port is 0, the default \ref * GSMTAP_UDP_PORT will be used. @@ -230,6 +292,30 @@ int gsmtap_source_init_fd(const char *host, uint16_t port) OSMO_SOCK_F_CONNECT); } +/*! Create a new (sending) GSMTAP source socket + * \param[in] local_host local host name or IP address in string format + * \param[in] local_port local UDP port number in host byte order + * \param[in] rem_host remote host name or IP address in string format + * \param[in] rem_port remote UDP port number in host byte order + * \return file descriptor of the new socket + * + * Opens a GSMTAP source (sending) socket, connect it to remote host/port, + * bind to local host/port and return resulting fd. + * If \a local_host is NULL, the default address is used. + * If \a local_port is 0, than random unused port will be selected by OS. + * If \a rem_host is NULL, the destination address will be localhost. + * If \a rem_port is 0, the default \ref GSMTAP_UDP_PORT will be used. + */ +int gsmtap_source_init_fd2(const char *local_host, uint16_t local_port, const char *rem_host, uint16_t rem_port) +{ + if (!local_host) + return gsmtap_source_init_fd(rem_host, rem_port); + + return osmo_sock_init2(AF_UNSPEC, SOCK_DGRAM, IPPROTO_UDP, local_host, local_port, + rem_host ? rem_host : "localhost", rem_port ? rem_port : GSMTAP_UDP_PORT, + OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT); +} + /*! Add a local sink to an existing GSMTAP source and return fd * \param[in] gsmtap_fd file descriptor of the gsmtap socket * \returns file descriptor of locally bound receive socket @@ -277,13 +363,13 @@ int gsmtap_sendmsg(struct gsmtap_inst *gti, struct msgb *msg) if (!gti) return -ENODEV; - if (gti->ofd_wq_mode) - return osmo_wqueue_enqueue(>i->wq, msg); + if (gti->osmo_io_mode) + return osmo_iofd_write_msgb(gti->out, msg); else { /* try immediate send and return error if any */ int rc; - rc = write(gsmtap_inst_fd(gti), msg->data, msg->len); + rc = write(gsmtap_inst_fd2(gti), msg->data, msg->len); if (rc < 0) { return rc; } else if (rc >= msg->len) { @@ -296,12 +382,26 @@ int gsmtap_sendmsg(struct gsmtap_inst *gti, struct msgb *msg) } } +/*! Send a \ref msgb through a GSMTAP source; free the message even if tx queue full. + * \param[in] gti GSMTAP instance + * \param[in] msg message buffer; always freed, caller must not reference it later. + * \return 0 in case of success; negative in case of error + */ +int gsmtap_sendmsg_free(struct gsmtap_inst *gti, struct msgb *msg) +{ + int rc; + rc = gsmtap_sendmsg(gti, msg); + if (rc < 0) + msgb_free(msg); + return rc; +} + /*! send an arbitrary type through GSMTAP. * See \ref gsmtap_makemsg_ex for arguments */ int gsmtap_send_ex(struct gsmtap_inst *gti, uint8_t type, uint16_t arfcn, uint8_t ts, uint8_t chan_type, uint8_t ss, uint32_t fn, - int8_t signal_dbm, uint8_t snr, const uint8_t *data, + int8_t signal_dbm, int8_t snr, const uint8_t *data, unsigned int len) { struct msgb *msg; @@ -326,47 +426,13 @@ int gsmtap_send_ex(struct gsmtap_inst *gti, uint8_t type, uint16_t arfcn, uint8_ */ int gsmtap_send(struct gsmtap_inst *gti, uint16_t arfcn, uint8_t ts, uint8_t chan_type, uint8_t ss, uint32_t fn, - int8_t signal_dbm, uint8_t snr, const uint8_t *data, + int8_t signal_dbm, int8_t snr, const uint8_t *data, unsigned int len) { return gsmtap_send_ex(gti, GSMTAP_TYPE_UM, arfcn, ts, chan_type, ss, fn, signal_dbm, snr, data, len); } -/* Callback from select layer if we can write to the socket */ -static int gsmtap_wq_w_cb(struct osmo_fd *ofd, struct msgb *msg) -{ - int rc; - - rc = write(ofd->fd, msg->data, msg->len); - if (rc < 0) { - return rc; - } - if (rc != msg->len) { - return -EIO; - } - - return 0; -} - -/* Callback from select layer if we can read from the sink socket */ -static int gsmtap_sink_fd_cb(struct osmo_fd *fd, unsigned int flags) -{ - int rc; - uint8_t buf[4096]; - - if (!(flags & OSMO_FD_READ)) - return 0; - - rc = read(fd->fd, buf, sizeof(buf)); - if (rc < 0) { - return rc; - } - /* simply discard any data arriving on the socket */ - - return 0; -} - /*! Add a local sink to an existing GSMTAP source and return fd * \param[in] gti existing GSMTAP source * \returns file descriptor of locally bound receive socket @@ -384,30 +450,63 @@ static int gsmtap_sink_fd_cb(struct osmo_fd *fd, unsigned int flags) */ int gsmtap_source_add_sink(struct gsmtap_inst *gti) { - int fd, rc; + return gti->sink_fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd2(gti)); +} - fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd(gti)); - if (fd < 0) - return fd; +/* Registered in Osmo IO as a no-op to set the write callback. */ +static void gsmtap_ops_noop_cb(struct osmo_io_fd *iofd, int res, struct msgb *msg) +{ +} - if (gti->ofd_wq_mode) { - struct osmo_fd *sink_ofd; +static struct osmo_io_ops gsmtap_ops = { .write_cb = gsmtap_ops_noop_cb }; - sink_ofd = >i->sink_ofd; - sink_ofd->fd = fd; - sink_ofd->when = OSMO_FD_READ; - sink_ofd->cb = gsmtap_sink_fd_cb; +/*! Open GSMTAP source socket, connect and register osmo_fd + * \param[in] local_host IP address in string format + * \param[in] local_port UDP port number in host byte order + * \param[in] rem_host host name or IP address in string format + * \param[in] rem_port UDP port number in host byte order + * \param[in] ofd_wq_mode Register \ref osmo_wqueue (1) or not (0) + * \return callee-allocated \ref gsmtap_inst + * + * Open GSMTAP source (sending) socket, connect it to remote host/port, + * bind it local host/port, + * allocate 'struct gsmtap_inst' and optionally osmo_fd/osmo_wqueue + * registration. + */ +struct gsmtap_inst *gsmtap_source_init2(const char *local_host, uint16_t local_port, + const char *rem_host, uint16_t rem_port, int ofd_wq_mode) +{ + struct gsmtap_inst *gti; + int fd; - rc = osmo_fd_register(sink_ofd); - if (rc < 0) { - close(fd); - return rc; - } + fd = gsmtap_source_init_fd2(local_host, local_port, rem_host, rem_port); + if (fd < 0) + return NULL; + + gti = talloc_zero(NULL, struct gsmtap_inst); + gti->osmo_io_mode = ofd_wq_mode; + /* Still using the wq member for its 'fd' field only, since we are keeping it for now, anyways */ + gti->wq.bfd.fd = fd; + gti->sink_fd = -1; + + if (ofd_wq_mode) { + gti->out = osmo_iofd_setup(gti, gti->wq.bfd.fd, "gsmtap_inst.io_fd", OSMO_IO_FD_MODE_READ_WRITE, &gsmtap_ops, NULL); + if (gti->out == NULL) + goto err_cleanup; + if (osmo_iofd_register(gti->out, gti->wq.bfd.fd) < 0) + goto err_cleanup; + + /* osmo write queue previously used was set up with value of 64 */ + osmo_iofd_set_txqueue_max_length(gti->out, 64); } - return fd; -} + return gti; +err_cleanup: + talloc_free(gti); + close(fd); + return NULL; +} /*! Open GSMTAP source socket, connect and register osmo_fd * \param[in] host host name or IP address in string format @@ -422,31 +521,25 @@ int gsmtap_source_add_sink(struct gsmtap_inst *gti) struct gsmtap_inst *gsmtap_source_init(const char *host, uint16_t port, int ofd_wq_mode) { - struct gsmtap_inst *gti; - int fd, rc; - - fd = gsmtap_source_init_fd(host, port); - if (fd < 0) - return NULL; + return gsmtap_source_init2(NULL, 0, host, port, ofd_wq_mode); +} - gti = talloc_zero(NULL, struct gsmtap_inst); - gti->ofd_wq_mode = ofd_wq_mode; - gti->wq.bfd.fd = fd; - gti->sink_ofd.fd = -1; +void gsmtap_source_free(struct gsmtap_inst *gti) +{ + if (!gti) + return; - if (ofd_wq_mode) { - osmo_wqueue_init(>i->wq, 64); - gti->wq.write_cb = &gsmtap_wq_w_cb; + if (gti->osmo_io_mode) { + osmo_iofd_free(gti->out); - rc = osmo_fd_register(>i->wq.bfd); - if (rc < 0) { - talloc_free(gti); - close(fd); - return NULL; + if (gti->sink_fd != -1) { + close(gti->sink_fd); + gti->sink_fd = -1; } + } - return gti; + talloc_free(gti); } #endif /* HAVE_SYS_SOCKET_H */ @@ -461,8 +554,8 @@ const struct value_string gsmtap_gsm_channel_names[] = { { GSMTAP_CHANNEL_SDCCH, "SDCCH" }, { GSMTAP_CHANNEL_SDCCH4, "SDCCH/4" }, { GSMTAP_CHANNEL_SDCCH8, "SDCCH/8" }, - { GSMTAP_CHANNEL_TCH_F, "TCH/F/FACCH/F" }, - { GSMTAP_CHANNEL_TCH_H, "TCH/H/FACCH/H" }, + { GSMTAP_CHANNEL_FACCH_F, "FACCH/F" }, + { GSMTAP_CHANNEL_FACCH_H, "FACCH/H" }, { GSMTAP_CHANNEL_PACCH, "PACCH" }, { GSMTAP_CHANNEL_CBCH52, "CBCH" }, { GSMTAP_CHANNEL_PDCH, "PDCH" } , @@ -471,8 +564,10 @@ const struct value_string gsmtap_gsm_channel_names[] = { { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_SDCCH, "LSACCH" }, { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_SDCCH4, "SACCH/4" }, { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_SDCCH8, "SACCH/8" }, - { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_TCH_F, "SACCH/F" }, - { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_TCH_H, "SACCH/H" }, + { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_FACCH_F, "SACCH/F" }, + { GSMTAP_CHANNEL_ACCH | GSMTAP_CHANNEL_FACCH_H, "SACCH/H" }, + { GSMTAP_CHANNEL_VOICE_F, "TCH/F" }, + { GSMTAP_CHANNEL_VOICE_H, "TCH/H" }, { 0, NULL } }; @@ -485,6 +580,8 @@ const struct value_string gsmtap_type_names[] = { { GSMTAP_TYPE_TETRA_I1, "TETRA V+D" }, { GSMTAP_TYPE_TETRA_I1_BURST, "TETRA bursts" }, { GSMTAP_TYPE_WMX_BURST, "WiMAX burst" }, + { GSMTAP_TYPE_GB_LLC, "GPRS Gb LLC" }, + { GSMTAP_TYPE_GB_SNDCP, "GPRS Gb SNDCP" }, { GSMTAP_TYPE_GMR1_UM, "GMR-1 air interfeace (MES-MS<->GTS)"}, { GSMTAP_TYPE_UMTS_RLC_MAC, "UMTS RLC/MAC" }, { GSMTAP_TYPE_UMTS_RRC, "UMTS RRC" }, @@ -493,6 +590,9 @@ const struct value_string gsmtap_type_names[] = { { GSMTAP_TYPE_LTE_MAC_FRAMED, "LTE MAC with context hdr" }, { GSMTAP_TYPE_OSMOCORE_LOG, "libosmocore logging" }, { GSMTAP_TYPE_QC_DIAG, "Qualcomm DIAG" }, + { GSMTAP_TYPE_LTE_NAS, "LTE Non-Access Stratum" }, + { GSMTAP_TYPE_E1T1, "E1/T1 lines" }, + { GSMTAP_TYPE_GSM_RLP, "GSM Radio Link Protocol" }, { 0, NULL } }; diff --git a/src/isdnhdlc.c b/src/core/isdnhdlc.c index 58b4a661..4ced5afd 100644 --- a/src/isdnhdlc.c +++ b/src/core/isdnhdlc.c @@ -18,10 +18,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */ #include <string.h> @@ -101,13 +97,12 @@ check_frame(struct osmo_isdnhdlc_vars *hdlc) When a new flag is found, the complete frame has been received and the CRC is checked. If a valid frame is found, the function returns the frame length - excluding the CRC with the bit HDLC_END_OF_FRAME set. + excluding the CRC. If the beginning of a valid frame is found, the function returns the length. If a framing error is found (too many 1s and not a flag) the function - returns the length with the bit OSMO_HDLC_FRAMING_ERROR set. - If a CRC error is found the function returns the length with the - bit OSMO_HDLC_CRC_ERROR set. + returns -OSMO_HDLC_FRAMING_ERROR. + If a CRC error is found the function returns -OSMO_HDLC_CRC_ERROR. If the frame length exceeds the destination buffer size, the function returns the length with the bit OSMO_HDLC_LENGTH_ERROR set. diff --git a/src/core/it_q.c b/src/core/it_q.c new file mode 100644 index 00000000..810dc903 --- /dev/null +++ b/src/core/it_q.c @@ -0,0 +1,269 @@ +/*! \file it_q.c + * Osmocom Inter-Thread queue implementation */ +/* (C) 2019 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +/*! \addtogroup it_q + * @{ + * Inter-Thread Message Queue. + * + * This implements a general-purpose queue between threads. It uses + * user-provided data types (containing a llist_head as initial member) + * as elements in the queue and an eventfd-based notification mechanism. + * Hence, it can be used for pretty much anything, including but not + * limited to msgbs, including msgb-wrapped osmo_prim. + * + * The idea is that the sending thread simply calls osmo_it_q_enqueue(). + * The receiving thread is woken up from its osmo_select_main() loop by eventfd, + * and a general osmo_fd callback function for the eventfd will dequeue each item + * and call a queue-specific callback function. + */ + +#include "config.h" + +#ifdef HAVE_SYS_EVENTFD_H + +#include <pthread.h> +#include <unistd.h> +#include <string.h> +#include <errno.h> +#include <sys/eventfd.h> + +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/it_q.h> + +/* "increment" the eventfd by specified 'inc' */ +static int eventfd_increment(int fd, uint64_t inc) +{ + int rc; + + rc = write(fd, &inc, sizeof(inc)); + if (rc != sizeof(inc)) + return -1; + + return 0; +} + +/* global (for all threads) list of message queues in a program + associated lock */ +static LLIST_HEAD(it_queues); +static pthread_rwlock_t it_queues_rwlock = PTHREAD_RWLOCK_INITIALIZER; + +/* resolve it-queue by its [globally unique] name; must be called with rwlock held */ +static struct osmo_it_q *_osmo_it_q_by_name(const char *name) +{ + struct osmo_it_q *q; + llist_for_each_entry(q, &it_queues, entry) { + if (!strcmp(q->name, name)) + return q; + } + return NULL; +} + +/*! resolve it-queue by its [globally unique] name */ +struct osmo_it_q *osmo_it_q_by_name(const char *name) +{ + struct osmo_it_q *q; + pthread_rwlock_rdlock(&it_queues_rwlock); + q = _osmo_it_q_by_name(name); + pthread_rwlock_unlock(&it_queues_rwlock); + return q; +} + +/* osmo_fd call-back when eventfd is readable */ +static int osmo_it_q_fd_cb(struct osmo_fd *ofd, unsigned int what) +{ + struct osmo_it_q *q = (struct osmo_it_q *) ofd->data; + uint64_t val; + int i, rc; + + if (!(what & OSMO_FD_READ)) + return 0; + + rc = read(ofd->fd, &val, sizeof(val)); + if (rc < sizeof(val)) + return rc; + + for (i = 0; i < val; i++) { + struct llist_head *item = _osmo_it_q_dequeue(q); + /* in case the user might have called osmo_it_q_flush() we may + * end up in the eventfd-dispatch but without any messages left in the queue, + * otherwise I'd have loved to OSMO_ASSERT(msg) here. */ + if (!item) + break; + q->read_cb(q, item); + } + + return 0; +} + +/*! Allocate a new inter-thread message queue. + * \param[in] ctx talloc context from which to allocate the queue + * \param[in] name human-readable string name of the queue; function creates a copy. + * \param[in] read_cb call-back function to be called for each de-queued message; may be + * NULL in case you don't want eventfd/osmo_select integration and + * will manually take care of noticing if and when to dequeue. + * \returns a newly-allocated inter-thread message queue; NULL in case of error */ +struct osmo_it_q *osmo_it_q_alloc(void *ctx, const char *name, unsigned int max_length, + void (*read_cb)(struct osmo_it_q *q, struct llist_head *item), + void *data) +{ + struct osmo_it_q *q; + int fd; + + q = talloc_zero(ctx, struct osmo_it_q); + if (!q) + return NULL; + q->data = data; + q->name = talloc_strdup(q, name); + q->current_length = 0; + q->max_length = max_length; + q->read_cb = read_cb; + INIT_LLIST_HEAD(&q->list); + pthread_mutex_init(&q->mutex, NULL); + q->event_ofd.fd = -1; + + if (q->read_cb) { + /* create eventfd *if* the user has provided a read_cb function */ + fd = eventfd(0, 0); + if (fd < 0) { + talloc_free(q); + return NULL; + } + + /* initialize BUT NOT REGISTER the osmo_fd. The receiving thread must + * take are to select/poll/read/... on it */ + osmo_fd_setup(&q->event_ofd, fd, OSMO_FD_READ, osmo_it_q_fd_cb, q, 0); + } + + /* add to global list of queues, checking for duplicate names */ + pthread_rwlock_wrlock(&it_queues_rwlock); + if (_osmo_it_q_by_name(q->name)) { + pthread_rwlock_unlock(&it_queues_rwlock); + if (q->event_ofd.fd >= 0) + osmo_fd_close(&q->event_ofd); + talloc_free(q); + return NULL; + } + llist_add_tail(&q->entry, &it_queues); + pthread_rwlock_unlock(&it_queues_rwlock); + + return q; +} + +static void *item_dequeue(struct llist_head *queue) +{ + struct llist_head *lh; + + if (llist_empty(queue)) + return NULL; + + lh = queue->next; + if (lh) { + llist_del(lh); + return lh; + } else + return NULL; +} + +/*! Flush all messages currently present in queue */ +static void _osmo_it_q_flush(struct osmo_it_q *q) +{ + void *item; + while ((item = item_dequeue(&q->list))) { + talloc_free(item); + } + q->current_length = 0; +} + +/*! Flush all messages currently present in queue */ +void osmo_it_q_flush(struct osmo_it_q *q) +{ + OSMO_ASSERT(q); + + pthread_mutex_lock(&q->mutex); + _osmo_it_q_flush(q); + pthread_mutex_unlock(&q->mutex); +} + +/*! Destroy a message queue */ +void osmo_it_q_destroy(struct osmo_it_q *q) +{ + OSMO_ASSERT(q); + + /* first remove from global list of queues */ + pthread_rwlock_wrlock(&it_queues_rwlock); + llist_del(&q->entry); + pthread_rwlock_unlock(&it_queues_rwlock); + /* next, close the eventfd */ + if (q->event_ofd.fd >= 0) + osmo_fd_close(&q->event_ofd); + /* flush all messages still present */ + osmo_it_q_flush(q); + pthread_mutex_destroy(&q->mutex); + /* and finally release memory */ + talloc_free(q); +} + +/*! Thread-safe en-queue to an inter-thread message queue. + * \param[in] queue Inter-thread queue on which to enqueue + * \param[in] item Item to enqueue. Must have llist_head as first member! + * \returns 0 on success; negative on error */ +int _osmo_it_q_enqueue(struct osmo_it_q *queue, struct llist_head *item) +{ + OSMO_ASSERT(queue); + OSMO_ASSERT(item); + + pthread_mutex_lock(&queue->mutex); + if (queue->current_length+1 > queue->max_length) { + pthread_mutex_unlock(&queue->mutex); + return -ENOSPC; + } + llist_add_tail(item, &queue->list); + queue->current_length++; + pthread_mutex_unlock(&queue->mutex); + /* increment eventfd counter by one */ + if (queue->event_ofd.fd >= 0) + eventfd_increment(queue->event_ofd.fd, 1); + return 0; +} + + +/*! Thread-safe de-queue from an inter-thread message queue. + * \param[in] queue Inter-thread queue from which to dequeue + * \returns llist_head of dequeued message; NULL if none available + */ +struct llist_head *_osmo_it_q_dequeue(struct osmo_it_q *queue) +{ + struct llist_head *l; + OSMO_ASSERT(queue); + + pthread_mutex_lock(&queue->mutex); + + l = item_dequeue(&queue->list); + if (l != NULL) + queue->current_length--; + + pthread_mutex_unlock(&queue->mutex); + + return l; +} + + +#endif /* HAVE_SYS_EVENTFD_H */ + +/*! @} */ diff --git a/src/core/libosmocore.map b/src/core/libosmocore.map new file mode 100644 index 00000000..c5ab6e37 --- /dev/null +++ b/src/core/libosmocore.map @@ -0,0 +1,631 @@ +LIBOSMOCORE_1.0 { +global: + +assert_loginfo; +bit_value_to_char; +bitvec_add_array; +bitvec_alloc; +bitvec_fill; +bitvec_find_bit_pos; +bitvec_free; +bitvec_get_bit_high; +bitvec_get_bit_pos; +bitvec_get_bit_pos_high; +bitvec_get_bytes; +bitvec_get_int16_msb; +bitvec_get_nth_set_bit; +bitvec_get_uint; +bitvec_pack; +bitvec_read_field; +bitvec_rl; +bitvec_rl_curbit; +bitvec_set_bit; +bitvec_set_bit_pos; +bitvec_set_bits; +bitvec_set_bytes; +bitvec_set_u64; +bitvec_set_uint; +bitvec_shiftl; +bitvec_spare_padding; +bitvec_to_string_r; +bitvec_unhex; +bitvec_unpack; +bitvec_write_field; +bitvec_zero; +chantype_gsmtap2rsl; +chantype_rsl2gsmtap; +chantype_rsl2gsmtap2; +get_string_value; +get_value_string; +get_value_string_or_null; +gsmtap_gsm_channel_names; +gsmtap_inst_fd; +gsmtap_inst_fd2; +gsmtap_makemsg; +gsmtap_makemsg_ex; +gsmtap_send; +gsmtap_send_ex; +gsmtap_sendmsg; +gsmtap_sendmsg_free; +gsmtap_source_add_sink; +gsmtap_source_add_sink_fd; +gsmtap_source_free; +gsmtap_source_init; +gsmtap_source_init2; +gsmtap_source_init_fd; +gsmtap_source_init_fd2; +gsmtap_type_names; +log_add_target; +log_category_name; +log_check_level; +log_cache_enable; +log_cache_update; +log_del_target; +log_enable_multithread; +log_fini; +log_init; +log_level_str; +loglevel_strs; +logp; +logp2; +log_parse_category; +log_parse_category_mask; +log_parse_level; +logp_stub; +log_reset_context; +log_set_all_filter; +log_set_category_filter; +log_set_context; +log_set_log_level; +log_set_print_category; +log_set_print_category_hex; +log_set_print_extended_timestamp; +log_set_print_filename; +log_set_print_filename2; +log_set_print_filename_pos; +log_set_print_level; +log_set_print_tid; +log_set_print_timestamp; +log_set_use_color; +log_target_create; +log_target_create_file; +log_target_create_file_stream; +log_target_create_gsmtap; +log_target_create_rb; +log_target_create_stderr; +log_target_create_syslog; +log_target_create_systemd; +log_target_destroy; +log_target_file_reopen; +log_target_file_switch_to_stream; +log_target_file_switch_to_wqueue; +log_target_find; +log_target_rb_avail_size; +log_target_rb_get; +log_target_rb_used_size; +log_target_systemd_set_raw; +log_targets_reopen; +log_tgt_mutex_lock_impl; +log_tgt_mutex_unlock_impl; +msgb_alloc; +msgb_alloc_c; +msgb_copy; +msgb_copy_c; +msgb_copy_resize; +msgb_copy_resize_c; +msgb_data; +msgb_dequeue; +msgb_enqueue; +_msgb_eq; +msgb_free; +msgb_hexdump; +msgb_hexdump_buf; +msgb_hexdump_c; +msgb_length; +msgb_printf; +msgb_reset; +msgb_resize_area; +msgb_set_talloc_ctx; +msgb_talloc_ctx_init; +osmo_base64_decode; +osmo_base64_encode; +osmo_bcd2char; +osmo_bcd2str; +osmo_bit_reversal; +osmo_char2bcd; +osmo_clock_gettime; +osmo_clock_override_add; +osmo_clock_override_enable; +osmo_clock_override_gettimespec; +osmo_close_all_fds_above; +osmo_config_list_parse; +osmo_constant_time_cmp; +osmo_conv_decode; +osmo_conv_decode_acc; +osmo_conv_decode_deinit; +osmo_conv_decode_flush; +osmo_conv_decode_get_best_end_state; +osmo_conv_decode_get_output; +osmo_conv_decode_init; +osmo_conv_decode_reset; +osmo_conv_decode_rewind; +osmo_conv_decode_scan; +osmo_conv_encode; +osmo_conv_encode_flush; +osmo_conv_encode_init; +osmo_conv_encode_load_state; +osmo_conv_encode_raw; +osmo_conv_get_input_length; +osmo_conv_get_output_length; +osmo_counter_alloc; +osmo_counter_difference; +osmo_counter_free; +osmo_counter_get_by_name; +osmo_counters_count; +osmo_counters_for_each; +osmo_crc16; +osmo_crc16_ccitt; +osmo_crc16_ccitt_table; +osmo_crc16_table; +osmo_crc16gen_check_bits; +osmo_crc16gen_compute_bits; +osmo_crc16gen_set_bits; +osmo_crc32gen_check_bits; +osmo_crc32gen_compute_bits; +osmo_crc32gen_set_bits; +osmo_crc64gen_check_bits; +osmo_crc64gen_compute_bits; +osmo_crc64gen_set_bits; +osmo_crc8gen_check_bits; +osmo_crc8gen_compute_bits; +osmo_crc8gen_set_bits; +osmo_ctx; +osmo_ctx_init; +osmo_daemonize; +osmo_decode_big_endian; +osmo_encode_big_endian; +osmo_environment_append; +osmo_environment_filter; +osmo_environment_whitelist; +osmo_escape_cstr_buf; +osmo_escape_cstr_c; +osmo_escape_str; +osmo_escape_str_buf; +osmo_escape_str_buf2; +osmo_escape_str_buf3; +osmo_escape_str_c; +osmo_event_for_prim; +osmo_fd_close; +osmo_fd_disp_fds; +osmo_fd_fill_fds; +osmo_fd_get_by_fd; +osmo_fd_is_registered; +osmo_fd_register; +osmo_fd_setup; +osmo_fd_unregister; +osmo_fd_update_when; +osmo_float_str_to_int; +osmo_fsm_event_name; +osmo_fsm_find_by_name; +osmo_fsm_inst_alloc; +osmo_fsm_inst_alloc_child; +_osmo_fsm_inst_broadcast_children; +osmo_fsm_inst_change_parent; +_osmo_fsm_inst_dispatch; +osmo_fsm_inst_find_by_id; +osmo_fsm_inst_find_by_name; +osmo_fsm_inst_free; +osmo_fsm_inst_name; +_osmo_fsm_inst_state_chg; +_osmo_fsm_inst_state_chg_keep_or_start_timer; +_osmo_fsm_inst_state_chg_keep_or_start_timer_ms; +_osmo_fsm_inst_state_chg_keep_timer; +_osmo_fsm_inst_state_chg_ms; +_osmo_fsm_inst_term; +_osmo_fsm_inst_term_children; +osmo_fsm_inst_unlink_parent; +osmo_fsm_inst_update_id; +osmo_fsm_inst_update_id_f; +osmo_fsm_inst_update_id_f_sanitize; +osmo_fsm_log_addr; +osmo_fsm_log_timeouts; +osmo_fsm_register; +osmo_fsm_set_dealloc_ctx; +osmo_fsm_state_name; +osmo_fsm_term_cause_names; +osmo_fsm_term_safely; +osmo_fsm_unregister; +osmo_generate_backtrace; +osmo_get_macaddr; +osmo_gettid; +osmo_gettimeofday; +osmo_gettimeofday_override; +osmo_gettimeofday_override_add; +osmo_gettimeofday_override_time; +osmo_g_fsms; +osmo_hexdump; +osmo_hexdump_buf; +osmo_hexdump_c; +osmo_hexdump_nospc; +osmo_hexdump_nospc_c; +osmo_hexparse; +osmo_identifier_sanitize_buf; +osmo_identifier_valid; +osmo_init_ignore_signals; +osmo_init_logging; +osmo_init_logging2; +osmo_int_to_float_str_buf; +osmo_int_to_float_str_c; +osmo_io_backend_names; +osmo_iofd_close; +osmo_iofd_free; +osmo_iofd_get_data; +osmo_iofd_get_ioops; +osmo_iofd_get_fd; +osmo_iofd_get_name; +osmo_iofd_set_name; +osmo_iofd_get_priv_nr; +osmo_iofd_init; +osmo_iofd_mode_names; +osmo_iofd_ops; +osmo_iofd_register; +osmo_iofd_sendto_msgb; +osmo_iofd_sctp_send_msgb; +osmo_iofd_sendmsg_msgb; +osmo_iofd_set_alloc_info; +osmo_iofd_set_cmsg_size; +osmo_iofd_set_data; +osmo_iofd_set_ioops; +osmo_iofd_set_priv_nr; +osmo_iofd_set_txqueue_max_length; +osmo_iofd_setup; +osmo_iofd_txqueue_clear; +osmo_iofd_txqueue_len; +osmo_iofd_unregister; +osmo_iofd_uring_init; +osmo_iofd_notify_connected; +osmo_iofd_write_msgb; +osmo_ip_str_type; +osmo_isdnhdlc_decode; +osmo_isdnhdlc_encode; +osmo_isdnhdlc_out_init; +osmo_isdnhdlc_rcv_init; +osmo_is_hexstr; +osmo_isqrt32; +osmo_it_q_alloc; +osmo_it_q_by_name; +_osmo_it_q_dequeue; +osmo_it_q_destroy; +_osmo_it_q_enqueue; +osmo_it_q_flush; +osmo_log_backtrace; +osmo_log_info; +osmo_log_target_list; +osmo_luhn; +osmo_macaddr_parse; +osmo_mnl_destroy; +osmo_mnl_init; +osmo_multiaddr_ip_and_port_snprintf; +osmo_netdev_add_addr; +osmo_netdev_add_route; +osmo_netdev_alloc; +osmo_netdev_free; +osmo_netdev_get_dev_name; +osmo_netdev_get_ifindex; +osmo_netdev_get_name; +osmo_netdev_get_netns_name; +osmo_netdev_get_priv_data; +osmo_netdev_ifupdown; +osmo_netdev_is_registered; +osmo_netdev_register; +osmo_netdev_set_dev_name_chg_cb; +osmo_netdev_set_ifindex; +osmo_netdev_set_ifupdown_ind_cb; +osmo_netdev_set_mtu_chg_cb; +osmo_netdev_set_netns_name; +osmo_netdev_set_priv_data; +osmo_netdev_unregister; +osmo_netns_open_fd; +osmo_netns_switch_enter; +osmo_netns_switch_exit; +osmo_nibble_shift_left_unal; +osmo_nibble_shift_right; +osmo_panic; +osmo_pbit2ubit; +osmo_pbit2ubit_ext; +osmo_plugin_load_all; +osmo_prbs11; +osmo_prbs15; +osmo_prbs7; +osmo_prbs9; +osmo_prbs_get_ubit; +osmo_prbs_get_ubits; +osmo_prbs_state_init; +osmo_prim_op_names; +osmo_print_n; +osmo_quote_cstr_buf; +osmo_quote_cstr_c; +osmo_quote_str; +osmo_quote_str_buf; +osmo_quote_str_buf2; +osmo_quote_str_buf3; +osmo_quote_str_c; +osmo_revbytebits_32; +osmo_revbytebits_8; +osmo_revbytebits_buf; +osmo_sbit2ubit; +osmo_select_init; +osmo_select_main; +osmo_select_main_ctx; +osmo_select_shutdown_done; +osmo_select_shutdown_request; +osmo_select_shutdown_requested; +osmo_separated_identifiers_valid; +osmo_sercomm_change_speed; +osmo_sercomm_drv_pull; +osmo_sercomm_drv_rx_char; +osmo_sercomm_init; +osmo_sercomm_initialized; +osmo_sercomm_register_rx_cb; +osmo_sercomm_sendmsg; +osmo_sercomm_tx_queue_depth; +osmo_serial_clear_custom_baudrate; +osmo_serial_init; +osmo_serial_set_baudrate; +osmo_serial_set_custom_baudrate; +osmo_serial_speed_t; +osmo_set_panic_handler; +osmo_signal_dispatch; +osmo_signalfd_setup; +osmo_signal_register_handler; +osmo_signal_talloc_ctx_init; +osmo_signal_unregister_handler; +osmo_sockaddr_cmp; +osmo_sockaddr_from_octets; +osmo_sockaddr_from_str_and_uint; +osmo_sockaddr_in_to_str_and_uint; +osmo_sockaddr_is_any; +osmo_sockaddr_is_local; +osmo_sockaddr_local_ip; +osmo_sockaddr_netmask_to_prefixlen; +osmo_sockaddr_ntop; +osmo_sockaddr_port; +osmo_sockaddr_set_port; +osmo_sockaddr_str_cmp; +osmo_sockaddr_str_from_32; +osmo_sockaddr_str_from_32h; +osmo_sockaddr_str_from_32n; +osmo_sockaddr_str_from_in6_addr; +osmo_sockaddr_str_from_in_addr; +osmo_sockaddr_str_from_sockaddr; +osmo_sockaddr_str_from_sockaddr_in; +osmo_sockaddr_str_from_sockaddr_in6; +osmo_sockaddr_str_from_osa; +osmo_sockaddr_str_from_str; +osmo_sockaddr_str_from_str2; +osmo_sockaddr_str_is_nonzero; +osmo_sockaddr_str_is_set; +osmo_sockaddr_str_to_32; +osmo_sockaddr_str_to_32h; +osmo_sockaddr_str_to_32n; +osmo_sockaddr_str_to_in6_addr; +osmo_sockaddr_str_to_in_addr; +osmo_sockaddr_str_to_sockaddr; +osmo_sockaddr_str_to_sockaddr_in; +osmo_sockaddr_str_to_sockaddr_in6; +osmo_sockaddr_str_to_osa; +osmo_sockaddr_to_octets; +osmo_sockaddr_to_str; +osmo_sockaddr_to_str_and_uint; +osmo_sockaddr_to_str_buf; +osmo_sockaddr_to_str_buf2; +osmo_sockaddr_to_str_c; +osmo_sock_get_ip_and_port; +osmo_sock_get_local_ip; +osmo_sock_get_local_ip_port; +osmo_sock_get_name; +osmo_sock_get_name2; +osmo_sock_get_name2_c; +osmo_sock_get_name_buf; +osmo_sock_get_remote_ip; +osmo_sock_get_remote_ip_port; +osmo_sock_init; +osmo_sock_init2; +osmo_sock_init2_multiaddr; +osmo_sock_init2_multiaddr2; +osmo_sock_init2_ofd; +osmo_sock_init_ofd; +osmo_sock_init_osa; +osmo_sock_init_osa_ofd; +osmo_sock_init_sa; +osmo_sock_local_ip; +osmo_sock_mcast_all_set; +osmo_sock_mcast_iface_set; +osmo_sock_mcast_loop_set; +osmo_sock_mcast_subscribe; +osmo_sock_mcast_ttl_set; +osmo_sock_multiaddr_add_local_addr; +osmo_sock_multiaddr_del_local_addr; +osmo_sock_multiaddr_get_ip_and_port; +osmo_sock_multiaddr_get_name_buf; +osmo_sock_sctp_get_peer_addr_info; +osmo_sock_set_dscp; +osmo_sock_set_priority; +osmo_sock_unix_init; +osmo_sock_unix_init_ofd; +osmo_soft_uart_default_cfg; +osmo_soft_uart_alloc; +osmo_soft_uart_free; +osmo_soft_uart_configure; +osmo_soft_uart_get_name; +osmo_soft_uart_set_name; +osmo_soft_uart_set_rx; +osmo_soft_uart_set_tx; +osmo_soft_uart_rx_ubits; +osmo_soft_uart_tx_ubits; +osmo_soft_uart_get_status; +osmo_soft_uart_set_status; +osmo_soft_uart_set_status_line; +osmo_soft_uart_flush_rx; +osmo_stat_item_dec; +osmo_stat_item_flush; +osmo_stat_item_for_each_group; +osmo_stat_item_for_each_item; +osmo_stat_item_get_by_name; +osmo_stat_item_get_desc; +osmo_stat_item_get_group_by_name_idx; +osmo_stat_item_get_group_by_name_idxname; +osmo_stat_item_get_last; +osmo_stat_item_group_alloc; +osmo_stat_item_group_free; +osmo_stat_item_group_get_item; +osmo_stat_item_group_reset; +osmo_stat_item_group_set_name; +osmo_stat_item_inc; +osmo_stat_item_init; +osmo_stat_item_reset; +osmo_stat_item_set; +osmo_stats_config; +osmo_stats_init; +osmo_stats_report; +osmo_stats_reporter_alloc; +osmo_stats_reporter_create_log; +osmo_stats_reporter_create_statsd; +osmo_stats_reporter_disable; +osmo_stats_reporter_enable; +osmo_stats_reporter_find; +osmo_stats_reporter_free; +osmo_stats_reporter_list; +osmo_stats_reporter_send; +osmo_stats_reporter_send_buffer; +osmo_stats_reporter_set_flush_period; +osmo_stats_reporter_set_local_addr; +osmo_stats_reporter_set_max_class; +osmo_stats_reporter_set_mtu; +osmo_stats_reporter_set_name_prefix; +osmo_stats_reporter_set_remote_addr; +osmo_stats_reporter_set_remote_port; +osmo_stats_reporter_udp_close; +osmo_stats_reporter_udp_open; +osmo_stats_set_interval; +osmo_stats_tcp_osmo_fd_register; +osmo_stats_tcp_osmo_fd_unregister; +osmo_stats_tcp_set_interval; +osmo_stderr_target; +osmo_str2bcd; +osmo_str2lower; +osmo_str2upper; +osmo_strlcpy; +osmo_strnchr; +osmo_strrb_add; +osmo_strrb_create; +osmo_strrb_elements; +osmo_strrb_get_nth; +_osmo_strrb_is_bufindex_valid; +osmo_strrb_is_empty; +osmo_strbuf_drop_tail; +osmo_strbuf_added_tail; +osmo_str_startswith; +osmo_str_to_int; +osmo_str_to_int64; +osmo_str_tolower; +osmo_str_tolower_buf; +osmo_str_tolower_c; +osmo_str_toupper; +osmo_str_toupper_buf; +osmo_str_toupper_c; +osmo_system_nowait; +osmo_system_nowait2; +osmo_t4_encode; +osmo_talloc_replace_string_fmt; +osmo_tcp_stats_config; +_osmo_tdef_fsm_inst_state_chg; +osmo_tdef_get; +osmo_tdef_get_entry; +osmo_tdef_get_state_timeout; +osmo_tdef_range_str_buf; +osmo_tdef_set; +osmo_tdefs_reset; +osmo_tdef_unit_names; +osmo_tdef_val_in_range; +osmo_time_cc_cleanup; +osmo_time_cc_init; +osmo_time_cc_set_flag; +osmo_timer_add; +osmo_timer_del; +osmo_timerfd_disable; +osmo_timerfd_schedule; +osmo_timerfd_setup; +osmo_timer_pending; +osmo_timer_remaining; +osmo_timers_check; +osmo_timer_schedule; +osmo_timer_setup; +osmo_timers_nearest; +osmo_timers_nearest_ms; +osmo_timers_prepare; +osmo_timers_update; +osmo_tundev_alloc; +osmo_tundev_close; +osmo_tundev_free; +osmo_tundev_get_dev_name; +osmo_tundev_get_name; +osmo_tundev_get_netdev; +osmo_tundev_get_netns_name; +osmo_tundev_get_priv_data; +osmo_tundev_is_open; +osmo_tundev_open; +osmo_tundev_send; +osmo_tundev_set_data_ind_cb; +osmo_tundev_set_dev_name; +osmo_tundev_set_netns_name; +osmo_tundev_set_priv_data; +osmo_ubit2pbit; +osmo_ubit2pbit_ext; +osmo_ubit2sbit; +osmo_ubit_dump; +osmo_ubit_dump_buf; +osmo_use_count_by; +osmo_use_count_find; +osmo_use_count_free; +_osmo_use_count_get_put; +osmo_use_count_make_static_entries; +osmo_use_count_name_buf; +osmo_use_count_to_str_buf; +osmo_use_count_to_str_c; +osmo_use_count_total; +osmo_vlogp; +osmo_wqueue_bfd_cb; +osmo_wqueue_clear; +osmo_wqueue_enqueue; +osmo_wqueue_enqueue_quiet; +osmo_wqueue_set_maxlen; +osmo_wqueue_init; +rate_ctr_add; +rate_ctr_difference; +rate_ctr_for_each_counter; +rate_ctr_for_each_group; +rate_ctr_get_by_name; +rate_ctr_get_group_by_name_idx; +rate_ctr_group_alloc; +rate_ctr_group_free; +rate_ctr_group_get_ctr; +rate_ctr_group_reset; +rate_ctr_group_set_name; +rate_ctr_init; +rate_ctr_reset; +rb_erase; +rb_first; +rb_insert_color; +rb_last; +rb_next; +rb_prev; +rb_replace_node; +sercomm_drv_lock; +sercomm_drv_unlock; +tall_ctr_ctx; /* deprecated */ +tall_log_ctx; +tall_msgb_ctx; /* deprecated */ + +local: *; +}; diff --git a/src/logging.c b/src/core/logging.c index b030f8a6..e1721241 100644 --- a/src/logging.c +++ b/src/core/logging.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup logging @@ -29,18 +25,38 @@ * * \file logging.c */ -#include "../config.h" +#include "config.h" #include <stdarg.h> #include <stdlib.h> #include <stdio.h> #include <string.h> +#include <unistd.h> #ifdef HAVE_STRINGS_H #include <strings.h> #endif + +#ifdef HAVE_SYSLOG_H +#include <syslog.h> +#endif + +#ifdef HAVE_SYSTEMTAP +/* include the generated probes header and put markers in code */ +#include "probes.h" +#define TRACE(probe) probe +#define TRACE_ENABLED(probe) probe ## _ENABLED() +#else +/* Wrap the probe to allow it to be removed when no systemtap available */ +#define TRACE(probe) +#define TRACE_ENABLED(probe) (0) +#endif /* HAVE_SYSTEMTAP */ + #include <time.h> #include <sys/time.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> #include <errno.h> #include <pthread.h> @@ -48,9 +64,19 @@ #include <osmocom/core/utils.h> #include <osmocom/core/logging.h> #include <osmocom/core/timer.h> +#include <osmocom/core/thread.h> +#include <osmocom/core/select.h> +#include <osmocom/core/write_queue.h> +#include <osmocom/core/gsmtap_util.h> #include <osmocom/vty/logging.h> /* for LOGGING_STR. */ +/* maximum length of the log string of a single log event (typically line) */ +#define MAX_LOG_SIZE 4096 + +/* maximum number of log statements we queue in file/stderr target write queue */ +#define LOG_WQUEUE_LEN 156 + osmo_static_assert(_LOG_CTX_COUNT <= ARRAY_SIZE(((struct log_context*)NULL)->ctx), enum_logging_ctx_items_fit_in_struct_log_context); osmo_static_assert(_LOG_FLT_COUNT <= ARRAY_SIZE(((struct log_target*)NULL)->filter_data), @@ -64,7 +90,83 @@ static struct log_context log_context; void *tall_log_ctx = NULL; LLIST_HEAD(osmo_log_target_list); +static __thread long int logging_tid; + #if (!EMBEDDED) +/*! One global copy that contains the union of log levels for all targets +* for all categories, used for quick lock free checks of log targets. */ +static volatile uint8_t *log_level_lookup_cache; + +/*! Updates cache for all targets for all categies, caller must hold osmo_log_tgt_mutex. */ +static void log_cache_update_all(void) +{ + struct log_target *tgt; + uint8_t tmp_en[osmo_log_info->num_cat]; + uint8_t tmp_level[osmo_log_info->num_cat]; + + if (!log_level_lookup_cache) + return; + + memset(tmp_en, 0, osmo_log_info->num_cat); + memset(tmp_level, UINT8_MAX, osmo_log_info->num_cat); + + /* values can also decrease.. */ + llist_for_each_entry(tgt, &osmo_log_target_list, entry) { + for (int i = 0; i < osmo_log_info->num_cat; i++) { + struct log_category *cat = &tgt->categories[i]; + tmp_en[i] = OSMO_MAX(tmp_en[i], cat->enabled); + tmp_level[i] = OSMO_MIN(tmp_level[i], cat->loglevel); + tmp_level[i] = tgt->loglevel ? OSMO_MIN(tmp_level[i], tgt->loglevel) : tmp_level[i]; + } + } + + for (int i = 0; i < osmo_log_info->num_cat; i++) + log_level_lookup_cache[i] = tmp_en[i] ? tmp_level[i] : UINT8_MAX; +} + +/*! Updates single cache entry, caller must hold osmo_log_tgt_mutex. + * + * \param[in] mapped_subsys plain category index (after mapping) + * \param[in] enabled log category enabled? + * \param[in] level log level + */ +void log_cache_update(int mapped_subsys, uint8_t enabled, uint8_t level) +{ + struct log_target *tgt; + struct log_category tmp = { UINT8_MAX, 0 }; + + if (!log_level_lookup_cache) + return; + + /* values can also decrease.. */ + llist_for_each_entry(tgt, &osmo_log_target_list, entry) { + struct log_category *cat = &tgt->categories[mapped_subsys]; + tmp.enabled = OSMO_MAX(tmp.enabled, cat->enabled); + tmp.loglevel = OSMO_MIN(tmp.loglevel, cat->loglevel); + tmp.loglevel = tgt->loglevel ? OSMO_MIN(tmp.loglevel, tgt->loglevel) : tmp.loglevel; + } + tmp.enabled = OSMO_MAX(tmp.enabled, enabled); + tmp.loglevel = OSMO_MIN(tmp.loglevel, level); + + log_level_lookup_cache[mapped_subsys] = tmp.enabled ? tmp.loglevel : UINT8_MAX; +} + +/*! Queries log level cache. + * + * \param[in] mapped_subsys plain category index (after mapping) + * \param[in] level log level + * \returns true if logging should happen for at least one log target +*/ +static bool log_cache_check(int mapped_subsys, int level) +{ + if (!log_level_lookup_cache) { + /* log-cache is not enabled, so we simply behave like we did before the cache */ + return true; + } + + return (level < log_level_lookup_cache[mapped_subsys]) ? false : true; +} + /*! This mutex must be held while using osmo_log_target_list or any of its log_targets in a multithread program. Prevents race conditions between threads like producing unordered timestamps or VTY deleting a target while another @@ -123,6 +225,7 @@ const struct value_string loglevel_strs[] = { { 0, NULL }, }; +/* 256 color palette see https://en.wikipedia.org/wiki/ANSI_escape_code#8-bit */ #define INT2IDX(x) (-1*(x)-1) static const struct log_info_cat internal_cat[OSMO_NUM_DLIB] = { [INT2IDX(DLGLOBAL)] = { /* -1 becomes 0 */ @@ -136,94 +239,159 @@ static const struct log_info_cat internal_cat[OSMO_NUM_DLIB] = { .description = "LAPD in libosmogsm", .loglevel = LOGL_NOTICE, .enabled = 1, + .color = "\033[38;5;12m", }, [INT2IDX(DLINP)] = { .name = "DLINP", .description = "A-bis Intput Subsystem", .loglevel = LOGL_NOTICE, .enabled = 1, + .color = "\033[38;5;23m", }, [INT2IDX(DLMUX)] = { .name = "DLMUX", .description = "A-bis B-Subchannel TRAU Frame Multiplex", .loglevel = LOGL_NOTICE, .enabled = 1, + .color = "\033[38;5;25m", }, [INT2IDX(DLMI)] = { .name = "DLMI", .description = "A-bis Input Driver for Signalling", .enabled = 0, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;27m", }, [INT2IDX(DLMIB)] = { .name = "DLMIB", .description = "A-bis Input Driver for B-Channels (voice)", .enabled = 0, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;29m", }, [INT2IDX(DLSMS)] = { .name = "DLSMS", .description = "Layer3 Short Message Service (SMS)", .enabled = 1, .loglevel = LOGL_NOTICE, - .color = "\033[1;38m", + .color = "\033[38;5;31m", }, [INT2IDX(DLCTRL)] = { .name = "DLCTRL", .description = "Control Interface", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;33m", }, [INT2IDX(DLGTP)] = { .name = "DLGTP", .description = "GPRS GTP library", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;35m", }, [INT2IDX(DLSTATS)] = { .name = "DLSTATS", .description = "Statistics messages and logging", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;37m", }, [INT2IDX(DLGSUP)] = { .name = "DLGSUP", .description = "Generic Subscriber Update Protocol", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;39m", }, [INT2IDX(DLOAP)] = { .name = "DLOAP", .description = "Osmocom Authentication Protocol", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;41m", }, [INT2IDX(DLSS7)] = { .name = "DLSS7", .description = "libosmo-sigtran Signalling System 7", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;43m", }, [INT2IDX(DLSCCP)] = { .name = "DLSCCP", .description = "libosmo-sigtran SCCP Implementation", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;45m", }, [INT2IDX(DLSUA)] = { .name = "DLSUA", .description = "libosmo-sigtran SCCP User Adaptation", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;47m", }, [INT2IDX(DLM3UA)] = { .name = "DLM3UA", .description = "libosmo-sigtran MTP3 User Adaptation", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;49m", }, [INT2IDX(DLMGCP)] = { .name = "DLMGCP", .description = "libosmo-mgcp Media Gateway Control Protocol", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;51m", }, [INT2IDX(DLJIBUF)] = { .name = "DLJIBUF", .description = "libosmo-netif Jitter Buffer", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;53m", }, [INT2IDX(DLRSPRO)] = { .name = "DLRSPRO", .description = "Remote SIM protocol", .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;55m", + }, + [INT2IDX(DLNS)] = { + .name = "DLNS", + .description = "GPRS NS layer", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;57m", + }, + [INT2IDX(DLBSSGP)] = { + .name = "DLBSSGP", + .description = "GPRS BSSGP layer", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;59m", + }, + [INT2IDX(DLNSDATA)] = { + .name = "DLNSDATA", + .description = "GPRS NS layer data PDU", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;61m", + }, + [INT2IDX(DLNSSIGNAL)] = { + .name = "DLNSSIGNAL", + .description = "GPRS NS layer signal PDU", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;63m", + }, + [INT2IDX(DLIUUP)] = { + .name = "DLIUUP", + .description = "Iu UP layer", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;65m", + }, + [INT2IDX(DLPFCP)] = { + .name = "DLPFCP", + .description = "libosmo-pfcp Packet Forwarding Control Protocol", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;51m", + }, + [INT2IDX(DLCSN1)] = { + .name = "DLCSN1", + .description = "libosmo-csn1 Concrete Syntax Notation 1 codec", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;11m", + }, + [INT2IDX(DLIO)] = { + .name = "DLIO", + .description = "libosmocore IO Subsystem", + .enabled = 1, .loglevel = LOGL_NOTICE, + .color = "\033[38;5;67m", }, }; @@ -332,6 +500,10 @@ void log_parse_category_mask(struct log_target* target, const char *_mask) } } while ((category_token = strtok(NULL, ":"))); +#if !defined(EMBEDDED) + log_cache_update_all(); +#endif + free(mask); } @@ -344,11 +516,11 @@ static const char* color(int subsys) } static const struct value_string level_colors[] = { - { LOGL_DEBUG, "\033[1;34m" }, - { LOGL_INFO, "\033[1;32m" }, - { LOGL_NOTICE, "\033[1;33m" }, - { LOGL_ERROR, "\033[1;31m" }, - { LOGL_FATAL, "\033[1;31m" }, + { LOGL_DEBUG, OSMO_LOGCOLOR_BLUE }, + { LOGL_INFO, OSMO_LOGCOLOR_GREEN }, + { LOGL_NOTICE, OSMO_LOGCOLOR_YELLOW }, + { LOGL_ERROR, OSMO_LOGCOLOR_RED }, + { LOGL_FATAL, OSMO_LOGCOLOR_RED }, { 0, NULL } }; @@ -376,23 +548,34 @@ static const char *const_basename(const char *path) return bn + 1; } -static void _output(struct log_target *target, unsigned int subsys, - unsigned int level, const char *file, int line, int cont, - const char *format, va_list ap) +/*! main output formatting function for log lines. + * \param[out] buf caller-allocated output buffer for the generated string + * \param[in] buf_len number of bytes available in buf + * \param[in] target log target for which the string is to be formatted + * \param[in] subsys Log sub-system number + * \param[in] level Log level + * \param[in] file name of source code file generating the log + * \param[in] line line source code line number within 'file' generating the log + * \param[in] cont is this a continuation (true) or not (false) + * \param[in] format format string + * \param[in] ap variable argument list for format + * \returns number of bytes written to out */ +static int _output_buf(char *buf, int buf_len, struct log_target *target, unsigned int subsys, + unsigned int level, const char *file, int line, int cont, + const char *format, va_list ap) { - char buf[4096]; - int ret, len = 0, offset = 0, rem = sizeof(buf); + int ret; const char *c_subsys = NULL; + struct osmo_strbuf sb = { .buf = buf, .pos = buf, .len = buf_len }; + + /* safety net in case of encountering errors and returning nothing */ + buf[0] = '\0'; /* are we using color */ if (target->use_color) { c_subsys = color(subsys); - if (c_subsys) { - ret = snprintf(buf + offset, rem, "%s", c_subsys); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); - } + if (c_subsys) + OSMO_STRBUF_PRINTF(sb, "%s", c_subsys); } if (!cont) { if (target->print_ext_timestamp) { @@ -401,13 +584,10 @@ static void _output(struct log_target *target, unsigned int subsys, struct timeval tv; osmo_gettimeofday(&tv, NULL); localtime_r(&tv.tv_sec, &tm); - ret = snprintf(buf + offset, rem, "%04d%02d%02d%02d%02d%02d%03d ", - tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, - tm.tm_hour, tm.tm_min, tm.tm_sec, - (int)(tv.tv_usec / 1000)); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_PRINTF(sb, "%04d%02d%02d%02d%02d%02d%03d ", + tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, + tm.tm_hour, tm.tm_min, tm.tm_sec, + (int)(tv.tv_usec / 1000)); #endif } else if (target->print_timestamp) { time_t tm; @@ -415,100 +595,95 @@ static void _output(struct log_target *target, unsigned int subsys, goto err; /* Get human-readable representation of time. man ctime: we need at least 26 bytes in buf */ - if (rem < 26 || !ctime_r(&tm, buf + offset)) + if (OSMO_STRBUF_REMAIN(sb) < 26 || !ctime_r(&tm, sb.pos)) goto err; - ret = strlen(buf + offset); + ret = strnlen(sb.pos, 26); if (ret <= 0) goto err; + OSMO_STRBUF_ADDED_TAIL(sb, ret); /* Get rid of useless final '\n' added by ctime_r. We want a space instead. */ - buf[offset + ret - 1] = ' '; - OSMO_SNPRINTF_RET(ret, rem, offset, len); - } - if (target->print_category) { - ret = snprintf(buf + offset, rem, "%s%s%s%s ", - target->use_color ? level_color(level) : "", - log_category_name(subsys), - target->use_color ? "\033[0;m" : "", - c_subsys ? c_subsys : ""); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); - } - if (target->print_level) { - ret = snprintf(buf + offset, rem, "%s%s%s%s ", - target->use_color ? level_color(level) : "", - log_level_str(level), - target->use_color ? "\033[0;m" : "", - c_subsys ? c_subsys : ""); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_DROP_TAIL(sb, 1); + OSMO_STRBUF_PRINTF(sb, " "); } - if (target->print_category_hex) { - ret = snprintf(buf + offset, rem, "<%4.4x> ", subsys); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + if (target->print_tid) { + if (logging_tid == 0) + logging_tid = (long int)osmo_gettid(); + OSMO_STRBUF_PRINTF(sb, "%ld ", logging_tid); } + if (target->print_category) + OSMO_STRBUF_PRINTF(sb, "%s%s%s%s ", + target->use_color ? level_color(level) : "", + log_category_name(subsys), + target->use_color ? OSMO_LOGCOLOR_END : "", + c_subsys ? c_subsys : ""); + if (target->print_level) + OSMO_STRBUF_PRINTF(sb, "%s%s%s%s ", + target->use_color ? level_color(level) : "", + log_level_str(level), + target->use_color ? OSMO_LOGCOLOR_END : "", + c_subsys ? c_subsys : ""); + if (target->print_category_hex) + OSMO_STRBUF_PRINTF(sb, "<%4.4x> ", subsys); if (target->print_filename_pos == LOG_FILENAME_POS_HEADER_END) { switch (target->print_filename2) { case LOG_FILENAME_NONE: break; case LOG_FILENAME_PATH: - ret = snprintf(buf + offset, rem, "%s:%d ", file, line); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_PRINTF(sb, "%s:%d ", file, line); break; case LOG_FILENAME_BASENAME: - ret = snprintf(buf + offset, rem, "%s:%d ", const_basename(file), line); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_PRINTF(sb, "%s:%d ", const_basename(file), line); break; } } } - ret = vsnprintf(buf + offset, rem, format, ap); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_APPEND(sb, vsnprintf, format, ap); /* For LOG_FILENAME_POS_LAST, print the source file info only when the caller ended the log * message in '\n'. If so, nip the last '\n' away, insert the source file info and re-append an * '\n'. All this to allow LOGP("start..."); LOGPC("...end\n") constructs. */ if (target->print_filename_pos == LOG_FILENAME_POS_LINE_END - && offset > 0 && buf[offset-1] == '\n') { + && sb.pos > sb.buf && *(sb.pos - 1) == '\n') { switch (target->print_filename2) { case LOG_FILENAME_NONE: break; case LOG_FILENAME_PATH: - offset --; - ret = snprintf(buf + offset, rem, " (%s:%d)\n", file, line); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_DROP_TAIL(sb, 1); + OSMO_STRBUF_PRINTF(sb, " (%s:%d)\n", file, line); break; case LOG_FILENAME_BASENAME: - offset --; - ret = snprintf(buf + offset, rem, " (%s:%d)\n", const_basename(file), line); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + OSMO_STRBUF_DROP_TAIL(sb, 1); + OSMO_STRBUF_PRINTF(sb, " (%s:%d)\n", const_basename(file), line); break; } } - if (target->use_color) { - ret = snprintf(buf + offset, rem, "\033[0;m"); - if (ret < 0) - goto err; - OSMO_SNPRINTF_RET(ret, rem, offset, len); + if (target->use_color && c_subsys) { + /* Ensure the last color escape is sent before the newline + * (to not clobber journald, which works on single-lines only) */ + if (sb.pos > sb.buf && *(sb.pos - 1) == '\n') { + OSMO_STRBUF_DROP_TAIL(sb, 1); + OSMO_STRBUF_PRINTF(sb, OSMO_LOGCOLOR_END "\n"); + } else { + OSMO_STRBUF_PRINTF(sb, OSMO_LOGCOLOR_END); + } } err: - buf[sizeof(buf)-1] = '\0'; - target->output(target, level, buf); + return OSMO_STRBUF_CHAR_COUNT(sb); +} + +/* Format the log line for given target; use a stack buffer and call target->output */ +static void _output(struct log_target *target, unsigned int subsys, + unsigned int level, const char *file, int line, int cont, + const char *format, va_list ap) +{ + char buf[MAX_LOG_SIZE]; + int rc; + + rc = _output_buf(buf, sizeof(buf), target, subsys, level, file, line, cont, format, ap); + if (rc > 0) + target->output(target, level, buf); } /* Catch internal logging category indexes as well as out-of-bounds indexes. @@ -583,6 +758,11 @@ void osmo_vlogp(int subsys, int level, const char *file, int line, subsys = map_subsys(subsys); +#if !defined(EMBEDDED) + if (!log_cache_check(subsys, level)) + return; +#endif + log_tgt_mutex_lock(); llist_for_each_entry(tar, &osmo_log_target_list, entry) { @@ -632,9 +812,23 @@ void logp2(int subsys, unsigned int level, const char *file, int line, int cont, { va_list ap; + TRACE(LIBOSMOCORE_LOG_START()); va_start(ap, format); osmo_vlogp(subsys, level, file, line, cont, format, ap); va_end(ap); + TRACE(LIBOSMOCORE_LOG_DONE()); +} + +/* This logging function is used as a fallback when the logging framework is + * not is not properly initialized. */ +void logp_stub(const char *file, int line, int cont, const char *format, ...) +{ + va_list ap; + if (!cont) + fprintf(stderr, "%s:%d ", file, line); + va_start(ap, format); + vfprintf(stderr, format, ap); + va_end(ap); } /*! Register a new log target with the logging core @@ -643,6 +837,9 @@ void logp2(int subsys, unsigned int level, const char *file, int line, int cont, void log_add_target(struct log_target *target) { llist_add_tail(&target->entry, &osmo_log_target_list); +#if (!EMBEDDED) + log_cache_update_all(); +#endif } /*! Unregister a log target from the logging core @@ -651,6 +848,9 @@ void log_add_target(struct log_target *target) void log_del_target(struct log_target *target) { llist_del(&target->entry); +#if (!EMBEDDED) + log_cache_update_all(); +#endif } /*! Reset (clear) the logging context */ @@ -727,6 +927,15 @@ void log_set_print_extended_timestamp(struct log_target *target, int print_times target->print_ext_timestamp = print_timestamp; } +/*! Enable or disable printing of timestamps while logging + * \param[in] target Log target to be affected + * \param[in] print_tid Enable (1) or disable (0) Thread ID logging + */ +void log_set_print_tid(struct log_target *target, int print_tid) +{ + target->print_tid = print_tid; +} + /*! Use log_set_print_filename2() instead. * Call log_set_print_filename2() with LOG_FILENAME_PATH or LOG_FILENAME_NONE, *as well as* call * log_set_print_category_hex() with the argument passed to this function. This is to mirror legacy @@ -802,6 +1011,9 @@ void log_set_print_level(struct log_target *target, int print_level) void log_set_log_level(struct log_target *target, int log_level) { target->loglevel = log_level; +#if !defined(EMBEDDED) + log_cache_update_all(); +#endif } /*! Set a category filter on a given log target @@ -818,15 +1030,73 @@ void log_set_category_filter(struct log_target *target, int category, category = map_subsys(category); target->categories[category].enabled = !!enable; target->categories[category].loglevel = level; + +#if !defined(EMBEDDED) + log_cache_update(category, !!enable, level); +#endif } #if (!EMBEDDED) -static void _file_output(struct log_target *target, unsigned int level, +/* write-queue tells us we should write another msgb (log line) to the output fd */ +static int _file_wq_write_cb(struct osmo_fd *ofd, struct msgb *msg) +{ + int rc; + + rc = write(ofd->fd, msgb_data(msg), msgb_length(msg)); + if (rc < 0) + return rc; + if (rc != msgb_length(msg)) { + /* pull the number of bytes we have already written */ + msgb_pull(msg, rc); + /* ask write_queue to re-insert the msgb at the head of the queue */ + return -EAGAIN; + } + return 0; +} + +/* output via buffered, blocking stdio streams */ +static void _file_output_stream(struct log_target *target, unsigned int level, const char *log) { - fprintf(target->tgt_file.out, "%s", log); + OSMO_ASSERT(target->tgt_file.out); + fputs(log, target->tgt_file.out); fflush(target->tgt_file.out); } + +/* output via non-blocking write_queue, doing internal buffering */ +static void _file_raw_output(struct log_target *target, int subsys, unsigned int level, const char *file, + int line, int cont, const char *format, va_list ap) +{ + struct msgb *msg; + int rc; + + OSMO_ASSERT(target->tgt_file.wqueue); + msg = msgb_alloc_c(target->tgt_file.wqueue, MAX_LOG_SIZE, "log_file_msg"); + if (!msg) + return; + + /* we simply enqueue the log message to a write queue here, to avoid any blocking + * writes on the output file. The write queue will tell us once the file is writable + * and call _file_wq_write_cb() */ + rc = _output_buf((char *)msgb_data(msg), msgb_tailroom(msg), target, subsys, level, file, line, cont, format, ap); + msgb_put(msg, rc); + + /* attempt a synchronous, non-blocking write, if the write queue is empty */ + if (target->tgt_file.wqueue->current_length == 0) { + rc = _file_wq_write_cb(&target->tgt_file.wqueue->bfd, msg); + if (rc == 0) { + /* the write was complete, we can exit early */ + msgb_free(msg); + return; + } + } + /* if we reach here, either we already had elements in the write_queue, or the synchronous write + * failed: enqueue the message to the write_queue (backlog) */ + if (osmo_wqueue_enqueue_quiet(target->tgt_file.wqueue, msg) < 0) { + msgb_free(msg); + /* TODO: increment some counter so we can see that messages were dropped */ + } +} #endif /*! Create a new log target skeleton @@ -866,11 +1136,21 @@ struct log_target *log_target_create(void) /* global settings */ target->use_color = 1; target->print_timestamp = 0; + target->print_tid = 0; target->print_filename2 = LOG_FILENAME_PATH; target->print_category_hex = true; /* global log level */ target->loglevel = 0; + +#if !defined(EMBEDDED) + /* update cache */ + for (i = 0; i < osmo_log_info->num_cat; i++) { + const struct log_info_cat *c = &osmo_log_info->cat[i]; + log_cache_update(i, c->enabled, c->loglevel); + } +#endif + return target; } @@ -888,7 +1168,7 @@ struct log_target *log_target_create_stderr(void) target->type = LOG_TGT_TYPE_STDERR; target->tgt_file.out = stderr; - target->output = _file_output; + target->output = _file_output_stream; return target; #else return NULL; @@ -896,11 +1176,11 @@ struct log_target *log_target_create_stderr(void) } #if (!EMBEDDED) -/*! Create a new file-based log target +/*! Create a new file-based log target using buffered, blocking stream output * \param[in] fname File name of the new log file * \returns Log target in case of success, NULL otherwise */ -struct log_target *log_target_create_file(const char *fname) +struct log_target *log_target_create_file_stream(const char *fname) { struct log_target *target; @@ -910,11 +1190,169 @@ struct log_target *log_target_create_file(const char *fname) target->type = LOG_TGT_TYPE_FILE; target->tgt_file.out = fopen(fname, "a"); - if (!target->tgt_file.out) + if (!target->tgt_file.out) { + log_target_destroy(target); return NULL; + } + target->output = _file_output_stream; + target->tgt_file.fname = talloc_strdup(target, fname); + + return target; +} - target->output = _file_output; +/*! switch from non-blocking/write-queue to blocking + buffered stream output + * \param[in] target log target which we should switch + * \return 0 on success; 1 if already switched before; negative on error + * Must be called with mutex osmo_log_tgt_mutex held, see log_tgt_mutex_lock. + */ +int log_target_file_switch_to_stream(struct log_target *target) +{ + struct osmo_wqueue *wq; + if (!target) + return -ENODEV; + + if (target->tgt_file.out) { + /* target has already been switched over */ + return 1; + } + + wq = target->tgt_file.wqueue; + OSMO_ASSERT(wq); + + /* re-open output as stream */ + if (target->type == LOG_TGT_TYPE_STDERR) + target->tgt_file.out = stderr; + else + target->tgt_file.out = fopen(target->tgt_file.fname, "a"); + if (!target->tgt_file.out) { + return -EIO; + } + + /* synchronously write anything left in the queue */ + while (!llist_empty(&wq->msg_queue)) { + struct msgb *msg = msgb_dequeue(&wq->msg_queue); + fwrite(msgb_data(msg), msgb_length(msg), 1, target->tgt_file.out); + msgb_free(msg); + } + + /* now that everything succeeded, we can finally close the old output fd */ + if (target->type == LOG_TGT_TYPE_FILE) { + osmo_fd_unregister(&wq->bfd); + close(wq->bfd.fd); + wq->bfd.fd = -1; + } + + /* release the queue itself */ + talloc_free(wq); + target->tgt_file.wqueue = NULL; + target->output = _file_output_stream; + target->raw_output = NULL; + + return 0; +} + +/*! switch from blocking + buffered file output to non-blocking write-queue based output. + * \param[in] target log target which we should switch + * \return 0 on success; 1 if already switched before; negative on error + * Must be called with mutex osmo_log_tgt_mutex held, see log_tgt_mutex_lock. + */ +int log_target_file_switch_to_wqueue(struct log_target *target) +{ + struct osmo_wqueue *wq; + int rc; + + if (!target) + return -ENODEV; + + if (!target->tgt_file.out) { + /* target has already been switched over */ + return 1; + } + + /* we create a ~640kB sized talloc pool within the write-queue to ensure individual + * log lines (stored as msgbs) will not put result in malloc() calls, and also to + * reduce the OOM probability within logging, as the pool is already allocated */ + wq = talloc_pooled_object(target, struct osmo_wqueue, LOG_WQUEUE_LEN, + LOG_WQUEUE_LEN*(sizeof(struct msgb)+MAX_LOG_SIZE)); + if (!wq) + return -ENOMEM; + osmo_wqueue_init(wq, LOG_WQUEUE_LEN); + + fflush(target->tgt_file.out); + if (target->type == LOG_TGT_TYPE_FILE) { + rc = open(target->tgt_file.fname, O_WRONLY|O_APPEND|O_CREAT|O_NONBLOCK, 0660); + if (rc < 0) { + talloc_free(wq); + return -errno; + } + } else { + rc = STDERR_FILENO; + } + wq->bfd.fd = rc; + wq->bfd.when = OSMO_FD_WRITE; + wq->write_cb = _file_wq_write_cb; + + rc = osmo_fd_register(&wq->bfd); + if (rc < 0) { + talloc_free(wq); + return -EIO; + } + target->tgt_file.wqueue = wq; + target->raw_output = _file_raw_output; + target->output = NULL; + + /* now that everything succeeded, we can finally close the old output stream */ + if (target->type == LOG_TGT_TYPE_FILE) + fclose(target->tgt_file.out); + target->tgt_file.out = NULL; + + return 0; +} + +/*! Create a new file-based log target using non-blocking write_queue + * \param[in] fname File name of the new log file + * \returns Log target in case of success, NULL otherwise + */ +struct log_target *log_target_create_file(const char *fname) +{ + struct log_target *target; + struct osmo_wqueue *wq; + int rc; + + target = log_target_create(); + if (!target) + return NULL; + + target->type = LOG_TGT_TYPE_FILE; + /* we create a ~640kB sized talloc pool within the write-queue to ensure individual + * log lines (stored as msgbs) will not put result in malloc() calls, and also to + * reduce the OOM probability within logging, as the pool is already allocated */ + wq = talloc_pooled_object(target, struct osmo_wqueue, LOG_WQUEUE_LEN, + LOG_WQUEUE_LEN*(sizeof(struct msgb)+MAX_LOG_SIZE)); + if (!wq) { + log_target_destroy(target); + return NULL; + } + osmo_wqueue_init(wq, LOG_WQUEUE_LEN); + wq->bfd.fd = open(fname, O_WRONLY|O_APPEND|O_CREAT|O_NONBLOCK, 0660); + if (wq->bfd.fd < 0) { + talloc_free(wq); + log_target_destroy(target); + return NULL; + } + wq->bfd.when = OSMO_FD_WRITE; + wq->write_cb = _file_wq_write_cb; + + rc = osmo_fd_register(&wq->bfd); + if (rc < 0) { + talloc_free(wq); + log_target_destroy(target); + return NULL; + } + + target->tgt_file.wqueue = wq; + target->raw_output = _file_raw_output; target->tgt_file.fname = talloc_strdup(target, fname); return target; @@ -927,7 +1365,7 @@ struct log_target *log_target_create_file(const char *fname) * \returns Log target (if found), NULL otherwise * Must be called with mutex osmo_log_tgt_mutex held, see log_tgt_mutex_lock. */ -struct log_target *log_target_find(int type, const char *fname) +struct log_target *log_target_find(enum log_target_type type, const char *fname) { struct log_target *tgt; @@ -954,21 +1392,45 @@ struct log_target *log_target_find(int type, const char *fname) * \param[in] target log target to unregister, close and delete */ void log_target_destroy(struct log_target *target) { - /* just in case, to make sure we don't have any references */ log_del_target(target); #if (!EMBEDDED) - if (target->output == &_file_output) { -/* since C89/C99 says stderr is a macro, we can safely do this! */ -#ifdef stderr - /* don't close stderr */ - if (target->tgt_file.out != stderr) -#endif - { - fclose(target->tgt_file.out); + struct osmo_wqueue *wq; + switch (target->type) { + case LOG_TGT_TYPE_FILE: + case LOG_TGT_TYPE_STDERR: + if (target->tgt_file.out) { + if (target->type == LOG_TGT_TYPE_FILE) + fclose(target->tgt_file.out); target->tgt_file.out = NULL; } + wq = target->tgt_file.wqueue; + if (wq) { + if (wq->bfd.fd >= 0) { + osmo_fd_unregister(&wq->bfd); + if (target->type == LOG_TGT_TYPE_FILE) + close(wq->bfd.fd); + wq->bfd.fd = -1; + } + osmo_wqueue_clear(wq); + talloc_free(wq); + target->tgt_file.wqueue = NULL; + } + talloc_free((void *)target->tgt_file.fname); + target->tgt_file.fname = NULL; + break; + case LOG_TGT_TYPE_GSMTAP: + gsmtap_source_free(target->tgt_gsmtap.gsmtap_inst); + break; +#ifdef HAVE_SYSLOG_H + case LOG_TGT_TYPE_SYSLOG: + closelog(); + break; +#endif /* HAVE_SYSLOG_H */ + default: + /* make GCC happy */ + break; } #endif @@ -980,13 +1442,33 @@ void log_target_destroy(struct log_target *target) * \returns 0 in case of success; negative otherwise */ int log_target_file_reopen(struct log_target *target) { - fclose(target->tgt_file.out); - - target->tgt_file.out = fopen(target->tgt_file.fname, "a"); - if (!target->tgt_file.out) - return -errno; + struct osmo_wqueue *wq; + int rc; + + OSMO_ASSERT(target->type == LOG_TGT_TYPE_FILE || target->type == LOG_TGT_TYPE_STDERR); + OSMO_ASSERT(target->tgt_file.out || target->tgt_file.wqueue); + + if (target->tgt_file.out) { + fclose(target->tgt_file.out); + target->tgt_file.out = fopen(target->tgt_file.fname, "a"); + if (!target->tgt_file.out) + return -errno; + } else { + wq = target->tgt_file.wqueue; + if (wq->bfd.fd >= 0) { + osmo_fd_unregister(&wq->bfd); + close(wq->bfd.fd); + wq->bfd.fd = -1; + } - /* we assume target->output already to be set */ + rc = open(target->tgt_file.fname, O_WRONLY|O_APPEND|O_CREAT|O_NONBLOCK, 0660); + if (rc < 0) + return -errno; + wq->bfd.fd = rc; + rc = osmo_fd_register(&wq->bfd); + if (rc < 0) + return rc; + } return 0; } @@ -1016,6 +1498,31 @@ int log_targets_reopen(void) return rc; } +/*! Enable the log level lookup cache to bypass string formatting and other code for log statements which are + * not actually enabled/needed by any existing log target. + * \retruns 0 in case of success, negative -errno in case of error. */ +int log_cache_enable(void) +{ +#if !defined(EMBEDDED) + if (log_level_lookup_cache) + return -EEXIST; + + log_level_lookup_cache = talloc_zero_array(osmo_log_info, uint8_t, osmo_log_info->num_cat); + if (!log_level_lookup_cache) + return -ENOMEM; + + /* copy everything for level lookup cache */ + log_tgt_mutex_lock(); + log_cache_update_all(); + log_tgt_mutex_unlock(); + + return 0; +#else + return -ENOTSUP; +#endif +} + + /*! Initialize the Osmocom logging core * \param[in] inf Information regarding logging categories, could be NULL * \param[in] ctx talloc context for logging allocations @@ -1026,6 +1533,10 @@ int log_targets_reopen(void) int log_init(const struct log_info *inf, void *ctx) { int i; + struct log_info_cat *cat_ptr; + + /* Ensure that log_init is not called multiple times */ + OSMO_ASSERT(tall_log_ctx == NULL) tall_log_ctx = talloc_named_const(ctx, 1, "logging"); if (!tall_log_ctx) @@ -1043,29 +1554,36 @@ int log_init(const struct log_info *inf, void *ctx) osmo_log_info->num_cat += inf->num_cat; } - osmo_log_info->cat = talloc_zero_array(osmo_log_info, - struct log_info_cat, - osmo_log_info->num_cat); - if (!osmo_log_info->cat) { + cat_ptr = talloc_zero_array(osmo_log_info, struct log_info_cat, + osmo_log_info->num_cat); + if (!cat_ptr) { talloc_free(osmo_log_info); osmo_log_info = NULL; return -ENOMEM; } - if (inf) { /* copy over the user part */ + /* copy over the user part and sanitize loglevel */ + if (inf) { for (i = 0; i < inf->num_cat; i++) { - memcpy((struct log_info_cat *) &osmo_log_info->cat[i], - &inf->cat[i], sizeof(struct log_info_cat)); + memcpy(&cat_ptr[i], &inf->cat[i], + sizeof(struct log_info_cat)); + + /* Make sure that the loglevel is set to NOTICE in case + * no loglevel has been preset. */ + if (!cat_ptr[i].loglevel) { + cat_ptr[i].loglevel = LOGL_NOTICE; + } } } /* copy over the library part */ for (i = 0; i < ARRAY_SIZE(internal_cat); i++) { unsigned int cn = osmo_log_info->num_cat_user + i; - memcpy((struct log_info_cat *) &osmo_log_info->cat[cn], - &internal_cat[i], sizeof(struct log_info_cat)); + memcpy(&cat_ptr[cn], &internal_cat[i], sizeof(struct log_info_cat)); } + osmo_log_info->cat = cat_ptr; + return 0; } @@ -1098,6 +1616,11 @@ int log_check_level(int subsys, unsigned int level) subsys = map_subsys(subsys); +#if !defined(EMBEDDED) + if (!log_cache_check(subsys, level)) + return 0; +#endif + /* TODO: The following could/should be cached (update on config) */ log_tgt_mutex_lock(); diff --git a/src/logging_gsmtap.c b/src/core/logging_gsmtap.c index bd642715..7775c27d 100644 --- a/src/logging_gsmtap.c +++ b/src/core/logging_gsmtap.c @@ -21,23 +21,20 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup logging * @{ * \file logging_gsmtap.c */ -#include "../config.h" +#include "config.h" #include <stdarg.h> #include <stdlib.h> #include <stdio.h> #include <string.h> #include <stdbool.h> +#include <unistd.h> #ifdef HAVE_STRINGS_H #include <strings.h> @@ -50,9 +47,12 @@ #include <osmocom/core/logging.h> #include <osmocom/core/timer.h> #include <osmocom/core/byteswap.h> +#include <osmocom/core/thread.h> #define GSMTAP_LOG_MAX_SIZE 4096 +static __thread uint32_t logging_gsmtap_tid; + static void _gsmtap_raw_output(struct log_target *target, int subsys, unsigned int level, const char *file, int line, int cont, const char *format, @@ -65,6 +65,7 @@ static void _gsmtap_raw_output(struct log_target *target, int subsys, struct timeval tv; int rc; const char *file_basename; + unsigned int _level; /* get timestamp ASAP */ osmo_gettimeofday(&tv, NULL); @@ -82,6 +83,9 @@ static void _gsmtap_raw_output(struct log_target *target, int subsys, /* Logging header */ golh = (struct gsmtap_osmocore_log_hdr *) msgb_put(msg, sizeof(*golh)); OSMO_STRLCPY_ARRAY(golh->proc_name, target->tgt_gsmtap.ident); + if (logging_gsmtap_tid == 0) + osmo_store32be((uint32_t)osmo_gettid(), &logging_gsmtap_tid); + golh->pid = logging_gsmtap_tid; if (subsys_name) OSMO_STRLCPY_ARRAY(golh->subsys, subsys_name + 1); else @@ -111,7 +115,11 @@ static void _gsmtap_raw_output(struct log_target *target, int subsys, } msgb_put(msg, rc); + /* Ensure that any error occurring when sending the log message doesn't cause infinite recursion */ + _level = target->loglevel; + target->loglevel = UINT8_MAX; rc = gsmtap_sendmsg(target->tgt_gsmtap.gsmtap_inst, msg); + target->loglevel = _level; if (rc) msgb_free(msg); } diff --git a/src/logging_syslog.c b/src/core/logging_syslog.c index f980689d..1153bdf4 100644 --- a/src/logging_syslog.c +++ b/src/core/logging_syslog.c @@ -16,17 +16,13 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup logging * @{ * \file logging_syslog.c */ -#include "../config.h" +#include "config.h" #ifdef HAVE_SYSLOG_H diff --git a/src/core/logging_systemd.c b/src/core/logging_systemd.c new file mode 100644 index 00000000..2e86feb6 --- /dev/null +++ b/src/core/logging_systemd.c @@ -0,0 +1,117 @@ +/* + * (C) 2020 by Vadim Yanitskiy <axilirator@gmail.com> + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +/*! \addtogroup logging + * @{ + * \file logging_systemd.c */ + +#include <stdio.h> +#include <syslog.h> + +/* Do not use this file as location in sd_journal_print() */ +#define SD_JOURNAL_SUPPRESS_LOCATION + +#include <systemd/sd-journal.h> + +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> + +/* FIXME: copy-pasted from logging_syslog.c */ +static int logp2syslog_level(unsigned int level) +{ + if (level >= LOGL_FATAL) + return LOG_CRIT; + else if (level >= LOGL_ERROR) + return LOG_ERR; + else if (level >= LOGL_NOTICE) + return LOG_NOTICE; + else if (level >= LOGL_INFO) + return LOG_INFO; + else + return LOG_DEBUG; +} + +static void _systemd_output(struct log_target *target, + unsigned int level, const char *log) +{ + /* systemd accepts the same level constants as syslog */ + sd_journal_print(logp2syslog_level(level), "%s", log); +} + +static void _systemd_raw_output(struct log_target *target, int subsys, + unsigned int level, const char *file, + int line, int cont, const char *format, + va_list ap) +{ + char buf[4096]; + int rc; + + rc = vsnprintf(buf, sizeof(buf), format, ap); + if (rc < 0) { + sd_journal_print(LOG_ERR, "vsnprintf() failed to render a message " + "originated from %s:%d (rc=%d)\n", + file, line, rc); + return; + } + + sd_journal_send("CODE_FILE=%s, CODE_LINE=%d", file, line, + "PRIORITY=%d", logp2syslog_level(level), + "OSMO_SUBSYS=%s", log_category_name(subsys), + "OSMO_SUBSYS_HEX=%4.4x", subsys, + "MESSAGE=%s", buf, + NULL); +} + +/*! Create a new logging target for systemd journal logging. + * \param[in] raw whether to offload rendering of the meta information + * (location, category) to systemd-journal. + * \returns Log target in case of success, NULL in case of error. + */ +struct log_target *log_target_create_systemd(bool raw) +{ + struct log_target *target; + + target = log_target_create(); + if (!target) + return NULL; + + target->type = LOG_TGT_TYPE_SYSTEMD; + log_target_systemd_set_raw(target, raw); + + return target; +} + +/*! Change meta information handling of an existing logging target. + * \param[in] target logging target to be modified. + * \param[in] raw whether to offload rendering of the meta information + * (location, category) to systemd-journal. + */ +void log_target_systemd_set_raw(struct log_target *target, bool raw) +{ + target->sd_journal.raw = raw; + if (raw) { + target->raw_output = _systemd_raw_output; + target->output = NULL; + } else { + target->output = _systemd_output; + target->raw_output = NULL; + } +} + +/* @} */ diff --git a/src/loggingrb.c b/src/core/loggingrb.c index 4a80cc8a..2bf7b665 100644 --- a/src/loggingrb.c +++ b/src/core/loggingrb.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup loggingrb diff --git a/src/macaddr.c b/src/core/macaddr.c index de9d07af..3b231fb8 100644 --- a/src/macaddr.c +++ b/src/core/macaddr.c @@ -18,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup utils @@ -34,6 +30,7 @@ #include <string.h> #include <stdlib.h> #include <unistd.h> +#include <errno.h> /*! Parse a MAC address from human-readable notation * This function parses an ethernet MAC address in the commonly-used @@ -77,11 +74,11 @@ int osmo_macaddr_parse(uint8_t *out, const char *in) */ int osmo_get_macaddr(uint8_t *mac_out, const char *dev_name) { - int rc = -1; struct ifaddrs *ifa, *ifaddr; + int rc = -ENODEV; if (getifaddrs(&ifaddr) != 0) - return -1; + return -errno; for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { struct sockaddr_dl *sdl; @@ -102,7 +99,7 @@ int osmo_get_macaddr(uint8_t *mac_out, const char *dev_name) } freeifaddrs(ifaddr); - return 0; + return rc; } #else diff --git a/src/core/mnl.c b/src/core/mnl.c new file mode 100644 index 00000000..d148e1b3 --- /dev/null +++ b/src/core/mnl.c @@ -0,0 +1,111 @@ +/*! \file mnl.c + * + * This code integrates libmnl (minimal netlink library) into the osmocom select + * loop abstraction. It allows other osmocom libraries or application code to + * create netlink sockets and subscribe to netlink events via libmnl. The completion + * handler / callbacks are dispatched via libosmocore select loop handling. + */ + +/* + * (C) 2020 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <osmocom/core/select.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/mnl.h> + +#include <libmnl/libmnl.h> + +#include <errno.h> +#include <string.h> + +/* osmo_fd call-back for when RTNL socket is readable */ +static int osmo_mnl_fd_cb(struct osmo_fd *ofd, unsigned int what) +{ + uint8_t buf[MNL_SOCKET_BUFFER_SIZE]; + struct osmo_mnl *omnl = ofd->data; + int rc; + + if (!(what & OSMO_FD_READ)) + return 0; + + rc = mnl_socket_recvfrom(omnl->mnls, buf, sizeof(buf)); + if (rc <= 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Error in mnl_socket_recvfrom(): %s\n", + strerror(errno)); + return -EIO; + } + + return mnl_cb_run(buf, rc, 0, 0, omnl->mnl_cb, omnl); +} + +/*! create an osmocom-wrapped limnl netlink socket. + * \param[in] ctx talloc context from which to allocate + * \param[in] bus netlink socket bus ID (see NETLINK_* constants) + * \param[in] groups groups of messages to bind/subscribe to + * \param[in] mnl_cb callback function called for each incoming message + * \param[in] priv opaque private user data + * \returns newly-allocated osmo_mnl or NULL in case of error. */ +struct osmo_mnl *osmo_mnl_init(void *ctx, int bus, unsigned int groups, mnl_cb_t mnl_cb, void *priv) +{ + struct osmo_mnl *olm = talloc_zero(ctx, struct osmo_mnl); + + if (!olm) + return NULL; + + olm->priv = priv; + olm->mnl_cb = mnl_cb; + olm->mnls = mnl_socket_open(bus); + if (!olm->mnls) { + LOGP(DLGLOBAL, LOGL_ERROR, "Error creating netlink socket for bus %d: %s\n", + bus, strerror(errno)); + goto out_free; + } + + if (mnl_socket_bind(olm->mnls, groups, MNL_SOCKET_AUTOPID) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Error binding netlink socket for bus %d to groups 0x%x: %s\n", + bus, groups, strerror(errno)); + goto out_close; + } + + osmo_fd_setup(&olm->ofd, mnl_socket_get_fd(olm->mnls), OSMO_FD_READ, osmo_mnl_fd_cb, olm, 0); + + if (osmo_fd_register(&olm->ofd)) { + LOGP(DLGLOBAL, LOGL_ERROR, "Error registering netlinks socket\n"); + goto out_close; + } + + return olm; + +out_close: + mnl_socket_close(olm->mnls); +out_free: + talloc_free(olm); + return NULL; +} + +/*! destroy an existing osmocom-wrapped mnl netlink socket: Unregister + close + free. + * \param[in] omnl osmo_mnl socket previously returned by osmo_mnl_init() */ +void osmo_mnl_destroy(struct osmo_mnl *omnl) +{ + if (!omnl) + return; + + osmo_fd_unregister(&omnl->ofd); + mnl_socket_close(omnl->mnls); + talloc_free(omnl); +} diff --git a/src/msgb.c b/src/core/msgb.c index 4edbdf34..713510c6 100644 --- a/src/msgb.c +++ b/src/core/msgb.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup msgb @@ -64,7 +60,7 @@ #include <osmocom/core/talloc.h> #include <osmocom/core/logging.h> -/*! Allocate a new message buffer from given talloc cotext +/*! Allocate a new message buffer from given talloc context * \param[in] ctx talloc context from which to allocate * \param[in] size Length in octets, including headroom * \param[in] name Human-readable name to be associated with msgb @@ -318,24 +314,29 @@ void *msgb_talloc_ctx_init(void *root_ctx, unsigned int pool_size) return tall_msgb_ctx; } -/*! Copy an msgb. +/*! Copy an msgb with memory reallocation. * - * This function allocates a new msgb, copies the data buffer of msg, - * and adjusts the pointers (incl l1h-l4h) accordingly. The cb part - * is not copied. + * This function allocates a new msgb with new_len size, copies the data buffer of msg, + * and adjusts the pointers (incl l1h-l4h) accordingly. The cb part is not copied. + * \param[in] ctx talloc context on which allocation happens * \param[in] msg The old msgb object - * \param[in] name Human-readable name to be associated with msgb + * \param[in] new_len The length of new msgb object + * \param[in] name Human-readable name to be associated with new msgb */ -struct msgb *msgb_copy_c(const void *ctx, const struct msgb *msg, const char *name) +struct msgb *msgb_copy_resize_c(const void *ctx, const struct msgb *msg, uint16_t new_len, const char *name) { struct msgb *new_msg; - new_msg = msgb_alloc_c(ctx, msg->data_len, name); - if (!new_msg) + if (new_len < msgb_length(msg)) { + LOGP(DLGLOBAL, LOGL_ERROR, + "Data from old msgb (%u bytes) won't fit into new msgb (%u bytes) after reallocation\n", + msgb_length(msg), new_len); return NULL; + } - /* copy data */ - memcpy(new_msg->_data, msg->_data, new_msg->data_len); + new_msg = msgb_alloc_c(ctx, new_len, name); + if (!new_msg) + return NULL; /* copy header */ new_msg->len = msg->len; @@ -343,6 +344,9 @@ struct msgb *msgb_copy_c(const void *ctx, const struct msgb *msg, const char *na new_msg->head += msg->head - msg->_data; new_msg->tail += msg->tail - msg->_data; + /* copy data */ + memcpy(new_msg->data, msg->data, msgb_length(msg)); + if (msg->l1h) new_msg->l1h = new_msg->_data + (msg->l1h - msg->_data); if (msg->l2h) @@ -355,6 +359,32 @@ struct msgb *msgb_copy_c(const void *ctx, const struct msgb *msg, const char *na return new_msg; } +/*! Copy an msgb with memory reallocation. + * + * This function allocates a new msgb with new_len size, copies the data buffer of msg, + * and adjusts the pointers (incl l1h-l4h) accordingly. The cb part is not copied. + * \param[in] msg The old msgb object + * \param[in] name Human-readable name to be associated with new msgb + */ +struct msgb *msgb_copy_resize(const struct msgb *msg, uint16_t new_len, const char *name) +{ + return msgb_copy_resize_c(tall_msgb_ctx, msg, new_len, name); +} + +/*! Copy an msgb. + * + * This function allocates a new msgb, copies the data buffer of msg, + * and adjusts the pointers (incl l1h-l4h) accordingly. The cb part + * is not copied. + * \param[in] ctx talloc context on which allocation happens + * \param[in] msg The old msgb object + * \param[in] name Human-readable name to be associated with msgb + */ +struct msgb *msgb_copy_c(const void *ctx, const struct msgb *msg, const char *name) +{ + return msgb_copy_resize_c(ctx, msg, msg->data_len, name); +} + /*! Copy an msgb. * * This function allocates a new msgb, copies the data buffer of msg, @@ -430,11 +460,11 @@ int msgb_resize_area(struct msgb *msg, uint8_t *area, */ char *msgb_hexdump_buf(char *buf, size_t buf_len, const struct msgb *msg) { - int buf_offs = 0; + unsigned int buf_offs = 0; int nchars; const unsigned char *start = msg->data; const unsigned char *lxhs[4]; - int i; + unsigned int i; lxhs[0] = msg->l1h; lxhs[1] = msg->l2h; diff --git a/src/msgfile.c b/src/core/msgfile.c index 1f11aa60..abb4e7cf 100644 --- a/src/msgfile.c +++ b/src/core/msgfile.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #define _WITH_GETLINE diff --git a/src/core/netdev.c b/src/core/netdev.c new file mode 100644 index 00000000..318134af --- /dev/null +++ b/src/core/netdev.c @@ -0,0 +1,962 @@ + +/* network device (interface) functions. + * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Pau Espin Pedrol <pespin@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include "config.h" + +/*! \addtogroup netdev + * @{ + * network device (interface) convenience functions + * + * \file netdev.c + * + * Example lifecycle use of the API: + * + * struct osmo_sockaddr_str osa_str = {}; + * struct osmo_sockaddr osa = {}; + * + * // Allocate object: + * struct osmo_netdev *netdev = osmo_netdev_alloc(parent_talloc_ctx, name); + * OSMO_ASSERT(netdev); + * + * // Configure object (before registration): + * rc = osmo_netdev_set_netns_name(netdev, "some_netns_name_or_null"); + * rc = osmo_netdev_set_ifindex(netdev, if_nametoindex("eth0")); + * + * // Register object: + * rc = osmo_netdev_register(netdev); + * // The network interface is now being monitored and the network interface + * // can be operated (see below) + * + * // Add a local IPv4 address: + * rc = osmo_sockaddr_str_from_str2(&osa_str, "192.168.200.1"); + * rc = osmo_sockaddr_str_to_sockaddr(&osa_str, &osa.u.sas); + * rc = osmo_netdev_add_addr(netdev, &osa, 24); + * + * // Bring network interface up: + * rc = osmo_netdev_ifupdown(netdev, true); + * + * // Add default route (0.0.0.0/0): + * rc = osmo_sockaddr_str_from_str2(&osa_str, "0.0.0.0"); + * rc = osmo_sockaddr_str_to_sockaddr(&osa_str, &osa.u.sas); + * rc = osmo_netdev_add_route(netdev, &osa, 0, NULL); + * + * // Unregister (can be freed directly too): + * rc = osmo_netdev_unregister(netdev); + * // Free the object: + * osmo_netdev_free(netdev); + */ + +#if (!EMBEDDED) + +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <errno.h> +#include <ifaddrs.h> +#include <sys/types.h> +#include <sys/time.h> +#include <sys/ioctl.h> +#include <sys/socket.h> +#include <sys/stat.h> +#include <sys/time.h> +#include <net/if.h> +#include <net/route.h> + +#if defined(__linux__) +#include <linux/if_link.h> +#include <linux/rtnetlink.h> +#else +#error "Unknown platform!" +#endif + +#include <osmocom/core/utils.h> +#include <osmocom/core/select.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/netns.h> +#include <osmocom/core/netdev.h> + +#if ENABLE_LIBMNL +#include <osmocom/core/mnl.h> +#endif + +#define IFINDEX_UNUSED 0 + +#define LOGNETDEV(netdev, lvl, fmt, args ...) \ + LOGP(DLGLOBAL, lvl, "NETDEV(%s,if=%s/%u,ns=%s): " fmt, \ + (netdev)->name, osmo_netdev_get_dev_name(netdev) ? : "", \ + (netdev)->ifindex, (netdev)->netns_name ? : "", ## args) + +static struct llist_head g_netdev_netns_ctx_list = LLIST_HEAD_INIT(g_netdev_netns_ctx_list); +static struct llist_head g_netdev_list = LLIST_HEAD_INIT(g_netdev_list); + +/* One per netns, shared by all osmo_netdev in a given netns: */ +struct netdev_netns_ctx { + struct llist_head entry; /* entry in g_netdev_netns_ctx_list */ + unsigned int refcount; /* Number of osmo_netdev currently registered on this netns */ + const char *netns_name; /* default netns has empty string "" (never NULL!) */ + int netns_fd; /* FD to the netns with name "netns_name" above */ +#if ENABLE_LIBMNL + struct osmo_mnl *omnl; +#endif +}; + +static struct netdev_netns_ctx *netdev_netns_ctx_alloc(void *ctx, const char *netns_name) +{ + struct netdev_netns_ctx *netns_ctx; + OSMO_ASSERT(netns_name); + + netns_ctx = talloc_zero(ctx, struct netdev_netns_ctx); + if (!netns_ctx) + return NULL; + + netns_ctx->netns_name = talloc_strdup(netns_ctx, netns_name); + netns_ctx->netns_fd = -1; + + llist_add_tail(&netns_ctx->entry, &g_netdev_netns_ctx_list); + return netns_ctx; + +} + +static void netdev_netns_ctx_free(struct netdev_netns_ctx *netns_ctx) +{ + if (!netns_ctx) + return; + + llist_del(&netns_ctx->entry); + +#if ENABLE_LIBMNL + if (netns_ctx->omnl) { + osmo_mnl_destroy(netns_ctx->omnl); + netns_ctx->omnl = NULL; + } +#endif + + if (netns_ctx->netns_fd != -1) { + close(netns_ctx->netns_fd); + netns_ctx->netns_fd = -1; + } + talloc_free(netns_ctx); +} + +#if ENABLE_LIBMNL +static int netdev_netns_ctx_mnl_cb(const struct nlmsghdr *nlh, void *data); +#endif + +static int netdev_netns_ctx_init(struct netdev_netns_ctx *netns_ctx) +{ + struct osmo_netns_switch_state switch_state; + int rc; + + if (netns_ctx->netns_name[0] != '\0') { + LOGP(DLGLOBAL, LOGL_INFO, "Prepare netns: Switch to netns '%s'\n", netns_ctx->netns_name); + netns_ctx->netns_fd = osmo_netns_open_fd(netns_ctx->netns_name); + if (netns_ctx->netns_fd < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Prepare netns: Cannot switch to netns '%s': %s (%d)\n", + netns_ctx->netns_name, strerror(errno), errno); + return netns_ctx->netns_fd; + } + + /* temporarily switch to specified namespace to create netlink socket */ + rc = osmo_netns_switch_enter(netns_ctx->netns_fd, &switch_state); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Prepare netns: Cannot switch to netns '%s': %s (%d)\n", + netns_ctx->netns_name, strerror(errno), errno); + /* netns_ctx->netns_fd will be freed by future call to netdev_netns_ctx_free() */ + return rc; + } + } + +#if ENABLE_LIBMNL + netns_ctx->omnl = osmo_mnl_init(NULL, NETLINK_ROUTE, RTMGRP_LINK, netdev_netns_ctx_mnl_cb, netns_ctx); + rc = (netns_ctx->omnl ? 0 : -EFAULT); +#else + rc = 0; +#endif + + /* switch back to default namespace */ + if (netns_ctx->netns_name[0] != '\0') { + int rc2 = osmo_netns_switch_exit(&switch_state); + if (rc2 < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Prepare netns: Cannot switch back from netns '%s': %s\n", + netns_ctx->netns_name, strerror(errno)); + return rc2; + } + LOGP(DLGLOBAL, LOGL_INFO, "Prepare netns: Back from netns '%s'\n", + netns_ctx->netns_name); + } + return rc; +} + +static struct netdev_netns_ctx *netdev_netns_ctx_find_by_netns_name(const char *netns_name) +{ + struct netdev_netns_ctx *netns_ctx; + + llist_for_each_entry(netns_ctx, &g_netdev_netns_ctx_list, entry) { + if (strcmp(netns_ctx->netns_name, netns_name)) + continue; + return netns_ctx; + } + + return NULL; +} + +static struct netdev_netns_ctx *netdev_netns_ctx_get(const char *netns_name) +{ + struct netdev_netns_ctx *netns_ctx; + int rc; + + OSMO_ASSERT(netns_name); + netns_ctx = netdev_netns_ctx_find_by_netns_name(netns_name); + if (!netns_ctx) { + netns_ctx = netdev_netns_ctx_alloc(NULL, netns_name); + if (!netns_ctx) + return NULL; + rc = netdev_netns_ctx_init(netns_ctx); + if (rc < 0) { + netdev_netns_ctx_free(netns_ctx); + return NULL; + } + } + netns_ctx->refcount++; + return netns_ctx; +} + +static void netdev_netns_ctx_put(struct netdev_netns_ctx *netns_ctx) +{ + OSMO_ASSERT(netns_ctx); + netns_ctx->refcount--; + + if (netns_ctx->refcount == 0) + netdev_netns_ctx_free(netns_ctx); +} + +struct osmo_netdev { + /* entry in g_netdev_list */ + struct llist_head entry; + + /* Pointer to struct shared (refcounted) by all osmo_netdev in the same netns: */ + struct netdev_netns_ctx *netns_ctx; + + /* Name used to identify the osmo_netdev */ + char *name; + + /* ifindex of the network interface (address space is per netns) */ + unsigned int ifindex; + + /* Network interface name. Can change over lifetime of the interface. */ + char *dev_name; + + /* netns name where the netdev interface is created (NULL = default netns) */ + char *netns_name; + + /* API user private data */ + void *priv_data; + + /* Whether the netdev is in operation (managing the netdev interface) */ + bool registered; + + /* Called by netdev each time a new up/down state change is detected. Can be NULL. */ + osmo_netdev_ifupdown_ind_cb_t ifupdown_ind_cb; + + /* Called by netdev each time the registered network interface is renamed by the system. Can be NULL. */ + osmo_netdev_dev_name_chg_cb_t dev_name_chg_cb; + + /* Called by netdev each time the configured MTU changes in registered network interface. Can be NULL. */ + osmo_netdev_mtu_chg_cb_t mtu_chg_cb; + + /* Whether the netdev interface is UP */ + bool if_up; + /* Whether we know the interface updown state (aka if if_up holds information)*/ + bool if_up_known; + + /* The netdev interface MTU size */ + uint32_t if_mtu; + /* Whether we know the interface MTU size (aka if if_mtu holds information)*/ + bool if_mtu_known; +}; + +#define NETDEV_NETNS_ENTER(netdev, switch_state, str_prefix) \ + do { \ + if ((netdev)->netns_name) { \ + LOGNETDEV(netdev, LOGL_DEBUG, str_prefix ": Switch to netns '%s'\n", \ + (netdev)->netns_name); \ + int rc2 = osmo_netns_switch_enter((netdev)->netns_ctx->netns_fd, switch_state); \ + if (rc2 < 0) { \ + LOGNETDEV(netdev, LOGL_ERROR, str_prefix ": Cannot switch to netns '%s': %s (%d)\n", \ + (netdev)->netns_name, strerror(errno), errno); \ + return -EACCES; \ + } \ + } \ + } while (0) + +#define NETDEV_NETNS_EXIT(netdev, switch_state, str_prefix) \ + do { \ + if ((netdev)->netns_name) { \ + int rc2 = osmo_netns_switch_exit(switch_state); \ + if (rc2 < 0) { \ + LOGNETDEV(netdev, LOGL_ERROR, str_prefix ": Cannot switch back from netns '%s': %s\n", \ + (netdev)->netns_name, strerror(errno)); \ + return rc2; \ + } \ + LOGNETDEV(netdev, LOGL_DEBUG, str_prefix ": Back from netns '%s'\n", \ + (netdev)->netns_name); \ + } \ + } while (0) + +#if ENABLE_LIBMNL +/* validate the netlink attributes */ +static int netdev_mnl_data_attr_cb(const struct nlattr *attr, void *data) +{ + const struct nlattr **tb = data; + int type = mnl_attr_get_type(attr); + + if (mnl_attr_type_valid(attr, IFLA_MAX) < 0) + return MNL_CB_OK; + + switch (type) { + case IFLA_ADDRESS: + if (mnl_attr_validate(attr, MNL_TYPE_BINARY) < 0) + return MNL_CB_ERROR; + break; + case IFLA_MTU: + if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0) + return MNL_CB_ERROR; + break; + case IFLA_IFNAME: + if (mnl_attr_validate(attr, MNL_TYPE_STRING) < 0) + return MNL_CB_ERROR; + break; + } + tb[type] = attr; + return MNL_CB_OK; +} + +static void netdev_mnl_check_mtu_change(struct osmo_netdev *netdev, uint32_t mtu) +{ + if (netdev->if_mtu_known && netdev->if_mtu == mtu) + return; + + LOGNETDEV(netdev, LOGL_NOTICE, "MTU changed: %u\n", mtu); + if (netdev->mtu_chg_cb) + netdev->mtu_chg_cb(netdev, mtu); + + netdev->if_mtu_known = true; + netdev->if_mtu = mtu; +} + +static void netdev_mnl_check_link_state_change(struct osmo_netdev *netdev, bool if_up) +{ + if (netdev->if_up_known && netdev->if_up == if_up) + return; + + LOGNETDEV(netdev, LOGL_NOTICE, "Physical link state changed: %s\n", + if_up ? "UP" : "DOWN"); + if (netdev->ifupdown_ind_cb) + netdev->ifupdown_ind_cb(netdev, if_up); + + netdev->if_up_known = true; + netdev->if_up = if_up; +} + +static int netdev_mnl_cb(struct osmo_netdev *netdev, struct ifinfomsg *ifm, struct nlattr **tb) +{ + char ifnamebuf[IF_NAMESIZE]; + const char *ifname = NULL; + bool if_running; + + if (tb[IFLA_IFNAME]) { + ifname = mnl_attr_get_str(tb[IFLA_IFNAME]); + LOGNETDEV(netdev, LOGL_DEBUG, "%s(): ifname=%s\n", __func__, ifname); + } else { + /* Try harder to obtain the ifname. This code path should in + * general not be triggered since usually IFLA_IFNAME is there */ + struct osmo_netns_switch_state switch_state; + NETDEV_NETNS_ENTER(netdev, &switch_state, "if_indextoname"); + ifname = if_indextoname(ifm->ifi_index, ifnamebuf); + NETDEV_NETNS_EXIT(netdev, &switch_state, "if_indextoname"); + } + if (ifname) { + /* Update dev_name if it changed: */ + if (strcmp(netdev->dev_name, ifname) != 0) { + if (netdev->dev_name_chg_cb) + netdev->dev_name_chg_cb(netdev, ifname); + osmo_talloc_replace_string(netdev, &netdev->dev_name, ifname); + } + } + + if (tb[IFLA_MTU]) { + uint32_t mtu = mnl_attr_get_u32(tb[IFLA_MTU]); + LOGNETDEV(netdev, LOGL_DEBUG, "%s(): mtu=%u\n", __func__, mtu); + netdev_mnl_check_mtu_change(netdev, mtu); + } + if (tb[IFLA_ADDRESS]) { + uint8_t *hwaddr = mnl_attr_get_payload(tb[IFLA_ADDRESS]); + uint16_t hwaddr_len = mnl_attr_get_payload_len(tb[IFLA_ADDRESS]); + LOGNETDEV(netdev, LOGL_DEBUG, "%s(): hwaddress=%s\n", + __func__, osmo_hexdump(hwaddr, hwaddr_len)); + } + + if_running = !!(ifm->ifi_flags & IFF_RUNNING); + LOGNETDEV(netdev, LOGL_DEBUG, "%s(): up=%u running=%u\n", + __func__, !!(ifm->ifi_flags & IFF_UP), if_running); + netdev_mnl_check_link_state_change(netdev, if_running); + + return MNL_CB_OK; +} + +static int netdev_netns_ctx_mnl_cb(const struct nlmsghdr *nlh, void *data) +{ + struct osmo_mnl *omnl = data; + struct netdev_netns_ctx *netns_ctx = (struct netdev_netns_ctx *)omnl->priv; + struct nlattr *tb[IFLA_MAX+1] = {}; + struct ifinfomsg *ifm = mnl_nlmsg_get_payload(nlh); + struct osmo_netdev *netdev; + + OSMO_ASSERT(omnl); + OSMO_ASSERT(ifm); + + mnl_attr_parse(nlh, sizeof(*ifm), netdev_mnl_data_attr_cb, tb); + + LOGP(DLGLOBAL, LOGL_DEBUG, + "%s(): index=%d type=%d flags=0x%x family=%d\n", __func__, + ifm->ifi_index, ifm->ifi_type, ifm->ifi_flags, ifm->ifi_family); + + if (ifm->ifi_index == IFINDEX_UNUSED) + return MNL_CB_ERROR; + + /* Find the netdev (if any) using key <netns,ifindex>. + * Different users of the API may have its own osmo_netdev object + * tracking potentially same netif, hence we need to iterate the whole list + * and dispatch to all matches: + */ + bool found_any = false; + llist_for_each_entry(netdev, &g_netdev_list, entry) { + if (!netdev->registered) + continue; + if (netdev->ifindex != ifm->ifi_index) + continue; + if (strcmp(netdev->netns_ctx->netns_name, netns_ctx->netns_name)) + continue; + found_any = true; + netdev_mnl_cb(netdev, ifm, &tb[0]); + } + + if (!found_any) { + LOGP(DLGLOBAL, LOGL_DEBUG, "%s(): device with ifindex %u on netns %s not registered\n", __func__, + ifm->ifi_index, netns_ctx->netns_name); + } + return MNL_CB_OK; +} + +/* Trigger an initial dump of the iface to get link information */ +static int netdev_mnl_request_initial_dump(struct osmo_mnl *omnl, unsigned int if_index) +{ + char buf[MNL_SOCKET_BUFFER_SIZE]; + struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); + struct ifinfomsg *ifm; + + nlh->nlmsg_type = RTM_GETLINK; + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + nlh->nlmsg_seq = time(NULL); + + ifm = mnl_nlmsg_put_extra_header(nlh, sizeof(*ifm)); + ifm->ifi_family = AF_UNSPEC; + ifm->ifi_index = if_index; + + if (mnl_socket_sendto(omnl->mnls, nlh, nlh->nlmsg_len) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "mnl_socket_sendto\n"); + return -EIO; + } + + return 0; +} + +static int netdev_mnl_set_ifupdown(struct osmo_mnl *omnl, unsigned int if_index, + const char *dev_name, bool up) +{ + char buf[MNL_SOCKET_BUFFER_SIZE]; + struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); + struct ifinfomsg *ifm; + unsigned int change = 0; + unsigned int flags = 0; + + if (up) { + change |= IFF_UP; + flags |= IFF_UP; + } else { + change |= IFF_UP; + flags &= ~IFF_UP; + } + + nlh->nlmsg_type = RTM_NEWLINK; + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + nlh->nlmsg_seq = time(NULL); + + ifm = mnl_nlmsg_put_extra_header(nlh, sizeof(*ifm)); + ifm->ifi_family = AF_UNSPEC; + ifm->ifi_change = change; + ifm->ifi_flags = flags; + ifm->ifi_index = if_index; + + if (dev_name) + mnl_attr_put_str(nlh, IFLA_IFNAME, dev_name); + + if (mnl_socket_sendto(omnl->mnls, nlh, nlh->nlmsg_len) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "mnl_socket_sendto\n"); + return -EIO; + } + + return 0; +} + +static int netdev_mnl_add_addr(struct osmo_mnl *omnl, unsigned int if_index, const struct osmo_sockaddr *osa, uint8_t prefix) +{ + char buf[MNL_SOCKET_BUFFER_SIZE]; + struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); + struct ifaddrmsg *ifm; + + nlh->nlmsg_type = RTM_NEWADDR; + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_REPLACE | NLM_F_ACK; + nlh->nlmsg_seq = time(NULL); + + ifm = mnl_nlmsg_put_extra_header(nlh, sizeof(*ifm)); + ifm->ifa_family = osa->u.sa.sa_family; + ifm->ifa_prefixlen = prefix; + ifm->ifa_flags = IFA_F_PERMANENT; + ifm->ifa_scope = RT_SCOPE_UNIVERSE; + ifm->ifa_index = if_index; + + /* + * The exact meaning of IFA_LOCAL and IFA_ADDRESS depend + * on the address family being used and the device type. + * For broadcast devices (like the interfaces we use), + * for IPv4 we specify both and they are used interchangeably. + * For IPv6, only IFA_ADDRESS needs to be set. + */ + switch (osa->u.sa.sa_family) { + case AF_INET: + mnl_attr_put_u32(nlh, IFA_LOCAL, osa->u.sin.sin_addr.s_addr); + mnl_attr_put_u32(nlh, IFA_ADDRESS, osa->u.sin.sin_addr.s_addr); + break; + case AF_INET6: + mnl_attr_put(nlh, IFA_ADDRESS, sizeof(struct in6_addr), &osa->u.sin6.sin6_addr); + break; + default: + return -EINVAL; + } + + if (mnl_socket_sendto(omnl->mnls, nlh, nlh->nlmsg_len) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "mnl_socket_sendto\n"); + return -EIO; + } + + return 0; +} + +static int netdev_mnl_add_route(struct osmo_mnl *omnl, + unsigned int if_index, + const struct osmo_sockaddr *dst_osa, + uint8_t dst_prefix, + const struct osmo_sockaddr *gw_osa) +{ + char buf[MNL_SOCKET_BUFFER_SIZE]; + struct nlmsghdr *nlh = mnl_nlmsg_put_header(buf); + struct rtmsg *rtm; + + nlh->nlmsg_type = RTM_NEWROUTE; + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_ACK; + nlh->nlmsg_seq = time(NULL); + + rtm = mnl_nlmsg_put_extra_header(nlh, sizeof(*rtm)); + rtm->rtm_family = dst_osa->u.sa.sa_family; + rtm->rtm_dst_len = dst_prefix; + rtm->rtm_src_len = 0; + rtm->rtm_tos = 0; + rtm->rtm_protocol = RTPROT_STATIC; + rtm->rtm_table = RT_TABLE_MAIN; + rtm->rtm_type = RTN_UNICAST; + rtm->rtm_scope = gw_osa ? RT_SCOPE_UNIVERSE : RT_SCOPE_LINK; + rtm->rtm_flags = 0; + + switch (dst_osa->u.sa.sa_family) { + case AF_INET: + mnl_attr_put_u32(nlh, RTA_DST, dst_osa->u.sin.sin_addr.s_addr); + break; + case AF_INET6: + mnl_attr_put(nlh, RTA_DST, sizeof(struct in6_addr), &dst_osa->u.sin6.sin6_addr); + break; + default: + return -EINVAL; + } + + mnl_attr_put_u32(nlh, RTA_OIF, if_index); + + if (gw_osa) { + switch (gw_osa->u.sa.sa_family) { + case AF_INET: + mnl_attr_put_u32(nlh, RTA_GATEWAY, gw_osa->u.sin.sin_addr.s_addr); + break; + case AF_INET6: + mnl_attr_put(nlh, RTA_GATEWAY, sizeof(struct in6_addr), &gw_osa->u.sin6.sin6_addr); + break; + default: + return -EINVAL; + } + } + + if (mnl_socket_sendto(omnl->mnls, nlh, nlh->nlmsg_len) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "mnl_socket_sendto\n"); + return -EIO; + } + + return 0; +} +#endif /* if ENABLE_LIBMNL */ + +/*! Allocate a new netdev object. + * \param[in] ctx talloc context to use as a parent when allocating the netdev object + * \param[in] name A name providen to identify the netdev object + * \returns newly allocated netdev object on success; NULL on error + */ +struct osmo_netdev *osmo_netdev_alloc(void *ctx, const char *name) +{ + struct osmo_netdev *netdev; + + netdev = talloc_zero(ctx, struct osmo_netdev); + if (!netdev) + return NULL; + + netdev->name = talloc_strdup(netdev, name); + + llist_add_tail(&netdev->entry, &g_netdev_list); + return netdev; +} + +/*! Free an allocated netdev object. + * \param[in] netdev The netdev object to free + */ +void osmo_netdev_free(struct osmo_netdev *netdev) +{ + if (!netdev) + return; + if (osmo_netdev_is_registered(netdev)) + osmo_netdev_unregister(netdev); + llist_del(&netdev->entry); + talloc_free(netdev); +} + +/*! Start managing the network device referenced by the netdev object. + * \param[in] netdev The netdev object to open + * \returns 0 on success; negative on error + */ +int osmo_netdev_register(struct osmo_netdev *netdev) +{ + char ifnamebuf[IF_NAMESIZE]; + struct osmo_netns_switch_state switch_state; + int rc = 0; + + if (netdev->registered) + return -EALREADY; + + netdev->netns_ctx = netdev_netns_ctx_get(netdev->netns_name ? : ""); + if (!netdev->netns_ctx) + return -EFAULT; + + NETDEV_NETNS_ENTER(netdev, &switch_state, "register"); + + if (!if_indextoname(netdev->ifindex, ifnamebuf)) { + rc = -ENODEV; + goto err_put_exit; + } + osmo_talloc_replace_string(netdev, &netdev->dev_name, ifnamebuf); + +#if ENABLE_LIBMNL + rc = netdev_mnl_request_initial_dump(netdev->netns_ctx->omnl, netdev->ifindex); +#endif + + NETDEV_NETNS_EXIT(netdev, &switch_state, "register"); + + netdev->registered = true; + return rc; + +err_put_exit: + NETDEV_NETNS_EXIT(netdev, &switch_state, "register"); + netdev_netns_ctx_put(netdev->netns_ctx); + return rc; +} + +/*! Unregister the netdev object (stop managing /moniutoring the interface) + * \param[in] netdev The netdev object to close + * \returns 0 on success; negative on error + */ +int osmo_netdev_unregister(struct osmo_netdev *netdev) +{ + if (!netdev->registered) + return -EALREADY; + + netdev->if_up_known = false; + netdev->if_mtu_known = false; + + netdev_netns_ctx_put(netdev->netns_ctx); + netdev->registered = false; + return 0; +} + +/*! Retrieve whether the netdev object is in "registered" state. + * \param[in] netdev The netdev object to check + * \returns true if in state "registered"; false otherwise + */ +bool osmo_netdev_is_registered(struct osmo_netdev *netdev) +{ + return netdev->registered; +} + +/*! Set private user data pointer on the netdev object. + * \param[in] netdev The netdev object where the field is set + */ +void osmo_netdev_set_priv_data(struct osmo_netdev *netdev, void *priv_data) +{ + netdev->priv_data = priv_data; +} + +/*! Get private user data pointer from the netdev object. + * \param[in] netdev The netdev object from where to retrieve the field + * \returns The current value of the priv_data field. + */ +void *osmo_netdev_get_priv_data(struct osmo_netdev *netdev) +{ + return netdev->priv_data; +} + +/*! Set data_ind_cb callback, called when a new packet is received on the network interface. + * \param[in] netdev The netdev object where the field is set + * \param[in] data_ind_cb the user provided function to be called when the link status (UP/DOWN) changes + */ +void osmo_netdev_set_ifupdown_ind_cb(struct osmo_netdev *netdev, osmo_netdev_ifupdown_ind_cb_t ifupdown_ind_cb) +{ + netdev->ifupdown_ind_cb = ifupdown_ind_cb; +} + +/*! Set dev_name_chg_cb callback, called when a change in the network name is detected + * \param[in] netdev The netdev object where the field is set + * \param[in] dev_name_chg_cb the user provided function to be called when a the interface is renamed + */ +void osmo_netdev_set_dev_name_chg_cb(struct osmo_netdev *netdev, osmo_netdev_dev_name_chg_cb_t dev_name_chg_cb) +{ + netdev->dev_name_chg_cb = dev_name_chg_cb; +} + +/*! Set mtu_chg_cb callback, called when a change in the network name is detected + * \param[in] netdev The netdev object where the field is set + * \param[in] mtu_chg_cb the user provided function to be called when the configured MTU at the interface changes + */ +void osmo_netdev_set_mtu_chg_cb(struct osmo_netdev *netdev, osmo_netdev_mtu_chg_cb_t mtu_chg_cb) +{ + netdev->mtu_chg_cb = mtu_chg_cb; +} + +/*! Get name used to identify the netdev object. + * \param[in] netdev The netdev object from where to retrieve the field + * \returns The current value of the name used to identify the netdev object + */ +const char *osmo_netdev_get_name(const struct osmo_netdev *netdev) +{ + return netdev->name; +} + +/*! Set (specify) interface index identifying the network interface to manage + * \param[in] netdev The netdev object where the field is set + * \param[in] ifindex The interface index identifying the interface + * \returns 0 on success; negative on error + * + * The ifindex, together with the netns_name (see + * osmo_netdev_netns_name_set()), form together the key identifiers of a + * network interface to manage. + * This field is used during osmo_netdev_register() time, and hence must be set + * before calling that API, and cannot be changed when the netdev object is in + * "registered" state. + */ +int osmo_netdev_set_ifindex(struct osmo_netdev *netdev, unsigned int ifindex) +{ + if (netdev->registered) + return -EALREADY; + netdev->ifindex = ifindex; + return 0; +} + +/*! Get interface index identifying the interface managed by netdev + * \param[in] netdev The netdev object from where to retrieve the field + * \returns The current value of the configured netdev interface ifindex to use (0 = unset) + */ +unsigned int osmo_netdev_get_ifindex(const struct osmo_netdev *netdev) +{ + return netdev->ifindex; +} + +/*! Set (specify) name of the network namespace where the network interface to manage is located + * \param[in] netdev The netdev object where the field is set + * \param[in] netns_name The network namespace where the network interface is located + * \returns 0 on success; negative on error + * + * The netns_name, together with the ifindex (see + * osmo_netdev_ifindex_set()), form together the key identifiers of a + * network interface to manage. + * This field is used during osmo_netdev_register() time, and hence must be set + * before calling that API, and cannot be changed when the netdev object is in + * "registered" state. + * If left as NULL (default), the management will be done in the current network namespace. + */ +int osmo_netdev_set_netns_name(struct osmo_netdev *netdev, const char *netns_name) +{ + if (netdev->registered) + return -EALREADY; + osmo_talloc_replace_string(netdev, &netdev->netns_name, netns_name); + return 0; +} + +/*! Get name of network namespace used when opening the netdev interface + * \param[in] netdev The netdev object from where to retrieve the field + * \returns The current value of the configured network namespace + */ +const char *osmo_netdev_get_netns_name(const struct osmo_netdev *netdev) +{ + return netdev->netns_name; +} + +/*! Get name used to name the network interface created by the netdev object + * \param[in] netdev The netdev object from where to retrieve the field + * \returns The interface name (or NULL if unknown) + * + * This information is retrieved internally once the netdev object enters the + * "registered" state. Hence, when not registered NULL can be returned. + */ +const char *osmo_netdev_get_dev_name(const struct osmo_netdev *netdev) +{ + return netdev->dev_name; +} + +/*! Bring netdev interface UP or DOWN. + * \param[in] netdev The netdev object managing the netdev interface + * \param[in] ifupdown true to set the interface UP, false to set it DOWN + * \returns 0 on succes; negative on error. + */ +int osmo_netdev_ifupdown(struct osmo_netdev *netdev, bool ifupdown) +{ + struct osmo_netns_switch_state switch_state; + int rc; + + if (!netdev->registered) + return -ENODEV; + + LOGNETDEV(netdev, LOGL_NOTICE, "Bringing dev %s %s\n", + netdev->dev_name, ifupdown ? "UP" : "DOWN"); + + NETDEV_NETNS_ENTER(netdev, &switch_state, "ifupdown"); + +#if ENABLE_LIBMNL + rc = netdev_mnl_set_ifupdown(netdev->netns_ctx->omnl, netdev->ifindex, + netdev->dev_name, ifupdown); +#else + LOGNETDEV(netdev, LOGL_ERROR, "%s: NOT SUPPORTED. Build libosmocore with --enable-libmnl.\n", __func__); + rc = -ENOTSUP; +#endif + + NETDEV_NETNS_EXIT(netdev, &switch_state, "ifupdown"); + + return rc; +} + +/*! Add IP address to netdev interface + * \param[in] netdev The netdev object managing the netdev interface + * \param[in] addr The local address to set on the interface + * \param[in] prefixlen The network prefix of addr + * \returns 0 on succes; negative on error. + */ +int osmo_netdev_add_addr(struct osmo_netdev *netdev, const struct osmo_sockaddr *addr, uint8_t prefixlen) +{ + struct osmo_netns_switch_state switch_state; + char buf[INET6_ADDRSTRLEN]; + int rc; + + if (!netdev->registered) + return -ENODEV; + + LOGNETDEV(netdev, LOGL_NOTICE, "Adding address %s/%u to dev %s\n", + osmo_sockaddr_ntop(&addr->u.sa, buf), prefixlen, netdev->dev_name); + + NETDEV_NETNS_ENTER(netdev, &switch_state, "Add address"); + +#if ENABLE_LIBMNL + rc = netdev_mnl_add_addr(netdev->netns_ctx->omnl, netdev->ifindex, addr, prefixlen); +#else + LOGNETDEV(netdev, LOGL_ERROR, "%s: NOT SUPPORTED. Build libosmocore with --enable-libmnl.\n", __func__); + rc = -ENOTSUP; +#endif + + NETDEV_NETNS_EXIT(netdev, &switch_state, "Add address"); + + return rc; +} + +/*! Add IP route to netdev interface + * \param[in] netdev The netdev object managing the netdev interface + * \param[in] dst_addr The destination address of the route + * \param[in] dst_prefixlen The network prefix of dst_addr + * \param[in] gw_addr The gateway address. Optional, can be NULL. + * \returns 0 on succes; negative on error. + */ +int osmo_netdev_add_route(struct osmo_netdev *netdev, const struct osmo_sockaddr *dst_addr, uint8_t dst_prefixlen, const struct osmo_sockaddr *gw_addr) +{ + struct osmo_netns_switch_state switch_state; + char buf_dst[INET6_ADDRSTRLEN]; + char buf_gw[INET6_ADDRSTRLEN]; + int rc; + + if (!netdev->registered) + return -ENODEV; + + LOGNETDEV(netdev, LOGL_NOTICE, "Adding route %s/%u%s%s dev %s\n", + osmo_sockaddr_ntop(&dst_addr->u.sa, buf_dst), dst_prefixlen, + gw_addr ? " via " : "", + gw_addr ? osmo_sockaddr_ntop(&gw_addr->u.sa, buf_gw) : "", + netdev->dev_name); + + NETDEV_NETNS_ENTER(netdev, &switch_state, "Add route"); + +#if ENABLE_LIBMNL + rc = netdev_mnl_add_route(netdev->netns_ctx->omnl, netdev->ifindex, dst_addr, dst_prefixlen, gw_addr); +#else + LOGNETDEV(netdev, LOGL_ERROR, "%s: NOT SUPPORTED. Build libosmocore with --enable-libmnl.\n", __func__); + rc = -ENOTSUP; +#endif + + NETDEV_NETNS_EXIT(netdev, &switch_state, "Add route"); + + return rc; +} + +#endif /* (!EMBEDDED) */ + +/*! @} */ diff --git a/src/core/netns.c b/src/core/netns.c new file mode 100644 index 00000000..c1d75b1e --- /dev/null +++ b/src/core/netns.c @@ -0,0 +1,208 @@ + +/* Network namespace convenience functions + * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include "config.h" + +/*! \addtogroup netns + * @{ + * Network namespace convenience functions + * + * \file netns.c */ + +#if defined(__linux__) + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include <errno.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <sched.h> +#include <signal.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/socket.h> +#include <sys/mount.h> +#include <sys/param.h> +#include <fcntl.h> +#include <errno.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/netns.h> + +#define NETNS_PREFIX_PATH "/var/run/netns" +#define NETNS_CURRENT_PATH "/proc/self/ns/net" + +/*! Open a file descriptor for the current network namespace. + * \returns fd of the current network namespace on success; negative in case of error + */ +static int netns_open_current_fd(void) +{ + int fd; + /* store the default namespace for later reference */ + if ((fd = open(NETNS_CURRENT_PATH, O_RDONLY)) < 0) + return -errno; + return fd; +} + +/*! switch to a (non-default) namespace, store existing signal mask in oldmask. + * \param[in] nsfd file descriptor representing the namespace to which we shall switch + * \param[out] state caller-provided memory location to which state of previous netns is stored + * \returns 0 on success; negative on error */ +int osmo_netns_switch_enter(int nsfd, struct osmo_netns_switch_state *state) +{ + sigset_t intmask; + int rc; + + state->prev_nsfd = -1; + + if (sigfillset(&intmask) < 0) + return -errno; + if ((rc = sigprocmask(SIG_BLOCK, &intmask, &state->prev_sigmask)) != 0) + return -rc; + state->prev_nsfd = netns_open_current_fd(); + + if (setns(nsfd, CLONE_NEWNET) < 0) { + /* restore old mask if we couldn't switch the netns */ + sigprocmask(SIG_SETMASK, &state->prev_sigmask, NULL); + close(state->prev_nsfd); + state->prev_nsfd = -1; + return -errno; + } + return 0; +} + +/*! switch back to the previous namespace, restoring signal mask. + * \param[in] state information about previous netns, filled by osmo_netns_switch_enter() + * \returns 0 on successs; negative on error */ +int osmo_netns_switch_exit(struct osmo_netns_switch_state *state) +{ + if (state->prev_nsfd < 0) + return -EINVAL; + + int rc; + if (setns(state->prev_nsfd, CLONE_NEWNET) < 0) + return -errno; + + close(state->prev_nsfd); + state->prev_nsfd = -1; + + if ((rc = sigprocmask(SIG_SETMASK, &state->prev_sigmask, NULL)) != 0) + return -rc; + return 0; +} + +static int create_netns(const char *name) +{ + char path[MAXPATHLEN]; + sigset_t intmask, oldmask; + int fd, prev_nsfd; + int rc, rc2; + + /* create /var/run/netns, if it doesn't exist already */ + rc = mkdir(NETNS_PREFIX_PATH, S_IRWXU|S_IRGRP|S_IXGRP|S_IROTH|S_IXOTH); + if (rc < 0 && errno != EEXIST) + return rc; + + /* create /var/run/netns/[name], if it doesn't exist already */ + rc = snprintf(path, sizeof(path), "%s/%s", NETNS_PREFIX_PATH, name); + if (rc >= sizeof(path)) + return -ENAMETOOLONG; + fd = open(path, O_RDONLY|O_CREAT|O_EXCL, 0); + if (fd < 0) + return -errno; + if (close(fd) < 0) + return -errno; + + /* mask off all signals, store old signal mask */ + if (sigfillset(&intmask) < 0) + return -errno; + if ((rc = sigprocmask(SIG_BLOCK, &intmask, &oldmask)) != 0) + return -rc; + + prev_nsfd = netns_open_current_fd(); + if (prev_nsfd < 0) + return prev_nsfd; + + /* create a new network namespace */ + if (unshare(CLONE_NEWNET) < 0) { + rc = -errno; + goto restore_sigmask; + } + if (mount(NETNS_CURRENT_PATH, path, "none", MS_BIND, NULL) < 0) { + rc = -errno; + goto restore_sigmask; + } + + /* switch back to previous namespace */ + if (setns(prev_nsfd, CLONE_NEWNET) < 0) { + rc = -errno; + goto restore_sigmask; + } + +restore_sigmask: + close(prev_nsfd); + + /* restore process mask */ + if ((rc2 = sigprocmask(SIG_SETMASK, &oldmask, NULL)) != 0) + return -rc2; + + /* might have been set above in case mount fails */ + if (rc < 0) + return rc; + + /* finally, open the created namespace file descriptor from previous ns */ + if ((fd = open(path, O_RDONLY)) < 0) + return -errno; + + return fd; +} + +/*! Open a file descriptor for the network namespace with provided name. + * Creates /var/run/netns/ directory if it doesn't exist already. + * \param[in] name Name of the network namespace (in /var/run/netns/) + * \returns File descriptor of network namespace; negative in case of error + */ +int osmo_netns_open_fd(const char *name) +{ + int rc; + int fd; + char path[MAXPATHLEN]; + + /* path = /var/run/netns/[name] */ + rc = snprintf(path, sizeof(path), "%s/%s", NETNS_PREFIX_PATH, name); + if (rc >= sizeof(path)) + return -ENAMETOOLONG; + + /* If netns already exists, simply open it: */ + fd = open(path, O_RDONLY); + if (fd >= 0) + return fd; + + /* The netns doesn't exist yet, let's create it: */ + fd = create_netns(name); + return fd; +} + +#endif /* defined(__linux__) */ + +/*! @} */ diff --git a/src/core/osmo_io.c b/src/core/osmo_io.c new file mode 100644 index 00000000..af096e6e --- /dev/null +++ b/src/core/osmo_io.c @@ -0,0 +1,1013 @@ +/* + * New osmocom async I/O API. + * + * (C) 2022-2024 by Harald Welte <laforge@osmocom.org> + * (C) 2022-2024 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Daniel Willmann <dwillmann@sysmocom.de> + * + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include "../config.h" +#ifndef EMBEDDED + +#include <fcntl.h> +#include <stdio.h> +#include <stdlib.h> +#include <talloc.h> +#include <unistd.h> +#include <string.h> +#include <stdbool.h> +#include <errno.h> + +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> + +#include "osmo_io_internal.h" + +/*! \addtogroup osmo_io + * @{ + * + * \file osmo_io.c */ + +/*! This environment variable can be set to manually set the backend used in osmo_io */ +#define OSMO_IO_BACKEND_ENV "LIBOSMO_IO_BACKEND" + +const struct value_string osmo_io_backend_names[] = { + { OSMO_IO_BACKEND_POLL, "poll" }, + { OSMO_IO_BACKEND_IO_URING, "io_uring" }, + { 0, NULL } +}; + +const struct value_string osmo_iofd_mode_names[] = { + { OSMO_IO_FD_MODE_READ_WRITE, "read/write" }, + { OSMO_IO_FD_MODE_RECVFROM_SENDTO, "recvfrom/sendto" }, + { OSMO_IO_FD_MODE_RECVMSG_SENDMSG, "recvmsg/sendmsg" }, + { 0, NULL } +}; + +static enum osmo_io_backend g_io_backend; + +/* Used by some tests, can't be static */ +struct iofd_backend_ops osmo_iofd_ops; + +#if defined(HAVE_URING) +void osmo_iofd_uring_init(void); +#endif + +/*! initialize osmo_io for the current thread */ +void osmo_iofd_init(void) +{ + switch (g_io_backend) { + case OSMO_IO_BACKEND_POLL: + break; +#if defined(HAVE_URING) + case OSMO_IO_BACKEND_IO_URING: + osmo_iofd_uring_init(); + break; +#endif + default: + OSMO_ASSERT(0); + break; + } +} + +/* ensure main thread always has pre-initialized osmo_io + * priority 103: run after on_dso_load_select */ +static __attribute__((constructor(103))) void on_dso_load_osmo_io(void) +{ + char *backend = getenv(OSMO_IO_BACKEND_ENV); + if (backend == NULL) + backend = OSMO_IO_BACKEND_DEFAULT; + + if (!strcmp("POLL", backend)) { + g_io_backend = OSMO_IO_BACKEND_POLL; + osmo_iofd_ops = iofd_poll_ops; +#if defined(HAVE_URING) + } else if (!strcmp("IO_URING", backend)) { + g_io_backend = OSMO_IO_BACKEND_IO_URING; + osmo_iofd_ops = iofd_uring_ops; +#endif + } else { + fprintf(stderr, "Invalid osmo_io backend requested: \"%s\"\nCheck the environment variable %s\n", backend, OSMO_IO_BACKEND_ENV); + exit(1); + } + + OSMO_ASSERT(osmo_iofd_ops.close); + OSMO_ASSERT(osmo_iofd_ops.register_fd); + OSMO_ASSERT(osmo_iofd_ops.unregister_fd); + OSMO_ASSERT(osmo_iofd_ops.write_enable); + OSMO_ASSERT(osmo_iofd_ops.write_disable); + OSMO_ASSERT(osmo_iofd_ops.read_enable); + OSMO_ASSERT(osmo_iofd_ops.read_disable); + OSMO_ASSERT(osmo_iofd_ops.notify_connected); + + osmo_iofd_init(); +} + +/*! Allocate the msghdr. + * \param[in] iofd the osmo_io file structure + * \param[in] action the action this msg(hdr) is for (read, write, ..) + * \param[in] msg the msg buffer to use. Will allocate a new one if NULL + * \param[in] cmsg_size size (in bytes) of iofd_msghdr.cmsg buffer. Can be 0 if cmsg is not used. + * \returns the newly allocated msghdr or NULL in case of error */ +struct iofd_msghdr *iofd_msghdr_alloc(struct osmo_io_fd *iofd, enum iofd_msg_action action, struct msgb *msg, + size_t cmsg_size) +{ + bool free_msg = false; + struct iofd_msghdr *hdr; + + if (!msg) { + msg = iofd_msgb_alloc(iofd); + if (!msg) + return NULL; + free_msg = true; + } else { + talloc_steal(iofd, msg); + } + + hdr = talloc_zero_size(iofd, sizeof(struct iofd_msghdr) + cmsg_size); + if (!hdr) { + if (free_msg) + talloc_free(msg); + return NULL; + } + + hdr->action = action; + hdr->iofd = iofd; + hdr->msg = msg; + + return hdr; +} + +/*! Free the msghdr. + * \param[in] msghdr the msghdr to free + */ +void iofd_msghdr_free(struct iofd_msghdr *msghdr) +{ + /* msghdr->msg is never owned by msghdr, it will either be freed in the send path or + * or passed on to the read callback which takes ownership. */ + talloc_free(msghdr); +} + +/*! convenience wrapper to call msgb_alloc with parameters from osmo_io_fd */ +struct msgb *iofd_msgb_alloc(struct osmo_io_fd *iofd) +{ + uint16_t headroom = iofd->msgb_alloc.headroom; + + OSMO_ASSERT(iofd->msgb_alloc.size < 0xffff - headroom); + return msgb_alloc_headroom_c(iofd, iofd->msgb_alloc.size + headroom, headroom, "osmo_io_msgb"); +} + +/*! return the pending msgb in iofd or NULL if there is none*/ +struct msgb *iofd_msgb_pending(struct osmo_io_fd *iofd) +{ + struct msgb *msg = NULL; + + msg = iofd->pending; + iofd->pending = NULL; + + return msg; +} + +/*! Return the pending msgb or allocate and return a new one */ +struct msgb *iofd_msgb_pending_or_alloc(struct osmo_io_fd *iofd) +{ + struct msgb *msg = NULL; + + msg = iofd_msgb_pending(iofd); + if (!msg) + msg = iofd_msgb_alloc(iofd); + + return msg; +} + +/*! Enqueue a message to be sent. + * + * Enqueues the message at the back of the queue provided there is enough space. + * \param[in] iofd the file descriptor + * \param[in] msghdr the message to enqueue + * \returns 0 if the message was enqueued succcessfully, + * -ENOSPC if the queue already contains the maximum number of messages + */ +int iofd_txqueue_enqueue(struct osmo_io_fd *iofd, struct iofd_msghdr *msghdr) +{ + if (iofd->tx_queue.current_length >= iofd->tx_queue.max_length) + return -ENOSPC; + + llist_add_tail(&msghdr->list, &iofd->tx_queue.msg_queue); + iofd->tx_queue.current_length++; + + if (iofd->tx_queue.current_length == 1 && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + osmo_iofd_ops.write_enable(iofd); + + return 0; +} + +/*! Enqueue a message at the front. + * + * Used to enqueue a msgb from a partial send again. This function will always + * enqueue the message, even if the maximum number of messages is reached. + * \param[in] iofd the file descriptor + * \param[in] msghdr the message to enqueue + */ +void iofd_txqueue_enqueue_front(struct osmo_io_fd *iofd, struct iofd_msghdr *msghdr) +{ + llist_add(&msghdr->list, &iofd->tx_queue.msg_queue); + iofd->tx_queue.current_length++; + + if (iofd->tx_queue.current_length == 1 && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + osmo_iofd_ops.write_enable(iofd); +} + +/*! Dequeue a message from the front. + * + * \param[in] iofd the file descriptor + * \returns the msghdr from the front of the queue or NULL if the queue is empty + */ +struct iofd_msghdr *iofd_txqueue_dequeue(struct osmo_io_fd *iofd) +{ + struct llist_head *lh; + + if (iofd->tx_queue.current_length == 0) + return NULL; + + lh = iofd->tx_queue.msg_queue.next; + + OSMO_ASSERT(lh); + iofd->tx_queue.current_length--; + llist_del(lh); + + if (iofd->tx_queue.current_length == 0) + osmo_iofd_ops.write_disable(iofd); + + return llist_entry(lh, struct iofd_msghdr, list); +} + +/*! Handle segmentation of the msg. If this function returns *_HANDLE_ONE or MORE then the data in msg will contain + * one complete message. + * If there are bytes left over, *pending_out will point to a msgb with the remaining data. +*/ +static enum iofd_seg_act iofd_handle_segmentation(struct osmo_io_fd *iofd, struct msgb *msg, struct msgb **pending_out) +{ + int extra_len, received_len, expected_len; + struct msgb *msg_pending; + + /* Save the start of message before segmentation_cb (which could change it) */ + uint8_t *data = msg->data; + + received_len = msgb_length(msg); + + if (iofd->io_ops.segmentation_cb2) { + expected_len = iofd->io_ops.segmentation_cb2(iofd, msg); + } else if (iofd->io_ops.segmentation_cb) { + expected_len = iofd->io_ops.segmentation_cb(msg); + } else { + *pending_out = NULL; + return IOFD_SEG_ACT_HANDLE_ONE; + } + + if (expected_len == -EAGAIN) { + goto defer; + } else if (expected_len < 0) { + /* Something is wrong, skip this msgb */ + LOGPIO(iofd, LOGL_ERROR, "segmentation_cb returned error (%d), skipping msg of size %d\n", + expected_len, received_len); + *pending_out = NULL; + msgb_free(msg); + return IOFD_SEG_ACT_DEFER; + } + + extra_len = received_len - expected_len; + /* No segmentation needed, return the whole msgb */ + if (extra_len == 0) { + *pending_out = NULL; + return IOFD_SEG_ACT_HANDLE_ONE; + /* segment is incomplete */ + } else if (extra_len < 0) { + goto defer; + } + + /* msgb contains more than one segment */ + /* Copy the trailing data over */ + msg_pending = iofd_msgb_alloc(iofd); + memcpy(msgb_data(msg_pending), data + expected_len, extra_len); + msgb_put(msg_pending, extra_len); + *pending_out = msg_pending; + + /* Trim the original msgb to size. Don't use msgb_trim because we need to reference + * msg->data from before it might have been modified by the segmentation_cb(). */ + msg->tail = data + expected_len; + msg->len = msg->tail - msg->data; + return IOFD_SEG_ACT_HANDLE_MORE; + +defer: + *pending_out = msg; + return IOFD_SEG_ACT_DEFER; +} + +/*! Restore message boundaries on read() and pass individual messages to the read callback + */ +void iofd_handle_segmented_read(struct osmo_io_fd *iofd, struct msgb *msg, int rc) +{ + int res; + struct msgb *pending = NULL; + + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_READ_WRITE); + + if (rc <= 0) { + iofd->io_ops.read_cb(iofd, rc, msg); + return; + } + + do { + res = iofd_handle_segmentation(iofd, msg, &pending); + if (res != IOFD_SEG_ACT_DEFER || rc < 0) + iofd->io_ops.read_cb(iofd, rc, msg); + if (res == IOFD_SEG_ACT_HANDLE_MORE) + msg = pending; + } while (res == IOFD_SEG_ACT_HANDLE_MORE); + + OSMO_ASSERT(iofd->pending == NULL); + iofd->pending = pending; +} + +/*! completion handler: Internal function called by osmo_io_backend after a given I/O operation has completed + * \param[in] iofd I/O file-descriptor on which I/O has completed + * \param[in] msg message buffer containing data related to completed I/O + * \param[in] rc result code with read size or error (-errno) + * \param[in] hdr serialized msghdr containing state of completed I/O */ +void iofd_handle_recv(struct osmo_io_fd *iofd, struct msgb *msg, int rc, struct iofd_msghdr *hdr) +{ + talloc_steal(iofd->msgb_alloc.ctx, msg); + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + iofd_handle_segmented_read(iofd, msg, rc); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + iofd->io_ops.recvfrom_cb(iofd, rc, msg, &hdr->osa); + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + iofd->io_ops.recvmsg_cb(iofd, rc, msg, &hdr->hdr); + break; + default: + OSMO_ASSERT(false); + break; + } +} + +/*! completion handler: Internal function called by osmo_io_backend after a given I/O operation has completed + * \param[in] iofd I/O file-descriptor on which I/O has completed + * \param[in] rc return value of the I/O operation + * \param[in] msghdr serialized msghdr containing state of completed I/O + */ +void iofd_handle_send_completion(struct osmo_io_fd *iofd, int rc, struct iofd_msghdr *msghdr) +{ + struct msgb *msg = msghdr->msg; + + /* Incomplete write */ + if (rc > 0 && rc < msgb_length(msg)) { + /* Re-enqueue remaining data */ + msgb_pull(msg, rc); + msghdr->iov[0].iov_len = msgb_length(msg); + iofd_txqueue_enqueue_front(iofd, msghdr); + return; + } + + /* Reenqueue the complete msgb */ + if (rc == -EAGAIN) { + iofd_txqueue_enqueue_front(iofd, msghdr); + return; + } + + /* All other failure and success cases are handled here */ + switch (msghdr->action) { + case IOFD_ACT_WRITE: + if (iofd->io_ops.write_cb) + iofd->io_ops.write_cb(iofd, rc, msg); + break; + case IOFD_ACT_SENDTO: + if (iofd->io_ops.sendto_cb) + iofd->io_ops.sendto_cb(iofd, rc, msg, &msghdr->osa); + break; + case IOFD_ACT_SENDMSG: + if (iofd->io_ops.sendmsg_cb) + iofd->io_ops.sendmsg_cb(iofd, rc, msg); + break; + default: + OSMO_ASSERT(0); + } + + msgb_free(msghdr->msg); + iofd_msghdr_free(msghdr); +} + +/* Public functions */ + +/*! Write a message to a file descriptor / connected socket. + * The osmo_io_fd must be using OSMO_IO_FD_MODE_READ_WRITE. + * + * Appends the message to the internal transmit queue for eventual non-blocking + * write to the underlying socket/file descriptor. + * + * If the function returns success (0) it will take ownership of the msgb and + * internally call msgb_free() after the write request completes. + * In case of an error, the msgb needs to be freed by the caller. + * + * \param[in] iofd osmo_io_fd file descriptor to write data to + * \param[in] msg message buffer containing the data to write + * \returns 0 in case of success; a negative value in case of error + */ +int osmo_iofd_write_msgb(struct osmo_io_fd *iofd, struct msgb *msg) +{ + int rc; + + if (OSMO_UNLIKELY(msgb_length(msg) == 0)) { + LOGPIO(iofd, LOGL_ERROR, "Length is 0, rejecting msgb.\n"); + return -EINVAL; + } + + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_READ_WRITE); + + struct iofd_msghdr *msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_WRITE, msg, 0); + if (!msghdr) + return -ENOMEM; + + msghdr->flags = MSG_NOSIGNAL; + msghdr->iov[0].iov_base = msgb_data(msghdr->msg); + msghdr->iov[0].iov_len = msgb_length(msghdr->msg); + msghdr->hdr.msg_iov = &msghdr->iov[0]; + msghdr->hdr.msg_iovlen = 1; + + rc = iofd_txqueue_enqueue(iofd, msghdr); + if (rc < 0) { + iofd_msghdr_free(msghdr); + LOGPIO(iofd, LOGL_ERROR, "enqueueing message failed (%d). Rejecting msgb\n", rc); + return rc; + } + + return 0; +} + +/*! Send a message through an unconnected socket. + * The osmo_io_fd must be using OSMO_IO_FD_MODE_RECVFROM_SENDTO. + * + * Appends the message to the internal transmit queue for eventual non-blocking + * sendto on the underlying socket/file descriptor. + * + * If the function returns success (0), it will take ownership of the msgb and + * internally call msgb_free() after the sendto request completes. + * In case of an error the msgb needs to be freed by the caller. + * + * \param[in] iofd file descriptor to write to + * \param[in] msg message buffer to send + * \param[in] sendto_flags Flags to pass to the send call + * \param[in] dest destination address to send the message to + * \returns 0 in case of success; a negative value in case of error + */ +int osmo_iofd_sendto_msgb(struct osmo_io_fd *iofd, struct msgb *msg, int sendto_flags, const struct osmo_sockaddr *dest) +{ + int rc; + + if (OSMO_UNLIKELY(msgb_length(msg) == 0)) { + LOGPIO(iofd, LOGL_ERROR, "Length is 0, rejecting msgb.\n"); + return -EINVAL; + } + + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_RECVFROM_SENDTO); + + struct iofd_msghdr *msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_SENDTO, msg, 0); + if (!msghdr) + return -ENOMEM; + + if (dest) { + msghdr->osa = *dest; + msghdr->hdr.msg_name = &msghdr->osa.u.sa; + msghdr->hdr.msg_namelen = osmo_sockaddr_size(&msghdr->osa); + } + msghdr->flags = sendto_flags; + msghdr->iov[0].iov_base = msgb_data(msghdr->msg); + msghdr->iov[0].iov_len = msgb_length(msghdr->msg); + msghdr->hdr.msg_iov = &msghdr->iov[0]; + msghdr->hdr.msg_iovlen = 1; + + rc = iofd_txqueue_enqueue(iofd, msghdr); + if (rc < 0) { + iofd_msghdr_free(msghdr); + LOGPIO(iofd, LOGL_ERROR, "enqueueing message failed (%d). Rejecting msgb\n", rc); + return rc; + } + + return 0; +} + +/*! osmo_io equivalent of the sendmsg(2) socket API call. + * The osmo_io_fd must be using OSMO_IO_FD_MODE_RECVMSG_SENDMSG. + * + * Appends the message to the internal transmit queue for eventual non-blocking + * sendmsg on the underlying socket/file descriptor. + * + * If the function returns success (0), it will take ownership of the msgb and + * internally call msgb_free() after the sendmsg request completes. + * In case of an error the msgb needs to be freed by the caller. + * + * \param[in] iofd file descriptor to write to + * \param[in] msg message buffer to send; is used to fill msgh->iov[] + * \param[in] sendmsg_flags Flags to pass to the send call + * \param[in] msgh 'struct msghdr' for name/control/flags. iov must be empty! + * \returns 0 in case of success; a negative value in case of error + */ +int osmo_iofd_sendmsg_msgb(struct osmo_io_fd *iofd, struct msgb *msg, int sendmsg_flags, const struct msghdr *msgh) +{ + int rc; + struct iofd_msghdr *msghdr; + + if (OSMO_UNLIKELY(msgb_length(msg) == 0)) { + LOGPIO(iofd, LOGL_ERROR, "Length is 0, rejecting msgb.\n"); + return -EINVAL; + } + + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_RECVMSG_SENDMSG); + + if (OSMO_UNLIKELY(msgh->msg_namelen > sizeof(msghdr->osa))) { + LOGPIO(iofd, LOGL_ERROR, "osmo_iofd_sendmsg msg_namelen (%u) > supported %zu bytes\n", + msgh->msg_namelen, sizeof(msghdr->osa)); + return -EINVAL; + } + + if (OSMO_UNLIKELY(msgh->msg_iovlen)) { + LOGPIO(iofd, LOGL_ERROR, "osmo_iofd_sendmsg must have all in 'struct msgb', not in 'msg_iov'\n"); + return -EINVAL; + } + + msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_SENDMSG, msg, msgh->msg_controllen); + if (!msghdr) + return -ENOMEM; + + /* copy over optional address */ + if (msgh->msg_name) { + memcpy(&msghdr->osa, msgh->msg_name, msgh->msg_namelen); + msghdr->hdr.msg_name = &msghdr->osa.u.sa; + msghdr->hdr.msg_namelen = msgh->msg_namelen; + } + + /* build iov from msgb */ + msghdr->iov[0].iov_base = msgb_data(msghdr->msg); + msghdr->iov[0].iov_len = msgb_length(msghdr->msg); + msghdr->hdr.msg_iov = &msghdr->iov[0]; + msghdr->hdr.msg_iovlen = 1; + + /* copy over the cmsg from the msghdr */ + if (msgh->msg_control && msgh->msg_controllen) { + msghdr->hdr.msg_control = msghdr->cmsg; + msghdr->hdr.msg_controllen = msgh->msg_controllen; + memcpy(msghdr->cmsg, msgh->msg_control, msgh->msg_controllen); + } + + /* copy over msg_flags */ + msghdr->hdr.msg_flags = sendmsg_flags; + + rc = iofd_txqueue_enqueue(iofd, msghdr); + if (rc < 0) { + iofd_msghdr_free(msghdr); + LOGPIO(iofd, LOGL_ERROR, "enqueueing message failed (%d). Rejecting msgb\n", rc); + return rc; + } + + return 0; +} + +static int check_mode_callback_compat(enum osmo_io_fd_mode mode, const struct osmo_io_ops *ops) +{ + switch (mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + if (ops->recvfrom_cb || ops->sendto_cb) + return false; + if (ops->recvmsg_cb || ops->sendmsg_cb) + return false; + /* Forbid both segementation_cb set, something is wrong: */ + if (ops->segmentation_cb && ops->segmentation_cb2) + return false; + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + if (ops->read_cb || ops->write_cb) + return false; + if (ops->recvmsg_cb || ops->sendmsg_cb) + return false; + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + if (ops->recvfrom_cb || ops->sendto_cb) + return false; + if (ops->read_cb || ops->write_cb) + return false; + break; + default: + break; + } + + return true; +} + +/*! Allocate and setup a new iofd. + * + * Use this to create a new osmo_io_fd, specifying the osmo_io_fd_mode and osmo_io_ops, as well as optionally + * the file-descriptor number and a human-readable name. This is the first function you call for any + * osmo_io_fd. + * + * The created osmo_io_fd is not yet registered, and hence can not be used for any I/O until a subsequent + * call to osmo_iofd_register(). + * + * The created osmo_io_fd is initialized with some default settings: + * * msgb allocations size: OSMO_IO_DEFAULT_MSGB_SIZE (1024) + * * msgb headroom: OSMO_IO_DEFAULT_MSGB_HEADROOM (128) + * * tx_queue depth: 32 + * + * Those values may be adjusted from their defaults by using osmo_iofd_set_alloc_info() and + * osmo_iofd_set_txqueue_max_length() on the osmo_io_fd. + * + * \param[in] ctx the parent context from which to allocate + * \param[in] fd the underlying system file descriptor. May be -1 if not known yet; must then be specified + * at subsequent osmo_iofd_register() time. + * \param[in] name the optional human-readable name of the iofd; may be NULL + * \param[in] mode the osmo_io_fd_mode of the iofd, whether it should use read()/write(), sendto()/recvfrom() + * semantics. + * \param[in] ioops structure specifying the read/write/send/recv callbacks. Will be copied to the iofd, so + * the caller does not have to keep it around after issuing the osmo_iofd_setup call. + * \param[in] data opaque user data pointer accessible by the ioops callbacks + * \returns The newly allocated osmo_io_fd struct or NULL on failure + */ +struct osmo_io_fd *osmo_iofd_setup(const void *ctx, int fd, const char *name, enum osmo_io_fd_mode mode, + const struct osmo_io_ops *ioops, void *data) +{ + struct osmo_io_fd *iofd; + + /* reject unsupported/unknown modes */ + switch (mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + break; + default: + return NULL; + } + + if (ioops && !check_mode_callback_compat(mode, ioops)) { + LOGP(DLIO, LOGL_ERROR, "iofd(%s): rejecting call-backs incompatible with mode %s\n", + name ? name : "unknown", osmo_iofd_mode_name(mode)); + return NULL; + } + + iofd = talloc_zero(ctx, struct osmo_io_fd); + if (!iofd) + return NULL; + + iofd->fd = fd; + iofd->mode = mode; + IOFD_FLAG_SET(iofd, IOFD_FLAG_CLOSED); + + if (name) + iofd->name = talloc_strdup(iofd, name); + + if (ioops) + iofd->io_ops = *ioops; + + iofd->pending = NULL; + + iofd->data = data; + + iofd->msgb_alloc.ctx = ctx; + iofd->msgb_alloc.size = OSMO_IO_DEFAULT_MSGB_SIZE; + iofd->msgb_alloc.headroom = OSMO_IO_DEFAULT_MSGB_HEADROOM; + + iofd->tx_queue.max_length = 32; + INIT_LLIST_HEAD(&iofd->tx_queue.msg_queue); + + return iofd; +} + +/*! Set the size of the control message buffer allocated when submitting recvmsg. + * + * If your osmo_io_fd is in OSMO_IO_FD_MODE_RECVMSG_SENDMSG mode, this API function can be used to tell the + * osmo_io code how much memory should be allocated for the cmsg (control message) buffer when performing + * recvmsg(). */ +int osmo_iofd_set_cmsg_size(struct osmo_io_fd *iofd, size_t cmsg_size) +{ + if (iofd->mode != OSMO_IO_FD_MODE_RECVMSG_SENDMSG) + return -EINVAL; + + iofd->cmsg_size = cmsg_size; + return 0; +} + +/*! Register the osmo_io_fd for active I/O. + * + * Calling this function will register a previously initialized osmo_io_fd for performing I/O. + * + * If the osmo_iofd has a read_cb/recvfrom_cb_recvmsg_cb set in its osmo_io_ops, read/receive will be + * automatically enabled and the respective call-back is called at any time data becomes available. + * + * If there is to-be-transmitted data in the transmit queue, write will be automatically enabled, allowing + * the transmit queue to be drained as soon as the fd/socket becomes writable. + * + * \param[in] iofd the iofd file descriptor + * \param[in] fd the system fd number that will be registered. If you did not yet specify the file descriptor + * number during osmo_fd_setup(), or if it has changed since then, you can state the [new] file descriptor + * number as argument. If you wish to proceed with the previously specified file descriptor number, pass -1. + * \returns zero on success, a negative value on error +*/ +int osmo_iofd_register(struct osmo_io_fd *iofd, int fd) +{ + int rc = 0; + + if (fd >= 0) + iofd->fd = fd; + else if (iofd->fd < 0) { + /* this might happen if both osmo_iofd_setup() and osmo_iofd_register() are called with -1 */ + LOGPIO(iofd, LOGL_ERROR, "Cannot register io_fd using invalid fd == %d\n", iofd->fd); + return -EBADF; + } + + rc = osmo_iofd_ops.register_fd(iofd); + if (rc) + return rc; + + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_CLOSED); + if ((iofd->mode == OSMO_IO_FD_MODE_READ_WRITE && iofd->io_ops.read_cb) || + (iofd->mode == OSMO_IO_FD_MODE_RECVFROM_SENDTO && iofd->io_ops.recvfrom_cb) || + (iofd->mode == OSMO_IO_FD_MODE_RECVMSG_SENDMSG && iofd->io_ops.recvmsg_cb)) { + osmo_iofd_ops.read_enable(iofd); + } + + if (iofd->tx_queue.current_length > 0) + osmo_iofd_ops.write_enable(iofd); + + return rc; +} + +/*! Unregister the given osmo_io_fd from osmo_io. + * + * After an osmo_io_fd has been successfully unregistered, it can no longer perform any I/O via osmo_io. + * However, it can be subsequently re-registered using osmo_iofd_register(). + * + * \param[in] iofd the file descriptor + * \returns zero on success, a negative value on error + */ +int osmo_iofd_unregister(struct osmo_io_fd *iofd) +{ + return osmo_iofd_ops.unregister_fd(iofd); +} + +/*! Retrieve the number of messages pending in the transmit queue. + * + * \param[in] iofd the file descriptor + */ +unsigned int osmo_iofd_txqueue_len(struct osmo_io_fd *iofd) +{ + return iofd->tx_queue.current_length; +} + +/*! Clear the transmit queue of the given osmo_io_fd. + * + * This function frees all messages currently pending in the transmit queue + * \param[in] iofd the file descriptor + */ +void osmo_iofd_txqueue_clear(struct osmo_io_fd *iofd) +{ + struct iofd_msghdr *hdr; + while ((hdr = iofd_txqueue_dequeue(iofd))) { + msgb_free(hdr->msg); + iofd_msghdr_free(hdr); + } +} + +/*! Free the given osmo_io_fd. + * + * The iofd will be automatically closed before via osmo_iofd_close() [which in turn will unregister + * it and clear any pending transmit queue items]. You must not reference the iofd + * after calling this function. However, it is safe to call this function from any of osmo_io + * call-backs; in this case, actual free will be internally delayed until that call-back completes. + * + * \param[in] iofd the file descriptor + */ +void osmo_iofd_free(struct osmo_io_fd *iofd) +{ + if (!iofd) + return; + + osmo_iofd_close(iofd); + + if (!IOFD_FLAG_ISSET(iofd, IOFD_FLAG_IN_CALLBACK)) { + talloc_free(iofd); + } else { + /* Prevent our parent context from freeing us prematurely */ + talloc_steal(NULL, iofd); + IOFD_FLAG_SET(iofd, IOFD_FLAG_TO_FREE); + } +} + +/*! Close the given osmo_io_fd. + * + * This function closes the underlying fd, unregisters it from osmo_io and clears any messages in the tx + * queue. The iofd itself is not freed and can be assigned a new file descriptor with osmo_iofd_register() + * \param[in] iofd the file descriptor + * \returns 0 on success, a negative value otherwise + */ +int osmo_iofd_close(struct osmo_io_fd *iofd) +{ + int rc = 0; + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + return rc; + + IOFD_FLAG_SET(iofd, IOFD_FLAG_CLOSED); + + /* Free pending msgs in tx queue */ + osmo_iofd_txqueue_clear(iofd); + msgb_free(iofd->pending); + + iofd->pending = NULL; + + rc = osmo_iofd_ops.close(iofd); + iofd->fd = -1; + return rc; +} + +/*! Set the size and headroom of the msgb allocated when receiving messages. + * \param[in] iofd the file descriptor + * \param[in] size the size of the msgb when receiving data + * \param[in] headroom the headroom of the msgb when receiving data + */ +void osmo_iofd_set_alloc_info(struct osmo_io_fd *iofd, unsigned int size, unsigned int headroom) +{ + iofd->msgb_alloc.headroom = headroom; + iofd->msgb_alloc.size = size; +} + +/*! Set the maximum number of messages enqueued for sending. + * \param[in] iofd the file descriptor + * \param[in] size the maximum size of the transmit queue + */ +void osmo_iofd_set_txqueue_max_length(struct osmo_io_fd *iofd, unsigned int max_length) +{ + iofd->tx_queue.max_length = max_length; +} + +/*! Retrieve the associated user-data from an osmo_io_fd. + * + * A call to this function will return the opaque user data pointer which was specified previously + * via osmo_iofd_setup() or via osmo_iofd_set_data(). + * + * \param[in] iofd the file descriptor + * \returns the data that was previously set with \ref osmo_iofd_setup() + */ +void *osmo_iofd_get_data(const struct osmo_io_fd *iofd) +{ + return iofd->data; +} + +/*! Set the associated user-data from an osmo_io_fd. + * + * Calling this function will set/overwrite the opaque user data pointer, which can later be retrieved using + * osmo_iofd_get_data(). + * + * \param[in] iofd the file descriptor + * \param[in] data the data to set + */ +void osmo_iofd_set_data(struct osmo_io_fd *iofd, void *data) +{ + iofd->data = data; +} + +/*! Retrieve the private number from an osmo_io_fd. + * Calling this function will retrieve the private user number previously set via osmo_iofd_set_priv_nr(). + * \param[in] iofd the file descriptor + * \returns the private number that was previously set with \ref osmo_iofd_set_priv_nr() + */ +unsigned int osmo_iofd_get_priv_nr(const struct osmo_io_fd *iofd) +{ + return iofd->priv_nr; +} + +/*! Set the private number of an osmo_io_fd. + * The priv_nr passed in via this call can later be retrieved via osmo_iofd_get_priv_nr(). It provides + * a way how additional context can be stored in the osmo_io_fd beyond the opaque 'data' pointer. + * \param[in] iofd the file descriptor + * \param[in] priv_nr the private number to set + */ +void osmo_iofd_set_priv_nr(struct osmo_io_fd *iofd, unsigned int priv_nr) +{ + iofd->priv_nr = priv_nr; +} + +/*! Retrieve the underlying file descriptor from an osmo_io_fd. + * \param[in] iofd the file descriptor + * \returns the underlying file descriptor number */ +int osmo_iofd_get_fd(const struct osmo_io_fd *iofd) +{ + return iofd->fd; +} + +/*! Retrieve the human-readable name of the given osmo_io_fd. + * \param[in] iofd the file descriptor + * \returns the name of the iofd as given in \ref osmo_iofd_setup() */ +const char *osmo_iofd_get_name(const struct osmo_io_fd *iofd) +{ + return iofd->name; +} + +/*! Set the human-readable name of the file descriptor. + * The given name will be used as context by all related logging and future calls to osmo_iofd_get_name(). + * \param[in] iofd the file descriptor + * \param[in] name the name to set on the file descriptor */ +void osmo_iofd_set_name(struct osmo_io_fd *iofd, const char *name) +{ + osmo_talloc_replace_string(iofd, &iofd->name, name); +} + +/*! Set the osmo_io_ops calbacks for an osmo_io_fd. + * This function can be used to update/overwrite the call-back functions for the given osmo_io_fd; it + * replaces the currently-set call-back function pointers from a previous call to osmo_iofd_set_ioops() + * or the original osmo_iofd_setup(). + * \param[in] iofd Target iofd file descriptor + * \param[in] ioops osmo_io_ops structure to be copied to the osmo_io_fd. + * \returns 0 on success, negative on error */ +int osmo_iofd_set_ioops(struct osmo_io_fd *iofd, const struct osmo_io_ops *ioops) +{ + if (!check_mode_callback_compat(iofd->mode, ioops)) { + LOGPIO(iofd, LOGL_ERROR, "rejecting call-backs incompatible with mode %s\n", + osmo_iofd_mode_name(iofd->mode)); + return -EINVAL; + } + + iofd->io_ops = *ioops; + + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + if (iofd->io_ops.read_cb) + osmo_iofd_ops.read_enable(iofd); + else + osmo_iofd_ops.read_disable(iofd); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + if (iofd->io_ops.recvfrom_cb) + osmo_iofd_ops.read_enable(iofd); + else + osmo_iofd_ops.read_disable(iofd); + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + if (iofd->io_ops.recvmsg_cb) + osmo_iofd_ops.read_enable(iofd); + else + osmo_iofd_ops.read_disable(iofd); + break; + default: + OSMO_ASSERT(0); + } + + return 0; +} + +/*! Retrieve the osmo_io_ops for an iofd. + * \param[in] iofd Target iofd file descriptor + * \param[in] ioops caller-allocated osmo_io_ops structure to be filled */ +void osmo_iofd_get_ioops(struct osmo_io_fd *iofd, struct osmo_io_ops *ioops) +{ + *ioops = iofd->io_ops; +} + +/*! Request notification of the user if/when a client socket is connected. + * Calling this function will request osmo_io to notify the user (via + * write call-back) once a non-blocking outbound connect() of the + * socket completes. + * + * This only works for connection oriented sockets in either + * OSMO_IO_FD_MODE_READ_WRITE or OSMO_IO_FD_MODE_RECVMSG_SENDMSG mode. + * + * \param[in] iofd the file descriptor */ +void osmo_iofd_notify_connected(struct osmo_io_fd *iofd) +{ + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_READ_WRITE || + iofd->mode == OSMO_IO_FD_MODE_RECVMSG_SENDMSG); + osmo_iofd_ops.notify_connected(iofd); +} + +/*! @} */ + +#endif /* ifndef(EMBEDDED) */ diff --git a/src/core/osmo_io_internal.h b/src/core/osmo_io_internal.h new file mode 100644 index 00000000..a4b0749d --- /dev/null +++ b/src/core/osmo_io_internal.h @@ -0,0 +1,166 @@ +/*! \file osmo_io_internal.h */ + +#pragma once + +#include <unistd.h> +#include <stdbool.h> +#include <netinet/sctp.h> + +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> + +#include "../config.h" + +#define OSMO_IO_DEFAULT_MSGB_SIZE 1024 +#define OSMO_IO_DEFAULT_MSGB_HEADROOM 128 + +extern const struct iofd_backend_ops iofd_poll_ops; +#define OSMO_IO_BACKEND_DEFAULT "POLL" + +#if defined(HAVE_URING) +extern const struct iofd_backend_ops iofd_uring_ops; +#endif + +struct iofd_backend_ops { + int (*register_fd)(struct osmo_io_fd *iofd); + int (*unregister_fd)(struct osmo_io_fd *iofd); + int (*close)(struct osmo_io_fd *iofd); + void (*write_enable)(struct osmo_io_fd *iofd); + void (*write_disable)(struct osmo_io_fd *iofd); + void (*read_enable)(struct osmo_io_fd *iofd); + void (*read_disable)(struct osmo_io_fd *iofd); + void (*notify_connected)(struct osmo_io_fd *iofd); +}; + +#define IOFD_FLAG_CLOSED (1<<0) +#define IOFD_FLAG_IN_CALLBACK (1<<1) +#define IOFD_FLAG_TO_FREE (1<<2) +#define IOFD_FLAG_NOTIFY_CONNECTED (1<<3) +#define IOFD_FLAG_FD_REGISTERED (1<<4) + +#define IOFD_FLAG_SET(iofd, flag) \ + (iofd)->flags |= (flag) + +#define IOFD_FLAG_UNSET(iofd, flag) \ + (iofd)->flags &= ~(flag) + +#define IOFD_FLAG_ISSET(iofd, flag) ((iofd)->flags & (flag)) + +struct osmo_io_fd { + /*! linked list for internal management */ + struct llist_head list; + /*! actual operating-system level file decriptor */ + int fd; + /*! type of read/write mode to use */ + enum osmo_io_fd_mode mode; + + /*! flags to guard closing/freeing of iofd */ + uint32_t flags; + + /*! human-readable name to associte with fd */ + char *name; + + /*! send/recv (msg) callback functions */ + struct osmo_io_ops io_ops; + /*! Pending msgb to keep partial data during segmentation */ + struct msgb *pending; + + /*! data pointer passed through to call-back function */ + void *data; + /*! private number, extending \a data */ + unsigned int priv_nr; + + /*! size of iofd_msghdr.cmsg[] when allocated in recvmsg path */ + size_t cmsg_size; + + struct { + /*! talloc context from which to allocate msgb when reading */ + const void *ctx; + /*! size of msgb to allocate (excluding headroom) */ + unsigned int size; + /*! headroom to allocate when allocating msgb's */ + unsigned int headroom; + } msgb_alloc; + + struct { + /*! maximum length of write queue */ + unsigned int max_length; + /*! current length of write queue */ + unsigned int current_length; + /*! actual linked list implementing the transmit queue */ + struct llist_head msg_queue; + } tx_queue; + + union { + struct { + struct osmo_fd ofd; + } poll; + struct { + bool read_enabled; + bool write_enabled; + void *read_msghdr; + void *write_msghdr; + /* TODO: index into array of registered fd's? */ + /* osmo_fd for non-blocking connect handling */ + struct osmo_fd connect_ofd; + } uring; + } u; +}; + +enum iofd_msg_action { + IOFD_ACT_READ, + IOFD_ACT_WRITE, + IOFD_ACT_RECVFROM, + IOFD_ACT_SENDTO, + IOFD_ACT_RECVMSG, + IOFD_ACT_SENDMSG, +}; + + +/*! serialized version of 'struct msghdr' employed by sendmsg/recvmsg */ +struct iofd_msghdr { + /*! entry into osmo_io_fd.tx_queue.msg_queue */ + struct llist_head list; + enum iofd_msg_action action; + /*! the 'struct msghdr' we are wrapping/ecapsulating here */ + struct msghdr hdr; + /*! socket address of the remote peer */ + struct osmo_sockaddr osa; + /*! io-vector we need to pass as argument to sendmsg/recvmsg; is set up + * to point into msg below */ + struct iovec iov[1]; + /*! flags we pass as argument to sendmsg / recvmsg */ + int flags; + + /*! message-buffer containing data for this I/O operation */ + struct msgb *msg; + /*! I/O file descriptor on which we perform this I/O operation */ + struct osmo_io_fd *iofd; + + /*! control message buffer for passing sctp_sndrcvinfo along */ + char cmsg[0]; /* size is determined by iofd->cmsg_size on recvmsg, and by mcghdr->msg_controllen on sendmsg */ +}; + +enum iofd_seg_act { + IOFD_SEG_ACT_HANDLE_ONE, + IOFD_SEG_ACT_HANDLE_MORE, + IOFD_SEG_ACT_DEFER, +}; + +struct iofd_msghdr *iofd_msghdr_alloc(struct osmo_io_fd *iofd, enum iofd_msg_action action, struct msgb *msg, size_t cmsg_size); +void iofd_msghdr_free(struct iofd_msghdr *msghdr); + +struct msgb *iofd_msgb_alloc(struct osmo_io_fd *iofd); +struct msgb *iofd_msgb_pending(struct osmo_io_fd *iofd); +struct msgb *iofd_msgb_pending_or_alloc(struct osmo_io_fd *iofd); + +void iofd_handle_recv(struct osmo_io_fd *iofd, struct msgb *msg, int rc, struct iofd_msghdr *msghdr); +void iofd_handle_send_completion(struct osmo_io_fd *iofd, int rc, struct iofd_msghdr *msghdr); +void iofd_handle_segmented_read(struct osmo_io_fd *iofd, struct msgb *msg, int rc); + +int iofd_txqueue_enqueue(struct osmo_io_fd *iofd, struct iofd_msghdr *msghdr); +void iofd_txqueue_enqueue_front(struct osmo_io_fd *iofd, struct iofd_msghdr *msghdr); +struct iofd_msghdr *iofd_txqueue_dequeue(struct osmo_io_fd *iofd); diff --git a/src/core/osmo_io_poll.c b/src/core/osmo_io_poll.c new file mode 100644 index 00000000..c4bb376d --- /dev/null +++ b/src/core/osmo_io_poll.c @@ -0,0 +1,201 @@ +/*! \file osmo_io_poll.c + * New osmocom async I/O API. + * + * (C) 2022 by Harald Welte <laforge@osmocom.org> + * (C) 2022-2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Daniel Willmann <dwillmann@sysmocom.de> + * + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include "../config.h" +#ifndef EMBEDDED + +#include <errno.h> +#include <stdio.h> +#include <talloc.h> +#include <unistd.h> +#include <stdbool.h> +#include <sys/socket.h> + +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> + +#include "osmo_io_internal.h" + +static void iofd_poll_ofd_cb_recvmsg_sendmsg(struct osmo_fd *ofd, unsigned int what) +{ + struct osmo_io_fd *iofd = ofd->data; + struct msgb *msg; + int rc, flags = 0; + + if (what & OSMO_FD_READ) { + struct iofd_msghdr hdr; + + msg = iofd_msgb_pending_or_alloc(iofd); + if (!msg) { + LOGPIO(iofd, LOGL_ERROR, "Could not allocate msgb for reading\n"); + OSMO_ASSERT(0); + } + + hdr.msg = msg; + hdr.iov[0].iov_base = msg->tail; + hdr.iov[0].iov_len = msgb_tailroom(msg); + hdr.hdr = (struct msghdr) { + .msg_iov = &hdr.iov[0], + .msg_iovlen = 1, + .msg_name = &hdr.osa.u.sa, + .msg_namelen = sizeof(struct osmo_sockaddr), + }; + if (iofd->mode == OSMO_IO_FD_MODE_RECVMSG_SENDMSG) { + hdr.hdr.msg_control = alloca(iofd->cmsg_size); + hdr.hdr.msg_controllen = iofd->cmsg_size; + } + + rc = recvmsg(ofd->fd, &hdr.hdr, flags); + if (rc > 0) + msgb_put(msg, rc); + + iofd_handle_recv(iofd, msg, (rc < 0 && errno > 0) ? -errno : rc, &hdr); + } + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + return; + + if (what & OSMO_FD_WRITE) { + struct iofd_msghdr *msghdr = iofd_txqueue_dequeue(iofd); + if (msghdr) { + rc = sendmsg(ofd->fd, &msghdr->hdr, msghdr->flags); + iofd_handle_send_completion(iofd, (rc < 0 && errno > 0) ? -errno : rc, msghdr); + } else { + /* Socket is writable, but we have no data to send. A non-blocking/async + connect() is signalled this way. */ + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + iofd->io_ops.write_cb(iofd, 0, NULL); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + iofd->io_ops.sendto_cb(iofd, 0, NULL, NULL); + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + iofd->io_ops.sendmsg_cb(iofd, 0, NULL); + break; + default: + break; + } + if (osmo_iofd_txqueue_len(iofd) == 0) + iofd_poll_ops.write_disable(iofd); + } + } +} + +static int iofd_poll_ofd_cb_dispatch(struct osmo_fd *ofd, unsigned int what) +{ + struct osmo_io_fd *iofd = ofd->data; + + IOFD_FLAG_SET(iofd, IOFD_FLAG_IN_CALLBACK); + iofd_poll_ofd_cb_recvmsg_sendmsg(ofd, what); + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_IN_CALLBACK); + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_TO_FREE)) { + talloc_free(iofd); + return 0; + } + + return 0; +} + +static int iofd_poll_register(struct osmo_io_fd *iofd) +{ + struct osmo_fd *ofd = &iofd->u.poll.ofd; + int rc; + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_FD_REGISTERED)) + return 0; + osmo_fd_setup(ofd, iofd->fd, 0, &iofd_poll_ofd_cb_dispatch, iofd, 0); + rc = osmo_fd_register(ofd); + if (!rc) + IOFD_FLAG_SET(iofd, IOFD_FLAG_FD_REGISTERED); + return rc; +} + +static int iofd_poll_unregister(struct osmo_io_fd *iofd) +{ + struct osmo_fd *ofd = &iofd->u.poll.ofd; + + if (!IOFD_FLAG_ISSET(iofd, IOFD_FLAG_FD_REGISTERED)) + return 0; + osmo_fd_unregister(ofd); + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_FD_REGISTERED); + + return 0; +} + +static int iofd_poll_close(struct osmo_io_fd *iofd) +{ + iofd_poll_unregister(iofd); + osmo_fd_close(&iofd->u.poll.ofd); + + return 0; +} + +static void iofd_poll_read_enable(struct osmo_io_fd *iofd) +{ + osmo_fd_read_enable(&iofd->u.poll.ofd); +} + +static void iofd_poll_read_disable(struct osmo_io_fd *iofd) +{ + osmo_fd_read_disable(&iofd->u.poll.ofd); +} + +static void iofd_poll_write_enable(struct osmo_io_fd *iofd) +{ + osmo_fd_write_enable(&iofd->u.poll.ofd); +} + +static void iofd_poll_write_disable(struct osmo_io_fd *iofd) +{ + osmo_fd_write_disable(&iofd->u.poll.ofd); +} + +static void iofd_poll_notify_connected(struct osmo_io_fd *iofd) +{ + int rc; + + rc = iofd_poll_register(iofd); + if (rc < 0) + return; + osmo_fd_write_enable(&iofd->u.poll.ofd); +} + +const struct iofd_backend_ops iofd_poll_ops = { + .register_fd = iofd_poll_register, + .unregister_fd = iofd_poll_unregister, + .close = iofd_poll_close, + .write_enable = iofd_poll_write_enable, + .write_disable = iofd_poll_write_disable, + .read_enable = iofd_poll_read_enable, + .read_disable = iofd_poll_read_disable, + .notify_connected = iofd_poll_notify_connected, +}; + +#endif /* ifndef EMBEDDED */ diff --git a/src/core/osmo_io_uring.c b/src/core/osmo_io_uring.c new file mode 100644 index 00000000..569f1505 --- /dev/null +++ b/src/core/osmo_io_uring.c @@ -0,0 +1,532 @@ +/*! \file osmo_io_uring.c + * io_uring backend for osmo_io. + * + * (C) 2022-2023 by sysmocom s.f.m.c. + * Author: Daniel Willmann <daniel@sysmocom.de> + * (C) 2023-2024 by Harald Welte <laforge@osmocom.org> + * + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +/* TODO: + * Parameters: + * - number of simultaneous read/write in uring for given fd + * + */ + +#include "../config.h" +#if defined(__linux__) + +#include <stdio.h> +#include <talloc.h> +#include <unistd.h> +#include <string.h> +#include <stdbool.h> +#include <errno.h> + +#include <netinet/in.h> +#include <netinet/sctp.h> +#include <sys/eventfd.h> +#include <liburing.h> + +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/select.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/socket.h> + +#include "osmo_io_internal.h" + +#define IOFD_URING_ENTRIES 4096 + +struct osmo_io_uring { + struct osmo_fd event_ofd; + struct io_uring ring; +}; + +static __thread struct osmo_io_uring g_ring; + +static void iofd_uring_cqe(struct io_uring *ring); + +/*! read call-back for eventfd notifying us if entries are in the completion queue */ +static int iofd_uring_poll_cb(struct osmo_fd *ofd, unsigned int what) +{ + struct io_uring *ring = ofd->data; + eventfd_t val; + int rc; + + if (what & OSMO_FD_READ) { + rc = eventfd_read(ofd->fd, &val); + if (rc < 0) { + LOGP(DLIO, LOGL_ERROR, "eventfd_read() returned error\n"); + return rc; + } + + iofd_uring_cqe(ring); + } + if (what & OSMO_FD_WRITE) + OSMO_ASSERT(0); + + return 0; +} + +/*! initialize the uring and tie it into our event loop */ +void osmo_iofd_uring_init(void) +{ + int rc, evfd; + + rc = io_uring_queue_init(IOFD_URING_ENTRIES, &g_ring.ring, 0); + if (rc < 0) + osmo_panic("failure during io_uring_queue_init(): %s\n", strerror(-rc)); + + rc = eventfd(0, 0); + if (rc < 0) { + io_uring_queue_exit(&g_ring.ring); + osmo_panic("failure creating eventfd(0, 0) for io_uring: %s\n", strerror(-rc)); + } + evfd = rc; + + osmo_fd_setup(&g_ring.event_ofd, evfd, OSMO_FD_READ, iofd_uring_poll_cb, &g_ring.ring, 0); + rc = osmo_fd_register(&g_ring.event_ofd); + if (rc < 0) { + close(evfd); + io_uring_queue_exit(&g_ring.ring); + osmo_panic("failure registering io_uring-eventfd as osmo_fd: %d\n", rc); + } + rc = io_uring_register_eventfd(&g_ring.ring, evfd); + if (rc < 0) { + osmo_fd_unregister(&g_ring.event_ofd); + close(evfd); + io_uring_queue_exit(&g_ring.ring); + osmo_panic("failure registering eventfd with io_uring: %s\n", strerror(-rc)); + } +} + + +static void iofd_uring_submit_recv(struct osmo_io_fd *iofd, enum iofd_msg_action action) +{ + struct msgb *msg; + struct iofd_msghdr *msghdr; + struct io_uring_sqe *sqe; + + msg = iofd_msgb_pending_or_alloc(iofd); + if (!msg) { + LOGPIO(iofd, LOGL_ERROR, "Could not allocate msgb for reading\n"); + OSMO_ASSERT(0); + } + + msghdr = iofd_msghdr_alloc(iofd, action, msg, iofd->cmsg_size); + if (!msghdr) { + LOGPIO(iofd, LOGL_ERROR, "Could not allocate msghdr for reading\n"); + OSMO_ASSERT(0); + } + + msghdr->iov[0].iov_base = msg->tail; + msghdr->iov[0].iov_len = msgb_tailroom(msg); + + switch (action) { + case IOFD_ACT_READ: + break; + case IOFD_ACT_RECVMSG: + msghdr->hdr.msg_control = msghdr->cmsg; + msghdr->hdr.msg_controllen = iofd->cmsg_size; + /* fall-through */ + case IOFD_ACT_RECVFROM: + msghdr->hdr.msg_iov = &msghdr->iov[0]; + msghdr->hdr.msg_iovlen = 1; + msghdr->hdr.msg_name = &msghdr->osa.u.sa; + msghdr->hdr.msg_namelen = osmo_sockaddr_size(&msghdr->osa); + break; + default: + OSMO_ASSERT(0); + } + + sqe = io_uring_get_sqe(&g_ring.ring); + if (!sqe) { + LOGPIO(iofd, LOGL_ERROR, "Could not get io_uring_sqe\n"); + OSMO_ASSERT(0); + } + + switch (action) { + case IOFD_ACT_READ: + io_uring_prep_readv(sqe, iofd->fd, msghdr->iov, 1, 0); + break; + case IOFD_ACT_RECVMSG: + case IOFD_ACT_RECVFROM: + io_uring_prep_recvmsg(sqe, iofd->fd, &msghdr->hdr, msghdr->flags); + break; + default: + OSMO_ASSERT(0); + } + io_uring_sqe_set_data(sqe, msghdr); + + io_uring_submit(&g_ring.ring); + /* NOTE: This only works if we have one read per fd */ + iofd->u.uring.read_msghdr = msghdr; +} + +/*! completion call-back for READ/RECVFROM */ +static void iofd_uring_handle_recv(struct iofd_msghdr *msghdr, int rc) +{ + struct osmo_io_fd *iofd = msghdr->iofd; + struct msgb *msg = msghdr->msg; + + if (rc > 0) + msgb_put(msg, rc); + + if (!IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_handle_recv(iofd, msg, rc, msghdr); + + if (iofd->u.uring.read_enabled && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_uring_submit_recv(iofd, msghdr->action); + else + iofd->u.uring.read_msghdr = NULL; + + + iofd_msghdr_free(msghdr); +} + +static int iofd_uring_submit_tx(struct osmo_io_fd *iofd); + +/*! completion call-back for WRITE/SENDTO */ +static void iofd_uring_handle_tx(struct iofd_msghdr *msghdr, int rc) +{ + struct osmo_io_fd *iofd = msghdr->iofd; + + /* Detach msghdr from iofd. It might get freed here or it is freed during iofd_handle_send_completion(). + * If there is pending data to send, iofd_uring_submit_tx() will attach it again. + * iofd_handle_send_completion() will invoke a callback function to signal the possibility of write/send. + * This callback function might close iofd, leading to the potential freeing of iofd->u.uring.write_msghdr if + * still attached. Since iofd_handle_send_completion() frees msghdr at the end of the function, detaching + * msghdr here prevents a double-free bug. */ + if (iofd->u.uring.write_msghdr == msghdr) + iofd->u.uring.write_msghdr = NULL; + + if (OSMO_UNLIKELY(IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED))) { + msgb_free(msghdr->msg); + iofd_msghdr_free(msghdr); + } else { + iofd_handle_send_completion(iofd, rc, msghdr); + } + + /* submit the next to-be-transmitted message for this file descriptor */ + if (iofd->u.uring.write_enabled && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_uring_submit_tx(iofd); +} + +/*! handle completion of a single I/O message */ +static void iofd_uring_handle_completion(struct iofd_msghdr *msghdr, int res) +{ + struct osmo_io_fd *iofd = msghdr->iofd; + + IOFD_FLAG_SET(iofd, IOFD_FLAG_IN_CALLBACK); + + switch (msghdr->action) { + case IOFD_ACT_READ: + case IOFD_ACT_RECVFROM: + case IOFD_ACT_RECVMSG: + iofd_uring_handle_recv(msghdr, res); + break; + case IOFD_ACT_WRITE: + case IOFD_ACT_SENDTO: + case IOFD_ACT_SENDMSG: + iofd_uring_handle_tx(msghdr, res); + break; + default: + OSMO_ASSERT(0) + } + + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_IN_CALLBACK); + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_TO_FREE) && !iofd->u.uring.read_msghdr && !iofd->u.uring.write_msghdr) + talloc_free(iofd); +} + +/*! process all pending completion queue entries in given io_uring */ +static void iofd_uring_cqe(struct io_uring *ring) +{ + int rc; + struct io_uring_cqe *cqe; + struct iofd_msghdr *msghdr; + + while (io_uring_peek_cqe(ring, &cqe) == 0) { + + msghdr = io_uring_cqe_get_data(cqe); + if (!msghdr) { + LOGP(DLIO, LOGL_DEBUG, "Cancellation returned\n"); + io_uring_cqe_seen(ring, cqe); + continue; + } + if (!msghdr->iofd) { + io_uring_cqe_seen(ring, cqe); + iofd_msghdr_free(msghdr); + continue; + } + + rc = cqe->res; + /* Hand the entry back to the kernel before */ + io_uring_cqe_seen(ring, cqe); + + iofd_uring_handle_completion(msghdr, rc); + + } +} + +/*! will submit the next to-be-transmitted message for given iofd */ +static int iofd_uring_submit_tx(struct osmo_io_fd *iofd) +{ + struct io_uring_sqe *sqe; + struct iofd_msghdr *msghdr; + + msghdr = iofd_txqueue_dequeue(iofd); + if (!msghdr) + return -ENODATA; + + sqe = io_uring_get_sqe(&g_ring.ring); + if (!sqe) { + LOGPIO(iofd, LOGL_ERROR, "Could not get io_uring_sqe\n"); + OSMO_ASSERT(0); + } + + io_uring_sqe_set_data(sqe, msghdr); + + switch (msghdr->action) { + case IOFD_ACT_WRITE: + case IOFD_ACT_SENDTO: + case IOFD_ACT_SENDMSG: + io_uring_prep_sendmsg(sqe, msghdr->iofd->fd, &msghdr->hdr, msghdr->flags); + break; + default: + OSMO_ASSERT(0); + } + + io_uring_submit(&g_ring.ring); + iofd->u.uring.write_msghdr = msghdr; + + return 0; +} + +static void iofd_uring_write_enable(struct osmo_io_fd *iofd); +static void iofd_uring_read_enable(struct osmo_io_fd *iofd); + +static int iofd_uring_register(struct osmo_io_fd *iofd) +{ + return 0; +} + +static int iofd_uring_unregister(struct osmo_io_fd *iofd) +{ + struct io_uring_sqe *sqe; + struct iofd_msghdr *msghdr; + + if (iofd->u.uring.read_msghdr) { + msghdr = iofd->u.uring.read_msghdr; + sqe = io_uring_get_sqe(&g_ring.ring); + OSMO_ASSERT(sqe != NULL); + io_uring_sqe_set_data(sqe, NULL); + LOGPIO(iofd, LOGL_DEBUG, "Cancelling read\n"); + iofd->u.uring.read_msghdr = NULL; + talloc_steal(OTC_GLOBAL, msghdr); + msghdr->iofd = NULL; + io_uring_prep_cancel(sqe, msghdr, 0); + } + + if (iofd->u.uring.write_msghdr) { + msghdr = iofd->u.uring.write_msghdr; + sqe = io_uring_get_sqe(&g_ring.ring); + OSMO_ASSERT(sqe != NULL); + io_uring_sqe_set_data(sqe, NULL); + LOGPIO(iofd, LOGL_DEBUG, "Cancelling write\n"); + iofd->u.uring.write_msghdr = NULL; + talloc_steal(OTC_GLOBAL, msghdr); + msgb_free(msghdr->msg); + msghdr->iofd = NULL; + io_uring_prep_cancel(sqe, msghdr, 0); + } + io_uring_submit(&g_ring.ring); + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED)) { + osmo_fd_unregister(&iofd->u.uring.connect_ofd); + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED); + } + + return 0; +} + +static void iofd_uring_write_enable(struct osmo_io_fd *iofd) +{ + iofd->u.uring.write_enabled = true; + + if (iofd->u.uring.write_msghdr) + return; + + /* This function is called again, once the socket is connected. */ + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED)) + return; + + if (osmo_iofd_txqueue_len(iofd) > 0) + iofd_uring_submit_tx(iofd); + else if (iofd->mode == OSMO_IO_FD_MODE_READ_WRITE) { + /* Empty write request to check when the socket is connected */ + struct iofd_msghdr *msghdr; + struct io_uring_sqe *sqe; + struct msgb *msg = msgb_alloc_headroom(0, 0, "io_uring write dummy"); + if (!msg) { + LOGPIO(iofd, LOGL_ERROR, "Could not allocate msgb for writing\n"); + OSMO_ASSERT(0); + } + msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_WRITE, msg, 0); + if (!msghdr) { + LOGPIO(iofd, LOGL_ERROR, "Could not allocate msghdr for writing\n"); + OSMO_ASSERT(0); + } + + msghdr->iov[0].iov_base = msgb_data(msg); + msghdr->iov[0].iov_len = msgb_length(msg); + + sqe = io_uring_get_sqe(&g_ring.ring); + if (!sqe) { + LOGPIO(iofd, LOGL_ERROR, "Could not get io_uring_sqe\n"); + OSMO_ASSERT(0); + } + io_uring_prep_writev(sqe, iofd->fd, msghdr->iov, 1, 0); + io_uring_sqe_set_data(sqe, msghdr); + + io_uring_submit(&g_ring.ring); + iofd->u.uring.write_msghdr = msghdr; + } +} + +static void iofd_uring_write_disable(struct osmo_io_fd *iofd) +{ + iofd->u.uring.write_enabled = false; +} + +static void iofd_uring_read_enable(struct osmo_io_fd *iofd) +{ + iofd->u.uring.read_enabled = true; + + if (iofd->u.uring.read_msghdr) + return; + + /* This function is called again, once the socket is connected. */ + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED)) + return; + + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + iofd_uring_submit_recv(iofd, IOFD_ACT_READ); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + iofd_uring_submit_recv(iofd, IOFD_ACT_RECVFROM); + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + iofd_uring_submit_recv(iofd, IOFD_ACT_RECVMSG); + break; + default: + OSMO_ASSERT(0); + } +} + +static void iofd_uring_read_disable(struct osmo_io_fd *iofd) +{ + iofd->u.uring.read_enabled = false; +} + +static int iofd_uring_close(struct osmo_io_fd *iofd) +{ + iofd_uring_read_disable(iofd); + iofd_uring_write_disable(iofd); + iofd_uring_unregister(iofd); + return close(iofd->fd); +} + +/* called via osmocom poll/select main handling once outbound non-blocking connect() completes */ +static int iofd_uring_connected_cb(struct osmo_fd *ofd, unsigned int what) +{ + struct osmo_io_fd *iofd = ofd->data; + + LOGPIO(iofd, LOGL_DEBUG, "Socket connected or failed.\n"); + + if (!(what & OSMO_FD_WRITE)) + return 0; + + /* Unregister from poll/select handling. */ + osmo_fd_unregister(ofd); + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED); + + /* Notify the application about this via a zero-length write completion call-back. */ + IOFD_FLAG_SET(iofd, IOFD_FLAG_IN_CALLBACK); + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + iofd->io_ops.write_cb(iofd, 0, NULL); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + iofd->io_ops.sendto_cb(iofd, 0, NULL, NULL); + break; + case OSMO_IO_FD_MODE_RECVMSG_SENDMSG: + iofd->io_ops.sendmsg_cb(iofd, 0, NULL); + break; + } + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_IN_CALLBACK); + + /* If write/read notifications are pending, enable it now. */ + if (iofd->u.uring.write_enabled && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_uring_write_enable(iofd); + if (iofd->u.uring.read_enabled && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_uring_read_enable(iofd); + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_TO_FREE) && !iofd->u.uring.read_msghdr && !iofd->u.uring.write_msghdr) + talloc_free(iofd); + return 0; +} + +static void iofd_uring_notify_connected(struct osmo_io_fd *iofd) +{ + if (iofd->mode == OSMO_IO_FD_MODE_RECVMSG_SENDMSG) { + /* Don't call this function after enabling read or write. */ + OSMO_ASSERT(!iofd->u.uring.write_enabled && !iofd->u.uring.read_enabled); + + /* Use a temporary osmo_fd which we can use to notify us once the connection is established + * or failed (indicated by FD becoming writable). + * This is needed as (at least for SCTP sockets) one cannot submit a zero-length writev/sendmsg + * in order to get notification when the socekt is writable.*/ + if (!IOFD_FLAG_ISSET(iofd, IOFD_FLAG_NOTIFY_CONNECTED)) { + osmo_fd_setup(&iofd->u.uring.connect_ofd, iofd->fd, OSMO_FD_WRITE, + iofd_uring_connected_cb, iofd, 0); + if (osmo_fd_register(&iofd->u.uring.connect_ofd) < 0) + LOGPIO(iofd, LOGL_ERROR, "Failed to register FD for connect event.\n"); + else + IOFD_FLAG_SET(iofd, IOFD_FLAG_NOTIFY_CONNECTED); + } + } else + iofd_uring_write_enable(iofd); +} + +const struct iofd_backend_ops iofd_uring_ops = { + .register_fd = iofd_uring_register, + .unregister_fd = iofd_uring_unregister, + .close = iofd_uring_close, + .write_enable = iofd_uring_write_enable, + .write_disable = iofd_uring_write_disable, + .read_enable = iofd_uring_read_enable, + .read_disable = iofd_uring_read_disable, + .notify_connected = iofd_uring_notify_connected, +}; + +#endif /* defined(__linux__) */ diff --git a/src/panic.c b/src/core/panic.c index 072f458b..bbf6d081 100644 --- a/src/panic.c +++ b/src/core/panic.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup utils @@ -31,7 +27,7 @@ #include <osmocom/core/panic.h> #include <osmocom/core/backtrace.h> -#include "../config.h" +#include "config.h" static osmo_panic_handler_t osmo_panic_handler = (void*)0; diff --git a/src/plugin.c b/src/core/plugin.c index 40de4f8c..687ad406 100644 --- a/src/plugin.c +++ b/src/core/plugin.c @@ -17,17 +17,13 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup utils * @{ * \file plugin.c */ -#include "../config.h" +#include "config.h" #if HAVE_DLFCN_H diff --git a/src/prbs.c b/src/core/prbs.c index 8fa04bb7..8fa04bb7 100644 --- a/src/prbs.c +++ b/src/core/prbs.c diff --git a/src/prim.c b/src/core/prim.c index 3c8a7f12..3c8a7f12 100644 --- a/src/prim.c +++ b/src/core/prim.c diff --git a/src/core/probes.d b/src/core/probes.d new file mode 100644 index 00000000..e4150f0c --- /dev/null +++ b/src/core/probes.d @@ -0,0 +1,6 @@ +provider libosmocore { + probe log_start(); + probe log_done(); + probe stats_start(); + probe stats_done(); +}; diff --git a/src/rate_ctr.c b/src/core/rate_ctr.c index 026670bd..44e26585 100644 --- a/src/rate_ctr.c +++ b/src/core/rate_ctr.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup rate_ctr @@ -57,14 +53,17 @@ * * \file rate_ctr.c */ +#include <errno.h> #include <stdbool.h> #include <stdint.h> #include <string.h> +#include <unistd.h> +#include <inttypes.h> #include <osmocom/core/utils.h> #include <osmocom/core/linuxlist.h> #include <osmocom/core/talloc.h> -#include <osmocom/core/timer.h> +#include <osmocom/core/select.h> #include <osmocom/core/rate_ctr.h> #include <osmocom/core/logging.h> @@ -263,6 +262,26 @@ void rate_ctr_group_free(struct rate_ctr_group *grp) talloc_free(grp); } +/*! Get rate counter from group, identified by index idx + * \param[in] grp Rate counter group + * \param[in] idx Index of the counter to retrieve + * \returns rate counter requested + */ +struct rate_ctr *rate_ctr_group_get_ctr(struct rate_ctr_group *grp, unsigned int idx) +{ + return &grp->ctr[idx]; +} + +/*! Set a name for the group of counters be used instead of index value + at report time. + * \param[in] grp Rate counter group + * \param[in] name Name identifier to assign to the rate counter group + */ +void rate_ctr_group_set_name(struct rate_ctr_group *grp, const char *name) +{ + osmo_talloc_replace_string(grp, &grp->name, name); +} + /*! Add a number to the counter */ void rate_ctr_add(struct rate_ctr *ctr, int inc) { @@ -287,14 +306,9 @@ static void interval_expired(struct rate_ctr *ctr, enum rate_ctr_intv intv) ctr->intv[intv].rate = ctr->current - ctr->intv[intv].last; /* save current counter for next interval */ ctr->intv[intv].last = ctr->current; - - /* update the rate of the next bigger interval. This will - * be overwritten when that next larger interval expires */ - if (intv + 1 < ARRAY_SIZE(ctr->intv)) - ctr->intv[intv+1].rate += ctr->intv[intv].rate; } -static struct osmo_timer_list rate_ctr_timer; +static struct osmo_fd rate_ctr_timer = { .fd = -1 }; static uint64_t timer_ticks; /* The one-second interval has expired */ @@ -315,18 +329,35 @@ static void rate_ctr_group_intv(struct rate_ctr_group *grp) } } -static void rate_ctr_timer_cb(void *data) +static int rate_ctr_timer_cb(struct osmo_fd *ofd, unsigned int what) { struct rate_ctr_group *ctrg; + uint64_t expire_count; + int rc; + + /* check that the timer has actually expired */ + if (!(what & OSMO_FD_READ)) + return 0; + + /* read from timerfd: number of expirations of periodic timer */ + rc = read(ofd->fd, (void *) &expire_count, sizeof(expire_count)); + if (rc < 0 && errno == EAGAIN) + return 0; + + OSMO_ASSERT(rc == sizeof(expire_count)); - /* Increment number of ticks before we calculate intervals, - * as a counter value of 0 would already wrap all counters */ - timer_ticks++; + if (expire_count > 1) + LOGP(DLGLOBAL, LOGL_NOTICE, "Stats timer expire_count=%" PRIu64 ": We missed %" PRIu64 " timers\n", + expire_count, expire_count - 1); - llist_for_each_entry(ctrg, &rate_ctr_groups, list) - rate_ctr_group_intv(ctrg); + do { /* Increment number of ticks before we calculate intervals, + * as a counter value of 0 would already wrap all counters */ + timer_ticks++; + llist_for_each_entry(ctrg, &rate_ctr_groups, list) + rate_ctr_group_intv(ctrg); + } while (--expire_count); - osmo_timer_schedule(&rate_ctr_timer, 1, 0); + return 0; } /*! Initialize the counter module. Call this once from your application. @@ -334,9 +365,27 @@ static void rate_ctr_timer_cb(void *data) * \returns 0 on success; negative on error */ int rate_ctr_init(void *tall_ctx) { + struct timespec ts_interval = { .tv_sec = 1, .tv_nsec = 0 }; + int rc; + + /* ignore repeated initialization */ + if (osmo_fd_is_registered(&rate_ctr_timer)) + return 0; + tall_rate_ctr_ctx = tall_ctx; - osmo_timer_setup(&rate_ctr_timer, rate_ctr_timer_cb, NULL); - osmo_timer_schedule(&rate_ctr_timer, 1, 0); + + rc = osmo_timerfd_setup(&rate_ctr_timer, rate_ctr_timer_cb, NULL); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Failed to setup the timer with error code %d (fd=%d)\n", + rc, rate_ctr_timer.fd); + return rc; + } + + rc = osmo_timerfd_schedule(&rate_ctr_timer, NULL, &ts_interval); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Failed to schedule the timer with error code %d (fd=%d)\n", + rc, rate_ctr_timer.fd); + } return 0; } @@ -426,4 +475,25 @@ int rate_ctr_for_each_group(rate_ctr_group_handler_t handle_group, void *data) return rc; } +/*! Reset a rate counter back to zero + * \param[in] ctr counter to reset + */ +void rate_ctr_reset(struct rate_ctr *ctr) +{ + memset(ctr, 0, sizeof(*ctr)); +} + +/*! Reset all counters in a group + * \param[in] ctrg counter group to reset + */ +void rate_ctr_group_reset(struct rate_ctr_group *ctrg) +{ + int i; + + for (i = 0; i < ctrg->desc->num_ctr; i++) { + struct rate_ctr *ctr = &ctrg->ctr[i]; + rate_ctr_reset(ctr); + } +} + /*! @} */ diff --git a/src/rbtree.c b/src/core/rbtree.c index 211978f1..f4dc219b 100644 --- a/src/rbtree.c +++ b/src/core/rbtree.c @@ -15,11 +15,6 @@ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - MA 02110-1301, USA - linux/lib/rbtree.c */ diff --git a/src/core/select.c b/src/core/select.c new file mode 100644 index 00000000..70047f09 --- /dev/null +++ b/src/core/select.c @@ -0,0 +1,731 @@ +/*! \file select.c + * select filedescriptor handling. + * Taken from: + * userspace logging daemon for the iptables ULOG target + * of the linux 2.4 netfilter subsystem. */ +/* + * (C) 2000-2020 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <fcntl.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <stdbool.h> +#include <errno.h> + +#include <osmocom/core/select.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/stats_tcp.h> + +#include "config.h" + +#if defined(HAVE_SYS_SELECT_H) && defined(HAVE_POLL_H) +#include <sys/select.h> +#include <poll.h> + +/*! \addtogroup select + * @{ + * select() loop abstraction + * + * \file select.c */ + +/* keep a set of file descriptors per-thread, so that each thread can have its own + * distinct set of file descriptors to interact with */ +static __thread int maxfd = 0; +static __thread struct llist_head osmo_fds; /* TLS cannot use LLIST_HEAD() */ +static __thread int unregistered_count; + +/* Array of struct osmo_fd * (size "max_fd") ordered by "ofd->fd" */ +static __thread struct { + struct osmo_fd **table; + unsigned int size; +} osmo_fd_lookup; + +static void osmo_fd_lookup_table_extend(unsigned int new_max_fd) +{ + unsigned int min_new_size; + unsigned int pw2; + unsigned int new_size; + + /* First calculate the minimally required new size of the array in bytes: */ + min_new_size = (new_max_fd + 1) * sizeof(struct osmo_fd *); + /* Now find the lower power of two of min_new_size (in bytes): */ + pw2 = 32 - __builtin_clz(min_new_size); + /* get next (upper side) power of 2: */ + pw2++; + OSMO_ASSERT(pw2 <= 31); /* Avoid shifting more than 31 bits */ + new_size = 1 << (pw2 + 1); + + /* Let's make it at least 1024B, to avoid reallocating quickly at startup */ + if (new_size < 1024) + new_size = 1024; + if (new_size > osmo_fd_lookup.size) { + uint8_t *ptr = talloc_realloc_size(OTC_GLOBAL, osmo_fd_lookup.table, new_size); + OSMO_ASSERT(ptr); + memset(ptr + osmo_fd_lookup.size, 0, new_size - osmo_fd_lookup.size); + osmo_fd_lookup.table = (struct osmo_fd **)ptr; + osmo_fd_lookup.size = new_size; + } +} + +#ifndef FORCE_IO_SELECT +struct poll_state { + /* array of pollfd */ + struct pollfd *poll; + /* number of entries in pollfd allocated */ + unsigned int poll_size; + /* number of osmo_fd registered */ + unsigned int num_registered; +}; +static __thread struct poll_state g_poll; +#endif /* FORCE_IO_SELECT */ + +/*! See osmo_select_shutdown_request() */ +static int _osmo_select_shutdown_requested = 0; +/*! See osmo_select_shutdown_request() */ +static bool _osmo_select_shutdown_done = false; + +/*! Set up an osmo-fd. Will not register it. + * \param[inout] ofd Osmo FD to be set-up + * \param[in] fd OS-level file descriptor number + * \param[in] when bit-mask of OSMO_FD_{READ,WRITE,EXECEPT} + * \param[in] cb Call-back function to be called + * \param[in] data Private context pointer + * \param[in] priv_nr Private number + */ +void osmo_fd_setup(struct osmo_fd *ofd, int fd, unsigned int when, + int (*cb)(struct osmo_fd *fd, unsigned int what), + void *data, unsigned int priv_nr) +{ + ofd->fd = fd; + ofd->when = when; + ofd->cb = cb; + ofd->data = data; + ofd->priv_nr = priv_nr; +} + +/*! Update the 'when' field of osmo_fd. "ofd->when = (ofd->when & when_mask) | when". + * Use this function instead of directly modifying ofd->when, as the latter will be + * removed soon. */ +void osmo_fd_update_when(struct osmo_fd *ofd, unsigned int when_mask, unsigned int when) +{ + ofd->when &= when_mask; + ofd->when |= when; +} + +/*! Check if a file descriptor is already registered + * \param[in] fd osmocom file descriptor to be checked + * \returns true if registered; otherwise false + */ +bool osmo_fd_is_registered(struct osmo_fd *fd) +{ + struct osmo_fd *entry; + llist_for_each_entry(entry, &osmo_fds, list) { + if (entry == fd) { + return true; + } + } + + return false; +} + +/*! Register a new file descriptor with select loop abstraction + * \param[in] fd osmocom file descriptor to be registered + * \returns 0 on success; negative in case of error + * + * The API expects fd field of the struct osmo_fd to remain unchanged while + * registered, ie until osmo_fd_unregister() is called on it. + */ +int osmo_fd_register(struct osmo_fd *fd) +{ + int flags; + + /* make FD nonblocking */ + flags = fcntl(fd->fd, F_GETFL); + if (flags < 0) + return flags; + flags |= O_NONBLOCK; + flags = fcntl(fd->fd, F_SETFL, flags); + if (flags < 0) + return flags; + + /* set close-on-exec flag */ + flags = fcntl(fd->fd, F_GETFD); + if (flags < 0) + return flags; + flags |= FD_CLOEXEC; + flags = fcntl(fd->fd, F_SETFD, flags); + if (flags < 0) + return flags; + + /* Register FD */ + if (fd->fd > maxfd) { + maxfd = fd->fd; + osmo_fd_lookup_table_extend(maxfd); + } + +#ifdef OSMO_FD_CHECK + if (osmo_fd_is_registered(fd)) { + fprintf(stderr, "Adding a osmo_fd that is already in the list.\n"); + return 0; + } +#endif +#ifndef FORCE_IO_SELECT + if (g_poll.num_registered + 1 > g_poll.poll_size) { + struct pollfd *p; + unsigned int new_size = g_poll.poll_size ? g_poll.poll_size * 2 : 1024; + p = talloc_realloc(OTC_GLOBAL, g_poll.poll, struct pollfd, new_size); + if (!p) + return -ENOMEM; + memset(p + g_poll.poll_size, 0, new_size - g_poll.poll_size); + g_poll.poll = p; + g_poll.poll_size = new_size; + } + g_poll.num_registered++; +#endif /* FORCE_IO_SELECT */ + + llist_add_tail(&fd->list, &osmo_fds); + osmo_fd_lookup.table[fd->fd] = fd; + + return 0; +} + +/*! Unregister a file descriptor from select loop abstraction + * \param[in] fd osmocom file descriptor to be unregistered + * + * Caller is responsible for ensuring the fd is really registered before calling this API. + * This function must be called before changing the value of the fd field in + * the struct osmo_fd. + */ +void osmo_fd_unregister(struct osmo_fd *fd) +{ + /* Note: when fd is inside the osmo_fds list (not registered before) + * this function will crash! If in doubt, check file descriptor with + * osmo_fd_is_registered() */ + unregistered_count++; + llist_del(&fd->list); +#ifndef FORCE_IO_SELECT + g_poll.num_registered--; +#endif /* FORCE_IO_SELECT */ + + if (OSMO_UNLIKELY(fd->fd < 0 || fd->fd > maxfd)) { + /* Some old users used to incorrectly set fd = -1 *before* calling osmo_unregister(). + * Hence, in order to keep backward compatibility it's not possible to assert() here. + * Instead, print an error message since this is actually a bug in the API user. */ +#ifdef OSMO_FD_CHECK + osmo_panic("osmo_fd_unregister(fd=%u) out of expected range (0..%u), fix your code!!!\n", + fd->fd, maxfd); +#else + fprintf(stderr, "osmo_fd_unregister(fd=%u) out of expected range (0..%u), fix your code!!!\n", + fd->fd, maxfd); + return; +#endif + } + + osmo_fd_lookup.table[fd->fd] = NULL; + /* If existent, free any statistical data */ + osmo_stats_tcp_osmo_fd_unregister(fd); +} + +/*! Close a file descriptor, mark it as closed + unregister from select loop abstraction + * \param[in] fd osmocom file descriptor to be unregistered + closed + * + * If \a fd is registered, we unregister it from the select() loop + * abstraction. We then close the fd and set it to -1, as well as + * unsetting any 'when' flags */ +void osmo_fd_close(struct osmo_fd *fd) +{ + if (osmo_fd_is_registered(fd)) + osmo_fd_unregister(fd); + if (fd->fd != -1) + close(fd->fd); + fd->fd = -1; + fd->when = 0; +} + +/*! Populate the fd_sets and return the highest fd number + * \param[in] _rset The readfds to populate + * \param[in] _wset The wrtiefds to populate + * \param[in] _eset The errorfds to populate + * + * \returns The highest file descriptor seen or 0 on an empty list + */ +inline int osmo_fd_fill_fds(void *_rset, void *_wset, void *_eset) +{ + fd_set *readset = _rset, *writeset = _wset, *exceptset = _eset; + struct osmo_fd *ufd; + int highfd = 0; + + llist_for_each_entry(ufd, &osmo_fds, list) { + if (ufd->when & OSMO_FD_READ) + FD_SET(ufd->fd, readset); + + if (ufd->when & OSMO_FD_WRITE) + FD_SET(ufd->fd, writeset); + + if (ufd->when & OSMO_FD_EXCEPT) + FD_SET(ufd->fd, exceptset); + + if (ufd->fd > highfd) + highfd = ufd->fd; + } + + return highfd; +} + +inline int osmo_fd_disp_fds(void *_rset, void *_wset, void *_eset) +{ + struct osmo_fd *ufd, *tmp; + int work = 0; + fd_set *readset = _rset, *writeset = _wset, *exceptset = _eset; + +restart: + unregistered_count = 0; + llist_for_each_entry_safe(ufd, tmp, &osmo_fds, list) { + int flags = 0; + + if (FD_ISSET(ufd->fd, readset)) { + flags |= OSMO_FD_READ; + FD_CLR(ufd->fd, readset); + } + + if (FD_ISSET(ufd->fd, writeset)) { + flags |= OSMO_FD_WRITE; + FD_CLR(ufd->fd, writeset); + } + + if (FD_ISSET(ufd->fd, exceptset)) { + flags |= OSMO_FD_EXCEPT; + FD_CLR(ufd->fd, exceptset); + } + + if (flags) { + work = 1; + /* make sure to clear any log context before processing the next incoming message + * as part of some file descriptor callback. This effectively prevents "context + * leaking" from processing of one message into processing of the next message as part + * of one iteration through the list of file descriptors here. See OS#3813 */ + log_reset_context(); + ufd->cb(ufd, flags); + } + /* ugly, ugly hack. If more than one filedescriptor was + * unregistered, they might have been consecutive and + * llist_for_each_entry_safe() is no longer safe */ + /* this seems to happen with the last element of the list as well */ + if (unregistered_count >= 1) + goto restart; + } + + return work; +} + + +#ifndef FORCE_IO_SELECT +/* fill g_poll.poll and return the number of entries filled */ +static unsigned int poll_fill_fds(void) +{ + struct osmo_fd *ufd; + unsigned int i = 0; + + llist_for_each_entry(ufd, &osmo_fds, list) { + struct pollfd *p; + + if (!ufd->when) + continue; + + p = &g_poll.poll[i++]; + + p->fd = ufd->fd; + p->events = 0; + p->revents = 0; + + /* use the same mapping as the Linux kernel does in fs/select.c */ + if (ufd->when & OSMO_FD_READ) + p->events |= POLLIN | POLLHUP | POLLERR; + + if (ufd->when & OSMO_FD_WRITE) + p->events |= POLLOUT | POLLERR; + + if (ufd->when & OSMO_FD_EXCEPT) + p->events |= POLLPRI; + + } + + return i; +} + +/* iterate over first n_fd entries of g_poll.poll + dispatch */ +static int poll_disp_fds(unsigned int n_fd) +{ + struct osmo_fd *ufd; + unsigned int i; + int work = 0; + int shutdown_pending_writes = 0; + + for (i = 0; i < n_fd; i++) { + struct pollfd *p = &g_poll.poll[i]; + int flags = 0; + + if (!p->revents) + continue; + + ufd = osmo_fd_get_by_fd(p->fd); + if (!ufd) { + /* FD might have been unregistered meanwhile */ + continue; + } + /* use the same mapping as the Linux kernel does in fs/select.c */ + if (p->revents & (POLLIN | POLLHUP | POLLERR)) + flags |= OSMO_FD_READ; + if (p->revents & (POLLOUT | POLLERR)) + flags |= OSMO_FD_WRITE; + if (p->revents & POLLPRI) + flags |= OSMO_FD_EXCEPT; + + /* make sure we never report more than the user requested */ + flags &= ufd->when; + + if (_osmo_select_shutdown_requested > 0) { + if (ufd->when & OSMO_FD_WRITE) + shutdown_pending_writes++; + } + + if (flags) { + work = 1; + /* make sure to clear any log context before processing the next incoming message + * as part of some file descriptor callback. This effectively prevents "context + * leaking" from processing of one message into processing of the next message as part + * of one iteration through the list of file descriptors here. See OS#3813 */ + log_reset_context(); + ufd->cb(ufd, flags); + } + } + + if (_osmo_select_shutdown_requested > 0 && !shutdown_pending_writes) + _osmo_select_shutdown_done = true; + + return work; +} + +static int _osmo_select_main(int polling) +{ + unsigned int n_poll; + int rc; + int timeout = 0; + + /* prepare read and write fdsets */ + n_poll = poll_fill_fds(); + + if (!polling) { + osmo_timers_prepare(); + timeout = osmo_timers_nearest_ms(); + + if (_osmo_select_shutdown_requested && timeout == -1) + timeout = 0; + } + + rc = poll(g_poll.poll, n_poll, timeout); + if (rc < 0) + return 0; + + /* fire timers */ + if (!_osmo_select_shutdown_requested) + osmo_timers_update(); + + OSMO_ASSERT(osmo_ctx->select); + + /* call registered callback functions */ + return poll_disp_fds(n_poll); +} +#else /* FORCE_IO_SELECT */ +/* the old implementation based on select, used 2008-2020 */ +static int _osmo_select_main(int polling) +{ + fd_set readset, writeset, exceptset; + int rc; + struct timeval no_time = {0, 0}; + + FD_ZERO(&readset); + FD_ZERO(&writeset); + FD_ZERO(&exceptset); + + /* prepare read and write fdsets */ + osmo_fd_fill_fds(&readset, &writeset, &exceptset); + + if (!polling) + osmo_timers_prepare(); + rc = select(maxfd+1, &readset, &writeset, &exceptset, polling ? &no_time : osmo_timers_nearest()); + if (rc < 0) + return 0; + + /* fire timers */ + osmo_timers_update(); + + OSMO_ASSERT(osmo_ctx->select); + + /* call registered callback functions */ + return osmo_fd_disp_fds(&readset, &writeset, &exceptset); +} +#endif /* FORCE_IO_SELECT */ + +/*! select main loop integration + * \param[in] polling should we pollonly (1) or block on select (0) + * \returns 0 if no fd handled; 1 if fd handled; negative in case of error + */ +int osmo_select_main(int polling) +{ + int rc = _osmo_select_main(polling); +#ifndef EMBEDDED + if (talloc_total_size(osmo_ctx->select) != 0) { + osmo_panic("You cannot use the 'select' volatile " + "context if you don't use osmo_select_main_ctx()!\n"); + } +#endif + return rc; +} + +#ifndef EMBEDDED +/*! select main loop integration with temporary select-dispatch talloc context + * \param[in] polling should we pollonly (1) or block on select (0) + * \returns 0 if no fd handled; 1 if fd handled; negative in case of error + */ +int osmo_select_main_ctx(int polling) +{ + int rc = _osmo_select_main(polling); + /* free all the children of the volatile 'select' scope context */ + talloc_free_children(osmo_ctx->select); + return rc; +} +#endif + +/*! find an osmo_fd based on the integer fd + * \param[in] fd file descriptor to use as search key + * \returns \ref osmo_fd for \ref fd; NULL in case it doesn't exist */ +struct osmo_fd *osmo_fd_get_by_fd(int fd) +{ + if (fd > maxfd || fd < 0) + return NULL; + return osmo_fd_lookup.table[fd]; +} + +/*! initialize the osmocom select abstraction for the current thread */ +void osmo_select_init(void) +{ + INIT_LLIST_HEAD(&osmo_fds); + osmo_fd_lookup_table_extend(0); +} + +/* ensure main thread always has pre-initialized osmo_fds + * priority 102: must run after on_dso_load_ctx */ +static __attribute__((constructor(102))) void on_dso_load_select(void) +{ + osmo_select_init(); +} + +#ifdef HAVE_SYS_TIMERFD_H +#include <sys/timerfd.h> + +/*! disable the osmocom-wrapped timerfd */ +int osmo_timerfd_disable(struct osmo_fd *ofd) +{ + const struct itimerspec its_null = { + .it_value = { 0, 0 }, + .it_interval = { 0, 0 }, + }; + return timerfd_settime(ofd->fd, 0, &its_null, NULL); +} + +/*! schedule the osmocom-wrapped timerfd to occur first at \a first, then periodically at \a interval + * \param[in] ofd Osmocom wrapped timerfd + * \param[in] first Relative time at which the timer should first execute (NULL = \a interval) + * \param[in] interval Time interval at which subsequent timer shall fire + * \returns 0 on success; negative on error */ +int osmo_timerfd_schedule(struct osmo_fd *ofd, const struct timespec *first, + const struct timespec *interval) +{ + struct itimerspec its; + + if (ofd->fd < 0) + return -EINVAL; + + /* first expiration */ + if (first) + its.it_value = *first; + else + its.it_value = *interval; + /* repeating interval */ + its.it_interval = *interval; + + return timerfd_settime(ofd->fd, 0, &its, NULL); +} + +/*! setup osmocom-wrapped timerfd + * \param[inout] ofd Osmocom-wrapped timerfd on which to operate + * \param[in] cb Call-back function called when timerfd becomes readable + * \param[in] data Opaque data to be passed on to call-back + * \returns 0 on success; negative on error + * + * We simply initialize the data structures here, but do not yet + * schedule the timer. + */ +int osmo_timerfd_setup(struct osmo_fd *ofd, int (*cb)(struct osmo_fd *, unsigned int), void *data) +{ + ofd->cb = cb; + ofd->data = data; + ofd->when = OSMO_FD_READ; + + if (ofd->fd < 0) { + int rc; + + ofd->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK); + if (ofd->fd < 0) + return ofd->fd; + + rc = osmo_fd_register(ofd); + if (rc < 0) { + osmo_fd_unregister(ofd); + close(ofd->fd); + ofd->fd = -1; + return rc; + } + } + return 0; +} + +#endif /* HAVE_SYS_TIMERFD_H */ + +#ifdef HAVE_SYS_SIGNALFD_H +#include <sys/signalfd.h> + +static int signalfd_callback(struct osmo_fd *ofd, unsigned int what) +{ + struct osmo_signalfd *osfd = ofd->data; + struct signalfd_siginfo fdsi; + int rc; + + rc = read(ofd->fd, &fdsi, sizeof(fdsi)); + if (rc < 0) { + osmo_fd_unregister(ofd); + close(ofd->fd); + ofd->fd = -1; + return rc; + } + + osfd->cb(osfd, &fdsi); + + return 0; +}; + +/*! create a signalfd and register it with osmocom select loop. + * \param[in] ctx talloc context from which osmo_signalfd is to be allocated + * \param[in] set of signals to be accept via this file descriptor + * \param[in] cb call-back function to be called for each arriving signal + * \param[in] data opaque user-provided data to pass to callback + * \returns pointer to newly-allocated + registered osmo_signalfd; NULL on error */ +struct osmo_signalfd * +osmo_signalfd_setup(void *ctx, sigset_t set, osmo_signalfd_cb *cb, void *data) +{ + struct osmo_signalfd *osfd = talloc_size(ctx, sizeof(*osfd)); + int fd, rc; + + if (!osfd) + return NULL; + + osfd->data = data; + osfd->sigset = set; + osfd->cb = cb; + + fd = signalfd(-1, &osfd->sigset, SFD_NONBLOCK); + if (fd < 0) { + talloc_free(osfd); + return NULL; + } + + osmo_fd_setup(&osfd->ofd, fd, OSMO_FD_READ, signalfd_callback, osfd, 0); + rc = osmo_fd_register(&osfd->ofd); + if (rc < 0) { + close(fd); + talloc_free(osfd); + return NULL; + } + + return osfd; +} + +#endif /* HAVE_SYS_SIGNALFD_H */ + +/*! Request osmo_select_* to only service pending OSMO_FD_WRITE requests. Once all writes are done, + * osmo_select_shutdown_done() returns true. This allows for example to send all outbound packets before terminating the + * process. + * + * Usage example: + * + * static void signal_handler(int signum) + * { + * fprintf(stdout, "signal %u received\n", signum); + * + * switch (signum) { + * case SIGINT: + * case SIGTERM: + * // If the user hits Ctrl-C the third time, just terminate immediately. + * if (osmo_select_shutdown_requested() >= 2) + * exit(-1); + * // Request write-only mode in osmo_select_main_ctx() + * osmo_select_shutdown_request(); + * break; + * [...] + * } + * + * main() + * { + * signal(SIGINT, &signal_handler); + * signal(SIGTERM, &signal_handler); + * + * [...] + * + * // After the signal_handler issued osmo_select_shutdown_request(), osmo_select_shutdown_done() returns true + * // as soon as all write queues are empty. + * while (!osmo_select_shutdown_done()) { + * osmo_select_main_ctx(0); + * } + * } + */ +void osmo_select_shutdown_request(void) +{ + _osmo_select_shutdown_requested++; +}; + +/*! Return the number of times osmo_select_shutdown_request() was called before. */ +int osmo_select_shutdown_requested(void) +{ + return _osmo_select_shutdown_requested; +}; + +/*! Return true after osmo_select_shutdown_requested() was called, and after an osmo_select poll loop found no more + * pending OSMO_FD_WRITE on any registered socket. */ +bool osmo_select_shutdown_done(void) { + return _osmo_select_shutdown_done; +}; + +/*! @} */ + +#endif /* _HAVE_SYS_SELECT_H */ diff --git a/src/sercomm.c b/src/core/sercomm.c index 2639bf8a..1798acec 100644 --- a/src/sercomm.c +++ b/src/core/sercomm.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup sercomm diff --git a/src/serial.c b/src/core/serial.c index 31cb81d1..117c049b 100644 --- a/src/serial.c +++ b/src/core/serial.c @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /*! \addtogroup serial @@ -67,7 +63,7 @@ osmo_serial_init(const char *dev, speed_t baudrate) return -errno; } - /* now put it into blcoking mode */ + /* now put it into blocking mode */ flags = fcntl(fd, F_GETFL, 0); if (flags < 0) { dbg_perror("fcntl get flags"); @@ -91,8 +87,10 @@ osmo_serial_init(const char *dev, speed_t baudrate) goto error; } - cfsetispeed(&tio, baudrate); - cfsetospeed(&tio, baudrate); + if (cfsetispeed(&tio, baudrate) < 0) + dbg_perror("cfsetispeed()"); + if (cfsetospeed(&tio, baudrate) < 0) + dbg_perror("cfsetospeed()"); tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS); tio.c_cflag |= (CREAD | CLOCAL | CS8); @@ -136,12 +134,15 @@ _osmo_serial_set_baudrate(int fd, speed_t baudrate) dbg_perror("tcgetattr()"); return -errno; } - cfsetispeed(&tio, baudrate); - cfsetospeed(&tio, baudrate); + + if (cfsetispeed(&tio, baudrate) < 0) + dbg_perror("cfsetispeed()"); + if (cfsetospeed(&tio, baudrate) < 0) + dbg_perror("cfsetospeed()"); rc = tcsetattr(fd, TCSANOW, &tio); if (rc < 0) { - dbg_perror("tcgetattr()"); + dbg_perror("tcsetattr()"); return -errno; } @@ -240,4 +241,39 @@ osmo_serial_clear_custom_baudrate(int fd) return 0; } +/*! Convert unsigned integer value to speed_t + * \param[in] baudrate integer value containing the desired standard baudrate + * \param[out] speed the standrd baudrate requested in speed_t format + * \returns 0 for success or negative errno. + */ +int +osmo_serial_speed_t(unsigned int baudrate, speed_t *speed) +{ + switch(baudrate) { + case 0: *speed = B0; break; + case 50: *speed = B50; break; + case 75: *speed = B75; break; + case 110: *speed = B110; break; + case 134: *speed = B134; break; + case 150: *speed = B150; break; + case 200: *speed = B200; break; + case 300: *speed = B300; break; + case 600: *speed = B600; break; + case 1200: *speed = B1200; break; + case 1800: *speed = B1800; break; + case 2400: *speed = B2400; break; + case 4800: *speed = B4800; break; + case 9600: *speed = B9600; break; + case 19200: *speed = B19200; break; + case 38400: *speed = B38400; break; + case 57600: *speed = B57600; break; + case 115200: *speed = B115200; break; + case 230400: *speed = B230400; break; + default: + *speed = B0; + return -EINVAL; + } + return 0; +} + /*! @} */ diff --git a/src/signal.c b/src/core/signal.c index be3b7778..ba1555ae 100644 --- a/src/signal.c +++ b/src/core/signal.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <osmocom/core/signal.h> diff --git a/src/sockaddr_str.c b/src/core/sockaddr_str.c index f523050c..bea61849 100644 --- a/src/sockaddr_str.c +++ b/src/core/sockaddr_str.c @@ -20,10 +20,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include "config.h" @@ -37,6 +33,7 @@ #include <osmocom/core/sockaddr_str.h> #include <osmocom/core/utils.h> #include <osmocom/core/byteswap.h> +#include <osmocom/core/socket.h> /*! \addtogroup sockaddr_str * @@ -95,6 +92,80 @@ bool osmo_sockaddr_str_is_nonzero(const struct osmo_sockaddr_str *sockaddr_str) } } +/*! Compare two osmo_sockaddr_str instances by string comparison. + * Compare by strcmp() for the address and compare port numbers, ignore the AF_INET/AF_INET6 value. + * \param[in] a left side of comparison. + * \param[in] b right side of comparison. + * \return -1 if a < b, 0 if a == b, 1 if a > b. + */ +static int osmo_sockaddr_str_cmp_by_string(const struct osmo_sockaddr_str *a, const struct osmo_sockaddr_str *b) +{ + int cmp; + if (a == b) + return 0; + if (!a) + return -1; + if (!b) + return 1; + cmp = strncmp(a->ip, b->ip, sizeof(a->ip)); + if (cmp) + return cmp; + return OSMO_CMP(a->port, b->port); +} + +/*! Compare two osmo_sockaddr_str instances by resulting IP address. + * Compare IP versions (AF_INET vs AF_INET6), compare resulting IP address bytes and compare port numbers. + * If the IP address strings cannot be parsed successfully / if the 'af' is neither AF_INET nor AF_INET6, fall back to + * pure string comparison of the ip address. + * \param[in] a left side of comparison. + * \param[in] b right side of comparison. + * \return -1 if a < b, 0 if a == b, 1 if a > b. + */ +int osmo_sockaddr_str_cmp(const struct osmo_sockaddr_str *a, const struct osmo_sockaddr_str *b) +{ + int cmp; + uint32_t ipv4_a, ipv4_b; + struct in6_addr ipv6_a = {}, ipv6_b = {}; + + if (a == b) + return 0; + if (!a) + return -1; + if (!b) + return 1; + cmp = OSMO_CMP(a->af, b->af); + if (cmp) + return cmp; + switch (a->af) { + case AF_INET: + if (osmo_sockaddr_str_to_32(a, &ipv4_a) + || osmo_sockaddr_str_to_32(b, &ipv4_b)) + goto fallback_to_strcmp; + cmp = OSMO_CMP(ipv4_a, ipv4_b); + break; + + case AF_INET6: + if (osmo_sockaddr_str_to_in6_addr(a, &ipv6_a) + || osmo_sockaddr_str_to_in6_addr(b, &ipv6_b)) + goto fallback_to_strcmp; + cmp = memcmp(&ipv6_a, &ipv6_b, sizeof(ipv6_a)); + break; + + default: + goto fallback_to_strcmp; + } + if (cmp) + return cmp; + + cmp = OSMO_CMP(a->port, b->port); + if (cmp) + return cmp; + return 0; + +fallback_to_strcmp: + return osmo_sockaddr_str_cmp_by_string(a, b); +} + /*! Distinguish between valid IPv4 and IPv6 strings. * This does not verify whether the string is a valid IP address; it assumes that the input is a valid IP address, and * on that premise returns whether it is an IPv4 or IPv6 string, by looking for '.' and ':' characters. It is safe to @@ -114,25 +185,24 @@ int osmo_ip_str_type(const char *ip) return AF_UNSPEC; } -/*! Safely copy the given ip string to sockaddr_str, classify to AF_INET or AF_INET6, and set the port. +/*! Safely copy the given ip string to sockaddr_str, classify to AF_INET or AF_INET6. * Data will be written to sockaddr_str even if an error is returned. * \param[out] sockaddr_str The instance to copy to. * \param[in] ip Valid IP address string. - * \param[in] port Port number. * \return 0 on success, negative if copying the address string failed (e.g. too long), if the address family could * not be detected (i.e. if osmo_ip_str_type() returned AF_UNSPEC), or if sockaddr_str is NULL. */ -int osmo_sockaddr_str_from_str(struct osmo_sockaddr_str *sockaddr_str, const char *ip, uint16_t port) +int osmo_sockaddr_str_from_str2(struct osmo_sockaddr_str *sockaddr_str, const char *ip) { int rc; if (!sockaddr_str) return -ENOSPC; if (!ip) ip = ""; - *sockaddr_str = (struct osmo_sockaddr_str){ - .af = osmo_ip_str_type(ip), - .port = port, - }; + sockaddr_str->af = osmo_ip_str_type(ip); + /* to be compatible with previous behaviour, zero the full IP field. + * Allow the usage of memcmp(&sockaddr_str, ...) */ + memset(sockaddr_str->ip, 0x0, sizeof(sockaddr_str->ip)); rc = osmo_strlcpy(sockaddr_str->ip, ip, sizeof(sockaddr_str->ip)); if (rc <= 0) return -EIO; @@ -143,6 +213,26 @@ int osmo_sockaddr_str_from_str(struct osmo_sockaddr_str *sockaddr_str, const cha return 0; } +/*! Safely copy the given ip string to sockaddr_str, classify to AF_INET or AF_INET6, and set the port. + * Data will be written to sockaddr_str even if an error is returned. + * \param[out] sockaddr_str The instance to copy to. + * \param[in] ip Valid IP address string. + * \param[in] port Port number. + * \return 0 on success, negative if copying the address string failed (e.g. too long), if the address family could + * not be detected (i.e. if osmo_ip_str_type() returned AF_UNSPEC), or if sockaddr_str is NULL. + */ +int osmo_sockaddr_str_from_str(struct osmo_sockaddr_str *sockaddr_str, const char *ip, uint16_t port) +{ + int rc; + if (!sockaddr_str) + return -ENOSPC; + + rc = osmo_sockaddr_str_from_str2(sockaddr_str, ip); + sockaddr_str->port = port; + + return rc; +} + /*! Convert IPv4 address to osmo_sockaddr_str, and set port. * \param[out] sockaddr_str The instance to copy to. * \param[in] addr IPv4 address data. @@ -181,7 +271,7 @@ int osmo_sockaddr_str_from_in6_addr(struct osmo_sockaddr_str *sockaddr_str, cons return 0; } -/*! Convert IPv4 address from 32bit host-byte-order to osmo_sockaddr_str, and set port. +/*! Convert IPv4 address from 32bit network-byte-order to osmo_sockaddr_str, and set port. * \param[out] sockaddr_str The instance to copy to. * \param[in] addr 32bit IPv4 address data. * \param[in] port Port number. @@ -196,19 +286,27 @@ int osmo_sockaddr_str_from_32(struct osmo_sockaddr_str *sockaddr_str, uint32_t i return osmo_sockaddr_str_from_in_addr(sockaddr_str, &addr, port); } -/*! Convert IPv4 address from 32bit network-byte-order to osmo_sockaddr_str, and set port. +/*! Convert IPv4 address from 32bit host-byte-order to osmo_sockaddr_str, and set port. + * For legacy reasons, this function has a misleading 'n' in its name. * \param[out] sockaddr_str The instance to copy to. * \param[in] addr 32bit IPv4 address data. * \param[in] port Port number. * \return 0 on success, negative on error. */ -int osmo_sockaddr_str_from_32n(struct osmo_sockaddr_str *sockaddr_str, uint32_t ip, uint16_t port) +int osmo_sockaddr_str_from_32h(struct osmo_sockaddr_str *sockaddr_str, uint32_t ip, uint16_t port) { if (!sockaddr_str) return -ENOSPC; return osmo_sockaddr_str_from_32(sockaddr_str, osmo_ntohl(ip), port); } +/*! DEPRECATED: the name suggests a conversion from network byte order, but actually converts from host byte order. Use + * osmo_sockaddr_str_from_32 for network byte order and osmo_sockaddr_str_from_32h for host byte order. */ +int osmo_sockaddr_str_from_32n(struct osmo_sockaddr_str *sockaddr_str, uint32_t ip, uint16_t port) +{ + return osmo_sockaddr_str_from_32h(sockaddr_str, ip, port); +} + /*! Convert IPv4 address and port to osmo_sockaddr_str. * \param[out] sockaddr_str The instance to copy to. * \param[in] src IPv4 address and port data. @@ -262,6 +360,17 @@ int osmo_sockaddr_str_from_sockaddr(struct osmo_sockaddr_str *sockaddr_str, cons return -EINVAL; } +/*! Convert IPv4 or IPv6 address and port to osmo_sockaddr_str. + * \param[out] sockaddr_str The instance to copy to. + * \param[in] src IPv4 or IPv6 address and port data. + * \return 0 on success, negative if src does not indicate AF_INET nor AF_INET6 (or if the conversion fails, which + * should not be possible in practice). + */ +int osmo_sockaddr_str_from_osa(struct osmo_sockaddr_str *sockaddr_str, const struct osmo_sockaddr *src) +{ + return osmo_sockaddr_str_from_sockaddr(sockaddr_str, &src->u.sas); +} + /*! Convert osmo_sockaddr_str address string to IPv4 address data. * \param[in] sockaddr_str The instance to convert the IP of. * \param[out] dst IPv4 address data to write to. @@ -302,9 +411,9 @@ int osmo_sockaddr_str_to_in6_addr(const struct osmo_sockaddr_str *sockaddr_str, return 0; } -/*! Convert osmo_sockaddr_str address string to IPv4 address data in host-byte-order. +/*! Convert osmo_sockaddr_str address string to IPv4 address data in network-byte-order. * \param[in] sockaddr_str The instance to convert the IP of. - * \param[out] dst IPv4 address data in 32bit host-byte-order format to write to. + * \param[out] dst IPv4 address data in 32bit network-byte-order format to write to. * \return 0 on success, negative on error (e.g. invalid IPv4 address string). */ int osmo_sockaddr_str_to_32(const struct osmo_sockaddr_str *sockaddr_str, uint32_t *ip) @@ -322,12 +431,13 @@ int osmo_sockaddr_str_to_32(const struct osmo_sockaddr_str *sockaddr_str, uint32 return 0; } -/*! Convert osmo_sockaddr_str address string to IPv4 address data in network-byte-order. +/*! Convert osmo_sockaddr_str address string to IPv4 address data in host-byte-order. + * For legacy reasons, this function has a misleading 'n' in its name. * \param[in] sockaddr_str The instance to convert the IP of. - * \param[out] dst IPv4 address data in 32bit network-byte-order format to write to. + * \param[out] dst IPv4 address data in 32bit host-byte-order format to write to. * \return 0 on success, negative on error (e.g. invalid IPv4 address string). */ -int osmo_sockaddr_str_to_32n(const struct osmo_sockaddr_str *sockaddr_str, uint32_t *ip) +int osmo_sockaddr_str_to_32h(const struct osmo_sockaddr_str *sockaddr_str, uint32_t *ip) { int rc; uint32_t ip_h; @@ -342,6 +452,13 @@ int osmo_sockaddr_str_to_32n(const struct osmo_sockaddr_str *sockaddr_str, uint3 return 0; } +/*! DEPRECATED: the name suggests a conversion to network byte order, but actually converts to host byte order. Use + * osmo_sockaddr_str_to_32() for network byte order and osmo_sockaddr_str_to_32h() for host byte order. */ +int osmo_sockaddr_str_to_32n(const struct osmo_sockaddr_str *sockaddr_str, uint32_t *ip) +{ + return osmo_sockaddr_str_to_32h(sockaddr_str, ip); +} + /*! Convert osmo_sockaddr_str address string and port to IPv4 address and port data. * \param[in] sockaddr_str The instance to convert the IP and port of. * \param[out] dst IPv4 address and port data to write to. @@ -404,5 +521,15 @@ int osmo_sockaddr_str_to_sockaddr(const struct osmo_sockaddr_str *sockaddr_str, } } +/*! Convert osmo_sockaddr_str address string and port to IPv4 or IPv6 address and port data. + * \param[in] sockaddr_str The instance to convert the IP and port of. + * \param[out] dst IPv4/IPv6 address and port data to write to. + * \return 0 on success, negative on error (e.g. invalid IP address string for the family indicated by sockaddr_str->af). + */ +int osmo_sockaddr_str_to_osa(const struct osmo_sockaddr_str *sockaddr_str, struct osmo_sockaddr *dst) +{ + return osmo_sockaddr_str_to_sockaddr(sockaddr_str, &dst->u.sas); +} + /*! @} */ #endif // HAVE_NETINET_IN_H diff --git a/src/core/socket.c b/src/core/socket.c new file mode 100644 index 00000000..80a9d0ec --- /dev/null +++ b/src/core/socket.c @@ -0,0 +1,2803 @@ +/* + * (C) 2011-2017 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include "config.h" + +/*! \addtogroup socket + * @{ + * Osmocom socket convenience functions. + * + * \file socket.c */ + +#ifdef HAVE_SYS_SOCKET_H +#define _GNU_SOURCE /* for struct ucred on glibc >= 2.8 */ + +#include <osmocom/core/logging.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> + +#include <sys/ioctl.h> +#include <sys/socket.h> +#include <sys/types.h> +#include <sys/un.h> +#include <net/if.h> + +#include <netinet/in.h> +#include <arpa/inet.h> + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <string.h> +#include <errno.h> +#include <netdb.h> +#include <ifaddrs.h> + +#ifdef HAVE_LIBSCTP +#include <netinet/sctp.h> +#endif + +static struct addrinfo *addrinfo_helper(uint16_t family, uint16_t type, uint8_t proto, + const char *host, uint16_t port, bool passive) +{ + struct addrinfo hints, *result, *rp; + char portbuf[6]; + int rc; + + snprintf(portbuf, sizeof(portbuf), "%u", port); + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = family; + if (type == SOCK_RAW) { + /* Workaround for glibc, that returns EAI_SERVICE (-8) if + * SOCK_RAW and IPPROTO_GRE is used. + * http://sourceware.org/bugzilla/show_bug.cgi?id=15015 + */ + hints.ai_socktype = SOCK_DGRAM; + hints.ai_protocol = IPPROTO_UDP; + } else { + hints.ai_socktype = type; + hints.ai_protocol = proto; + } + + if (passive) + hints.ai_flags |= AI_PASSIVE; + + rc = getaddrinfo(host, portbuf, &hints, &result); + if (rc != 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "getaddrinfo(%s, %u) failed: %s\n", + host, port, gai_strerror(rc)); + return NULL; + } + + for (rp = result; rp != NULL; rp = rp->ai_next) { + /* Workaround for glibc again */ + if (type == SOCK_RAW) { + rp->ai_socktype = SOCK_RAW; + rp->ai_protocol = proto; + } + } + + return result; +} + +#ifdef HAVE_LIBSCTP +/*! Retrieve an array of addrinfo with specified hints, one for each host in the hosts array. + * \param[out] addrinfo array of addrinfo pointers, will be filled by the function on success. + * Its size must be at least the one of hosts. + * \param[in] family Socket family like AF_INET, AF_INET6. + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM. + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP. + * \param[in] hosts array of char pointers (strings) containing the addresses to query. + * \param[in] host_cnt length of the hosts array (in items). + * \param[in] port port number in host byte order. + * \param[in] passive whether to include the AI_PASSIVE flag in getaddrinfo() hints. + * \returns 0 is returned on success together with a filled addrinfo array; negative on error + */ +static int addrinfo_helper_multi(struct addrinfo **addrinfo, uint16_t family, uint16_t type, uint8_t proto, + const char **hosts, size_t host_cnt, uint16_t port, bool passive) +{ + unsigned int i, j; + + for (i = 0; i < host_cnt; i++) { + addrinfo[i] = addrinfo_helper(family, type, proto, hosts[i], port, passive); + if (!addrinfo[i]) { + for (j = 0; j < i; j++) + freeaddrinfo(addrinfo[j]); + return -EINVAL; + } + } + return 0; +} +#endif /* HAVE_LIBSCTP*/ + +static int socket_helper_tail(int sfd, unsigned int flags) +{ + int rc, on = 1; + uint8_t dscp = GET_OSMO_SOCK_F_DSCP(flags); + uint8_t prio = GET_OSMO_SOCK_F_PRIO(flags); + + if (flags & OSMO_SOCK_F_NONBLOCK) { + if (ioctl(sfd, FIONBIO, (unsigned char *)&on) < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot set this socket unblocking: %s\n", + strerror(errno)); + close(sfd); + return -EINVAL; + } + } + + if (dscp) { + rc = osmo_sock_set_dscp(sfd, dscp); + if (rc) { + LOGP(DLGLOBAL, LOGL_ERROR, "cannot set IP DSCP of socket to %u: %s\n", + dscp, strerror(errno)); + /* we consider this a non-fatal error */ + } + } + + if (prio) { + rc = osmo_sock_set_priority(sfd, prio); + if (rc) { + LOGP(DLGLOBAL, LOGL_ERROR, "cannot set priority of socket to %u: %s\n", + prio, strerror(errno)); + /* we consider this a non-fatal error */ + } + } + + return 0; +} + +static int socket_helper(const struct addrinfo *rp, unsigned int flags) +{ + int sfd, rc; + + sfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); + if (sfd == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, + "unable to create socket: %s\n", strerror(errno)); + return sfd; + } + + rc = socket_helper_tail(sfd, flags); + if (rc < 0) + return rc; + + return sfd; +} + +static int socket_helper_osa(const struct osmo_sockaddr *addr, uint16_t type, uint8_t proto, unsigned int flags) +{ + int sfd, rc; + + sfd = socket(addr->u.sa.sa_family, type, proto); + if (sfd == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, + "unable to create socket: %s\n", strerror(errno)); + return sfd; + } + + rc = socket_helper_tail(sfd, flags); + if (rc < 0) + return rc; + + return sfd; +} + +#ifdef HAVE_LIBSCTP +/* Fill buf with a string representation of the address set, in the form: + * buf_len == 0: "()" + * buf_len == 1: "hostA" + * buf_len >= 2: (hostA|hostB|...|...) + */ +static int multiaddr_snprintf(char* buf, size_t buf_len, const char **hosts, size_t host_cnt) +{ + int len = 0, offset = 0, rem = buf_len; + size_t i; + int ret; + char *after; + + if (buf_len < 3) + return -EINVAL; + + if (host_cnt != 1) { + ret = snprintf(buf, rem, "("); + if (ret < 0) + return ret; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + } + for (i = 0; i < host_cnt; i++) { + if (host_cnt == 1) + after = ""; + else + after = (i == (host_cnt - 1)) ? ")" : "|"; + ret = snprintf(buf + offset, rem, "%s%s", hosts[i] ? : "0.0.0.0", after); + OSMO_SNPRINTF_RET(ret, rem, offset, len); + } + + return len; +} +#endif /* HAVE_LIBSCTP */ + +static int osmo_sock_init_tail(int fd, uint16_t type, unsigned int flags) +{ + int rc; + + /* Make sure to call 'listen' on a bound, connection-oriented sock */ + if ((flags & (OSMO_SOCK_F_BIND|OSMO_SOCK_F_CONNECT)) == OSMO_SOCK_F_BIND) { + switch (type) { + case SOCK_STREAM: + case SOCK_SEQPACKET: + rc = listen(fd, 10); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to listen on socket: %s\n", + strerror(errno)); + return -errno; + } + break; + } + } + + if (flags & OSMO_SOCK_F_NO_MCAST_LOOP) { + rc = osmo_sock_mcast_loop_set(fd, false); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to disable multicast loop: %s\n", + strerror(errno)); + return rc; + } + } + + if (flags & OSMO_SOCK_F_NO_MCAST_ALL) { + rc = osmo_sock_mcast_all_set(fd, false); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to disable receive of all multicast: %s\n", + strerror(errno)); + /* do not abort here, as this is just an + * optional additional optimization that only + * exists on Linux only */ + } + } + return 0; +} + +/*! Initialize a socket (including bind and/or connect) + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] local_host local host name or IP address in string form + * \param[in] local_port local port number in host byte order + * \param[in] remote_host remote host name or IP address in string form + * \param[in] remote_port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket file descriptor on success; negative on error + * + * This function creates a new socket of the designated \a family, \a + * type and \a proto and optionally binds it to the \a local_host and \a + * local_port as well as optionally connects it to the \a remote_host + * and \q remote_port, depending on the value * of \a flags parameter. + * + * As opposed to \ref osmo_sock_init(), this function allows to combine + * the \ref OSMO_SOCK_F_BIND and \ref OSMO_SOCK_F_CONNECT flags. This + * is useful if you want to connect to a remote host/port, but still + * want to bind that socket to either a specific local alias IP and/or a + * specific local source port. + * + * You must specify either \ref OSMO_SOCK_F_BIND, or \ref + * OSMO_SOCK_F_CONNECT, or both. + * + * If \ref OSMO_SOCK_F_NONBLOCK is specified, the socket will be set to + * non-blocking mode. + */ +int osmo_sock_init2(uint16_t family, uint16_t type, uint8_t proto, + const char *local_host, uint16_t local_port, + const char *remote_host, uint16_t remote_port, unsigned int flags) +{ + struct addrinfo *local = NULL, *remote = NULL, *rp; + int sfd = -1, rc, on = 1; + + bool local_ipv4 = false, local_ipv6 = false; + bool remote_ipv4 = false, remote_ipv6 = false; + + if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid: you have to specify either " + "BIND or CONNECT flags\n"); + return -EINVAL; + } + + /* figure out local address infos */ + if (flags & OSMO_SOCK_F_BIND) { + local = addrinfo_helper(family, type, proto, local_host, local_port, true); + if (!local) + return -EINVAL; + } + + /* figure out remote address infos */ + if (flags & OSMO_SOCK_F_CONNECT) { + remote = addrinfo_helper(family, type, proto, remote_host, remote_port, false); + if (!remote) { + if (local) + freeaddrinfo(local); + + return -EINVAL; + } + } + + /* It must do a full run to ensure AF_UNSPEC does not fail. + * In case first local valid entry is IPv4 and only remote valid entry + * is IPv6 or vice versa */ + if (family == AF_UNSPEC) { + for (rp = local; rp != NULL; rp = rp->ai_next) { + switch (rp->ai_family) { + case AF_INET: + local_ipv4 = true; + break; + case AF_INET6: + local_ipv6 = true; + break; + } + } + + for (rp = remote; rp != NULL; rp = rp->ai_next) { + switch (rp->ai_family) { + case AF_INET: + remote_ipv4 = true; + break; + case AF_INET6: + remote_ipv6 = true; + break; + } + } + + if ((flags & OSMO_SOCK_F_BIND) && (flags & OSMO_SOCK_F_CONNECT)) { + /* prioritize ipv6 as per RFC */ + if (local_ipv6 && remote_ipv6) + family = AF_INET6; + else if (local_ipv4 && remote_ipv4) + family = AF_INET; + else { + if (local) + freeaddrinfo(local); + if (remote) + freeaddrinfo(remote); + LOGP(DLGLOBAL, LOGL_ERROR, + "Unable to find a common protocol (IPv4 or IPv6) " + "for local host: %s and remote host: %s.\n", + local_host, remote_host); + return -ENODEV; + } + } else if ((flags & OSMO_SOCK_F_BIND)) { + family = local_ipv6 ? AF_INET6 : AF_INET; + } else if ((flags & OSMO_SOCK_F_CONNECT)) { + family = remote_ipv6 ? AF_INET6 : AF_INET; + } + } + + /* figure out local side of socket */ + if (flags & OSMO_SOCK_F_BIND) { + for (rp = local; rp != NULL; rp = rp->ai_next) { + /* When called with AF_UNSPEC, family will set to IPv4 or IPv6 */ + if (rp->ai_family != family) + continue; + + sfd = socket_helper(rp, flags); + if (sfd < 0) + continue; + + if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { + rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, + &on, sizeof(on)); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt socket:" + " %s:%u: %s\n", + local_host, local_port, + strerror(errno)); + close(sfd); + continue; + } + } + + if (bind(sfd, rp->ai_addr, rp->ai_addrlen) == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to bind socket: %s:%u: %s\n", + local_host, local_port, strerror(errno)); + close(sfd); + continue; + } + break; + } + + freeaddrinfo(local); + if (rp == NULL) { + if (remote) + freeaddrinfo(remote); + LOGP(DLGLOBAL, LOGL_ERROR, "no suitable local addr found for: %s:%u\n", + local_host, local_port); + return -ENODEV; + } + } + + /* Reached this point, if OSMO_SOCK_F_BIND then sfd is valid (>=0) or it + was already closed and func returned. If OSMO_SOCK_F_BIND is not + set, then sfd = -1 */ + + /* figure out remote side of socket */ + if (flags & OSMO_SOCK_F_CONNECT) { + for (rp = remote; rp != NULL; rp = rp->ai_next) { + /* When called with AF_UNSPEC, family will set to IPv4 or IPv6 */ + if (rp->ai_family != family) + continue; + + if (sfd < 0) { + sfd = socket_helper(rp, flags); + if (sfd < 0) + continue; + } + + rc = connect(sfd, rp->ai_addr, rp->ai_addrlen); + if (rc != 0 && errno != EINPROGRESS) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to connect socket: %s:%u: %s\n", + remote_host, remote_port, strerror(errno)); + /* We want to maintain the bind socket if bind was enabled */ + if (!(flags & OSMO_SOCK_F_BIND)) { + close(sfd); + sfd = -1; + } + continue; + } + break; + } + + freeaddrinfo(remote); + if (rp == NULL) { + LOGP(DLGLOBAL, LOGL_ERROR, "no suitable remote addr found for: %s:%u\n", + remote_host, remote_port); + if (sfd >= 0) + close(sfd); + return -ENODEV; + } + } + + rc = osmo_sock_init_tail(sfd, type, flags); + if (rc < 0) { + close(sfd); + sfd = -1; + } + + return sfd; +} + +#define _SOCKADDR_TO_STR(dest, sockaddr) do { \ + if (osmo_sockaddr_str_from_sockaddr(dest, &sockaddr->u.sas)) \ + osmo_strlcpy((dest)->ip, "Invalid IP", 11); \ + } while (0) + +/*! Initialize a socket (including bind and/or connect) + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] local local address + * \param[in] remote remote address + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket file descriptor on success; negative on error + * + * This function creates a new socket of the + * \a type and \a proto and optionally binds it to the \a local + * as well as optionally connects it to the \a remote + * depending on the value * of \a flags parameter. + * + * As opposed to \ref osmo_sock_init(), this function allows to combine + * the \ref OSMO_SOCK_F_BIND and \ref OSMO_SOCK_F_CONNECT flags. This + * is useful if you want to connect to a remote host/port, but still + * want to bind that socket to either a specific local alias IP and/or a + * specific local source port. + * + * You must specify either \ref OSMO_SOCK_F_BIND, or \ref + * OSMO_SOCK_F_CONNECT, or both. + * + * If \ref OSMO_SOCK_F_NONBLOCK is specified, the socket will be set to + * non-blocking mode. + */ +int osmo_sock_init_osa(uint16_t type, uint8_t proto, + const struct osmo_sockaddr *local, + const struct osmo_sockaddr *remote, + unsigned int flags) +{ + int sfd = -1, rc, on = 1; + struct osmo_sockaddr_str _sastr = {}; + struct osmo_sockaddr_str *sastr = &_sastr; + + if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid: you have to specify either " + "BIND or CONNECT flags\n"); + return -EINVAL; + } + + if ((flags & OSMO_SOCK_F_BIND) && !local) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid argument. Cannot BIND when local is NULL\n"); + return -EINVAL; + } + + if ((flags & OSMO_SOCK_F_CONNECT) && !remote) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid argument. Cannot CONNECT when remote is NULL\n"); + return -EINVAL; + } + + if ((flags & OSMO_SOCK_F_BIND) && + (flags & OSMO_SOCK_F_CONNECT) && + local->u.sa.sa_family != remote->u.sa.sa_family) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid: the family for " + "local and remote endpoint must be same.\n"); + return -EINVAL; + } + + /* figure out local side of socket */ + if (flags & OSMO_SOCK_F_BIND) { + sfd = socket_helper_osa(local, type, proto, flags); + if (sfd < 0) { + _SOCKADDR_TO_STR(sastr, local); + LOGP(DLGLOBAL, LOGL_ERROR, "no suitable local addr found for: " OSMO_SOCKADDR_STR_FMT "\n", + OSMO_SOCKADDR_STR_FMT_ARGS(sastr)); + return -ENODEV; + } + + if (proto != IPPROTO_UDP || (flags & OSMO_SOCK_F_UDP_REUSEADDR)) { + rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, + &on, sizeof(on)); + if (rc < 0) { + int err = errno; + _SOCKADDR_TO_STR(sastr, local); + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt socket: " OSMO_SOCKADDR_STR_FMT ": %s\n", + OSMO_SOCKADDR_STR_FMT_ARGS(sastr), strerror(err)); + close(sfd); + return rc; + } + } + + if (bind(sfd, &local->u.sa, sizeof(struct osmo_sockaddr)) == -1) { + int err = errno; + _SOCKADDR_TO_STR(sastr, local); + LOGP(DLGLOBAL, LOGL_ERROR, "unable to bind socket: " OSMO_SOCKADDR_STR_FMT ": %s\n", + OSMO_SOCKADDR_STR_FMT_ARGS(sastr), strerror(err)); + close(sfd); + return -1; + } + } + + /* Reached this point, if OSMO_SOCK_F_BIND then sfd is valid (>=0) or it + was already closed and func returned. If OSMO_SOCK_F_BIND is not + set, then sfd = -1 */ + + /* figure out remote side of socket */ + if (flags & OSMO_SOCK_F_CONNECT) { + if (sfd < 0) { + sfd = socket_helper_osa(remote, type, proto, flags); + if (sfd < 0) { + return sfd; + } + } + + rc = connect(sfd, &remote->u.sa, sizeof(struct osmo_sockaddr)); + if (rc != 0 && errno != EINPROGRESS) { + int err = errno; + _SOCKADDR_TO_STR(sastr, remote); + LOGP(DLGLOBAL, LOGL_ERROR, "unable to connect socket: " OSMO_SOCKADDR_STR_FMT ": %s\n", + OSMO_SOCKADDR_STR_FMT_ARGS(sastr), strerror(err)); + close(sfd); + return rc; + } + } + + rc = osmo_sock_init_tail(sfd, type, flags); + if (rc < 0) { + close(sfd); + sfd = -1; + } + + return sfd; +} + +#ifdef HAVE_LIBSCTP + +/* Check whether there's an addrinfo item in the addrinfo set with an IPv4 or IPv6 option */ +static void addrinfo_has_v4v6addr(const struct addrinfo **result, size_t result_count, bool *has_v4, bool *has_v6) +{ + size_t host_idx; + const struct addrinfo *rp; + *has_v4 = false; + *has_v6 = false; + + for (host_idx = 0; host_idx < result_count; host_idx++) { + for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { + if (result[host_idx]->ai_family == AF_INET) + *has_v4 = true; + else if (result[host_idx]->ai_family == AF_INET6) + *has_v6 = true; + } + } +} + +/* Check whether there's an addrinfo item in the addrinfo set with only an IPv4 or IPv6 option */ +static void addrinfo_has_v4v6only_addr(const struct addrinfo **result, size_t result_count, bool *has_v4only, bool *has_v6only) +{ + size_t host_idx; + const struct addrinfo *rp; + *has_v4only = false; + *has_v6only = false; + + for (host_idx = 0; host_idx < result_count; host_idx++) { + bool has_v4 = false; + bool has_v6 = false; + for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { + if (rp->ai_family == AF_INET6) + has_v6 = true; + else + has_v4 = true; + } + if (has_v4 && !has_v6) + *has_v4only = true; + else if (has_v6 && !has_v4) + *has_v6only = true; + } +} + +/* Check whether there's an IPv6 with IN6ADDR_ANY_INIT ("::") */ +static bool addrinfo_has_in6addr_any(const struct addrinfo **result, size_t result_count) +{ + size_t host_idx; + struct in6_addr in6addr_any = IN6ADDR_ANY_INIT; + const struct addrinfo *rp; + + for (host_idx = 0; host_idx < result_count; host_idx++) { + for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { + if (rp->ai_family != AF_INET6) + continue; + if (memcmp(&((struct sockaddr_in6 *)rp->ai_addr)->sin6_addr, + &in6addr_any, sizeof(in6addr_any)) == 0) + return true; + } + } + return false; +} + +static int socket_helper_multiaddr(uint16_t family, uint16_t type, uint8_t proto, unsigned int flags) +{ + int sfd, rc; + + sfd = socket(family, type, proto); + if (sfd == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, + "Unable to create socket: %s\n", strerror(errno)); + return sfd; + } + + rc = socket_helper_tail(sfd, flags); + if (rc < 0) + return rc; + + return sfd; +} + +/* Build array of addresses taking first addrinfo result of the requested family + * for each host in addrs_buf. */ +static int addrinfo_to_sockaddr(uint16_t family, const struct addrinfo **result, + const char **hosts, unsigned int host_cont, + uint8_t *addrs_buf, size_t addrs_buf_len) { + size_t host_idx, offset = 0; + const struct addrinfo *rp; + + for (host_idx = 0; host_idx < host_cont; host_idx++) { + /* Addresses are ordered based on RFC 3484, see man getaddrinfo */ + for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { + if (family == AF_UNSPEC || rp->ai_family == family) + break; + } + if (!rp && family == AF_INET6) { + /* See if we can find an AF_INET addr for the AF_INET6 socket instead: */ + for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { + if (rp->ai_family == AF_INET) + break; + } + } + if (!rp) { /* No addr could be bound for this host! */ + LOGP(DLGLOBAL, LOGL_ERROR, "No suitable remote address found for host: %s\n", + hosts[host_idx]); + return -ENODEV; + } + if (offset + rp->ai_addrlen > addrs_buf_len) { + LOGP(DLGLOBAL, LOGL_ERROR, "Output buffer to small: %zu\n", + addrs_buf_len); + return -ENOSPC; + } + memcpy(addrs_buf + offset, rp->ai_addr, rp->ai_addrlen); + offset += rp->ai_addrlen; + } + return 0; +} + +static int setsockopt_sctp_auth_supported(int fd, uint32_t val) +{ +#ifdef SCTP_AUTH_SUPPORTED + struct sctp_assoc_value assoc_val = { + .assoc_id = SCTP_FUTURE_ASSOC, + .assoc_value = val, + }; + return setsockopt(fd, IPPROTO_SCTP, SCTP_AUTH_SUPPORTED, &assoc_val, sizeof(assoc_val)); +#else +#pragma message "setsockopt(SCTP_AUTH_SUPPORTED) not supported! some SCTP features may not be available!" + LOGP(DLGLOBAL, LOGL_NOTICE, "Built without support for setsockopt(SCTP_AUTH_SUPPORTED), skipping\n"); + return -ENOTSUP; +#endif +} + +static int setsockopt_sctp_asconf_supported(int fd, uint32_t val) +{ +#ifdef SCTP_ASCONF_SUPPORTED + struct sctp_assoc_value assoc_val = { + .assoc_id = SCTP_FUTURE_ASSOC, + .assoc_value = val, + }; + return setsockopt(fd, IPPROTO_SCTP, SCTP_ASCONF_SUPPORTED, &assoc_val, sizeof(assoc_val)); +#else +#pragma message "setsockopt(SCTP_ASCONF_SUPPORTED) not supported! some SCTP features may not be available!" + LOGP(DLGLOBAL, LOGL_NOTICE, "Built without support for setsockopt(SCTP_ASCONF_SUPPORTED), skipping\n"); + return -ENOTSUP; +#endif +} + +static int setsockopt_sctp_initmsg(int fd, const struct osmo_sock_init2_multiaddr_pars *pars) +{ + if (!pars->sctp.sockopt_initmsg.num_ostreams_present && + !pars->sctp.sockopt_initmsg.max_instreams_present && + !pars->sctp.sockopt_initmsg.max_attempts_present && + !pars->sctp.sockopt_initmsg.max_init_timeo_present) + return 0; /* nothing to set/do */ + +#ifdef SCTP_INITMSG + struct sctp_initmsg si = {0}; + socklen_t si_len = sizeof(si); + int rc; + + /* If at least one field not present, obtain current value from kernel: */ + if (!pars->sctp.sockopt_initmsg.num_ostreams_present || + !pars->sctp.sockopt_initmsg.max_instreams_present || + !pars->sctp.sockopt_initmsg.max_attempts_present || + !pars->sctp.sockopt_initmsg.max_init_timeo_present) { + rc = getsockopt(fd, IPPROTO_SCTP, SCTP_INITMSG, &si, &si_len); + if (rc < 0) + return rc; + } + + if (pars->sctp.sockopt_initmsg.num_ostreams_present) + si.sinit_num_ostreams = pars->sctp.sockopt_initmsg.num_ostreams_value; + if (pars->sctp.sockopt_initmsg.max_instreams_present) + si.sinit_max_instreams = pars->sctp.sockopt_initmsg.max_instreams_value; + if (pars->sctp.sockopt_initmsg.max_attempts_present) + si.sinit_max_attempts = pars->sctp.sockopt_initmsg.max_attempts_value; + if (pars->sctp.sockopt_initmsg.max_init_timeo_present) + si.sinit_max_init_timeo = pars->sctp.sockopt_initmsg.max_init_timeo_value; + + return setsockopt(fd, IPPROTO_SCTP, SCTP_INITMSG, &si, sizeof(si)); +#else +#pragma message "setsockopt(SCTP_INITMSG) not supported! some SCTP features may not be available!" + LOGP(DLGLOBAL, LOGL_NOTICE, "Built without support for setsockopt(SCTP_INITMSG), skipping\n"); + return -ENOTSUP +#endif +} + +/*! Initialize a socket (including bind and/or connect) with multiple local or remote addresses. + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] local_hosts array of char pointers (strings), each containing local host name or IP address in string form + * \param[in] local_hosts_cnt length of local_hosts (in items) + * \param[in] local_port local port number in host byte order + * \param[in] remote_host array of char pointers (strings), each containing remote host name or IP address in string form + * \param[in] remote_hosts_cnt length of remote_hosts (in items) + * \param[in] remote_port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket file descriptor on success; negative on error + * + * This function is similar to \ref osmo_sock_init2(), but can be passed an + * array of local or remote addresses for protocols supporting multiple + * addresses per socket, like SCTP (currently only one supported). This function + * should not be used by protocols not supporting this kind of features, but + * rather \ref osmo_sock_init2() should be used instead. + * See \ref osmo_sock_init2() for more information on flags and general behavior. + */ +int osmo_sock_init2_multiaddr(uint16_t family, uint16_t type, uint8_t proto, + const char **local_hosts, size_t local_hosts_cnt, uint16_t local_port, + const char **remote_hosts, size_t remote_hosts_cnt, uint16_t remote_port, + unsigned int flags) +{ + return osmo_sock_init2_multiaddr2(family, type, proto, local_hosts, local_hosts_cnt, local_port, + remote_hosts, remote_hosts_cnt, remote_port, flags, NULL); +} + +/*! Initialize a socket (including bind and/or connect) with multiple local or remote addresses. + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] local_hosts array of char pointers (strings), each containing local host name or IP address in string form + * \param[in] local_hosts_cnt length of local_hosts (in items) + * \param[in] local_port local port number in host byte order + * \param[in] remote_host array of char pointers (strings), each containing remote host name or IP address in string form + * \param[in] remote_hosts_cnt length of remote_hosts (in items) + * \param[in] remote_port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \param[in] pars Extra parameters for multi-address specific protocols, such as SCTP. Can be NULL. + * \returns socket file descriptor on success; negative on error + * + * This function is similar to \ref osmo_sock_init2(), but can be passed an + * array of local or remote addresses for protocols supporting multiple + * addresses per socket, like SCTP (currently only one supported). This function + * should not be used by protocols not supporting this kind of features, but + * rather \ref osmo_sock_init2() should be used instead. + * See \ref osmo_sock_init2() for more information on flags and general behavior. + * + * pars: If "pars" parameter is passed to the function, sctp.version shall be set to 0. + */ +int osmo_sock_init2_multiaddr2(uint16_t family, uint16_t type, uint8_t proto, + const char **local_hosts, size_t local_hosts_cnt, uint16_t local_port, + const char **remote_hosts, size_t remote_hosts_cnt, uint16_t remote_port, + unsigned int flags, struct osmo_sock_init2_multiaddr_pars *pars) + +{ + struct addrinfo *res_loc[OSMO_SOCK_MAX_ADDRS], *res_rem[OSMO_SOCK_MAX_ADDRS]; + int sfd = -1, rc, on = 1; + unsigned int i; + bool loc_has_v4addr = false, loc_has_v6addr = false; + bool rem_has_v4addr = false, rem_has_v6addr = false; + bool loc_has_v4only_addr, rem_has_v4only_addr; + bool loc_has_v6only_addr, rem_has_v6only_addr; + struct sockaddr_in6 addrs_buf[OSMO_SOCK_MAX_ADDRS]; + char strbuf[512]; + + /* TODO: So far this function is only aimed for SCTP, but could be + reused in the future for other protocols with multi-addr support */ + if (proto != IPPROTO_SCTP) + return -ENOTSUP; + + if (pars && pars->sctp.version != 0) + return -EINVAL; + + if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid: you have to specify either " + "BIND or CONNECT flags\n"); + return -EINVAL; + } + + if (((flags & OSMO_SOCK_F_BIND) && !local_hosts_cnt) || + ((flags & OSMO_SOCK_F_CONNECT) && !remote_hosts_cnt) || + local_hosts_cnt > OSMO_SOCK_MAX_ADDRS || + remote_hosts_cnt > OSMO_SOCK_MAX_ADDRS) + return -EINVAL; + + /* figure out local side of socket */ + if (flags & OSMO_SOCK_F_BIND) { + rc = addrinfo_helper_multi(res_loc, family, type, proto, local_hosts, + local_hosts_cnt, local_port, true); + if (rc < 0) + return -EINVAL; + /* Figure out if there's any IPv4 or IPv6 entry in the result set */ + addrinfo_has_v4v6addr((const struct addrinfo **)res_loc, local_hosts_cnt, + &loc_has_v4addr, &loc_has_v6addr); + /* Figure out if there's any IPv4-only or IPv6-only addr in the result set */ + addrinfo_has_v4v6only_addr((const struct addrinfo **)res_loc, local_hosts_cnt, + &loc_has_v4only_addr, &loc_has_v6only_addr); + if (family == AF_INET && loc_has_v6only_addr) { + LOGP(DLGLOBAL, LOGL_ERROR, "Cannot bind an IPv6 address to an AF_INET socket\n"); + rc = -EINVAL; + goto ret_freeaddrinfo_loc; + } + } + /* figure out remote side of socket */ + if (flags & OSMO_SOCK_F_CONNECT) { + rc = addrinfo_helper_multi(res_rem, family, type, proto, remote_hosts, + remote_hosts_cnt, remote_port, false); + if (rc < 0) { + rc = -EINVAL; + goto ret_freeaddrinfo_loc; + } + /* Figure out if there's any IPv4 or IPv6 entry in the result set */ + addrinfo_has_v4v6addr((const struct addrinfo **)res_rem, remote_hosts_cnt, + &rem_has_v4addr, &rem_has_v6addr); + /* Figure out if there's any IPv4-only or IPv6-only addr in the result set */ + addrinfo_has_v4v6only_addr((const struct addrinfo **)res_rem, remote_hosts_cnt, + &rem_has_v4only_addr, &rem_has_v6only_addr); + if (family == AF_INET && rem_has_v6only_addr) { + LOGP(DLGLOBAL, LOGL_ERROR, "Cannot connect to an IPv6 address in an AF_INET socket\n"); + rc = -EINVAL; + goto ret_freeaddrinfo; + } + } + + /* Find out the socket family now if not established by caller: + * Both are checked here through "or" here to account for "bind flag set, + * connect flag not set" and viceversa. */ + if (family == AF_UNSPEC) { + if (!loc_has_v6addr && !rem_has_v6addr) + family = AF_INET; + else + family = AF_INET6; + } + + /* if both sets are used, make sure there's at least 1 address of the + * same type on each set so that SCTP INIT/INIT-ACK can work. */ + if (family == AF_INET6 && ((flags & OSMO_SOCK_F_BIND) && (flags & OSMO_SOCK_F_CONNECT)) && + (loc_has_v4addr != rem_has_v4addr || loc_has_v6addr != rem_has_v6addr)) { + if (!addrinfo_has_in6addr_any((const struct addrinfo **)res_loc, local_hosts_cnt)) { + LOGP(DLGLOBAL, LOGL_ERROR, "Invalid v4 vs v6 in local vs remote addresses: " + "local:%s%s remote:%s%s\n", + loc_has_v4addr ? " v4" : "", loc_has_v6addr ? " v6" : "", + rem_has_v4addr ? " v4" : "", rem_has_v6addr ? " v6" : ""); + rc = -EINVAL; + goto ret_freeaddrinfo; + } + } + + sfd = socket_helper_multiaddr(family, type, proto, flags); + if (sfd < 0) { + rc = sfd; + goto ret_freeaddrinfo; + } + + if (pars) { + if (pars->sctp.sockopt_auth_supported.set) { + /* RFC 5061 4.2.7: ASCONF also requires AUTH feature. */ + rc = setsockopt_sctp_auth_supported(sfd, pars->sctp.sockopt_auth_supported.value); + if (rc < 0) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt(SCTP_AUTH_SUPPORTED) socket: %s:%u: %s\n", + strbuf, local_port, strerror(err)); + if (pars->sctp.sockopt_auth_supported.abort_on_failure) + goto ret_close; + /* do not fail, some features such as Peer Primary Address won't be available + * unless configured system-wide through sysctl */ + } + } + + if (pars->sctp.sockopt_asconf_supported.set) { + rc = setsockopt_sctp_asconf_supported(sfd, pars->sctp.sockopt_asconf_supported.value); + if (rc < 0) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt(SCTP_ASCONF_SUPPORTED) socket: %s:%u: %s\n", + strbuf, local_port, strerror(err)); + if (pars->sctp.sockopt_asconf_supported.abort_on_failure) + goto ret_close; + /* do not fail, some features such as Peer Primary Address won't be available + * unless configured system-wide through sysctl */ + } + } + + if (pars->sctp.sockopt_initmsg.set) { + rc = setsockopt_sctp_initmsg(sfd, pars); + if (rc < 0) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt(SCTP_INITMSG) socket: %s:%u: %s\n", + strbuf, local_port, strerror(err)); + if (pars->sctp.sockopt_initmsg.abort_on_failure) + goto ret_close; + /* do not fail, some parameters will be left as the global default */ + } + } + } + + if (flags & OSMO_SOCK_F_BIND) { + /* Since so far we only allow IPPROTO_SCTP in this function, + no need to check below for "proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR" */ + rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, + &on, sizeof(on)); + if (rc < 0) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt socket:" + " %s:%u: %s\n", + strbuf, local_port, + strerror(err)); + goto ret_close; + } + + /* Build array of addresses taking first entry for each host. + TODO: Ideally we should use backtracking storing last used + indexes and trying next combination if connect() fails .*/ + /* We could alternatively use v4v6 mapped addresses and call sctp_bindx once with an array od sockaddr_in6 */ + rc = addrinfo_to_sockaddr(family, (const struct addrinfo **)res_loc, + local_hosts, local_hosts_cnt, + (uint8_t*)addrs_buf, sizeof(addrs_buf)); + if (rc < 0) { + rc = -ENODEV; + goto ret_close; + } + + rc = sctp_bindx(sfd, (struct sockaddr *)addrs_buf, local_hosts_cnt, SCTP_BINDX_ADD_ADDR); + if (rc == -1) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); + LOGP(DLGLOBAL, LOGL_NOTICE, "unable to bind socket: %s:%u: %s\n", + strbuf, local_port, strerror(err)); + rc = -ENODEV; + goto ret_close; + } + } + + if (flags & OSMO_SOCK_F_CONNECT) { + /* Build array of addresses taking first of same family for each host. + TODO: Ideally we should use backtracking storing last used + indexes and trying next combination if connect() fails .*/ + rc = addrinfo_to_sockaddr(family, (const struct addrinfo **)res_rem, + remote_hosts, remote_hosts_cnt, + (uint8_t*)addrs_buf, sizeof(addrs_buf)); + if (rc < 0) { + rc = -ENODEV; + goto ret_close; + } + + rc = sctp_connectx(sfd, (struct sockaddr *)addrs_buf, remote_hosts_cnt, NULL); + if (rc != 0 && errno != EINPROGRESS) { + int err = errno; + multiaddr_snprintf(strbuf, sizeof(strbuf), remote_hosts, remote_hosts_cnt); + LOGP(DLGLOBAL, LOGL_ERROR, "unable to connect socket: %s:%u: %s\n", + strbuf, remote_port, strerror(err)); + rc = -ENODEV; + goto ret_close; + } + } + + rc = osmo_sock_init_tail(sfd, type, flags); + if (rc < 0) { + close(sfd); + sfd = -1; + } + + rc = sfd; + goto ret_freeaddrinfo; + +ret_close: + if (sfd >= 0) + close(sfd); +ret_freeaddrinfo: + if (flags & OSMO_SOCK_F_CONNECT) { + for (i = 0; i < remote_hosts_cnt; i++) + freeaddrinfo(res_rem[i]); + } +ret_freeaddrinfo_loc: + if (flags & OSMO_SOCK_F_BIND) { + for (i = 0; i < local_hosts_cnt; i++) + freeaddrinfo(res_loc[i]); + } + return rc; +} +#endif /* HAVE_LIBSCTP */ + +/*! Initialize a socket (including bind/connect) + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] host remote host name or IP address in string form + * \param[in] port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket file descriptor on success; negative on error + * + * This function creates a new socket of the designated \a family, \a + * type and \a proto and optionally binds or connects it, depending on + * the value of \a flags parameter. + */ +int osmo_sock_init(uint16_t family, uint16_t type, uint8_t proto, + const char *host, uint16_t port, unsigned int flags) +{ + struct addrinfo *result, *rp; + int sfd = -1; /* initialize to avoid uninitialized false warnings on some gcc versions (11.1.0) */ + int on = 1; + int rc; + + if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == + (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) { + LOGP(DLGLOBAL, LOGL_ERROR, "invalid: both bind and connect flags set:" + " %s:%u\n", host, port); + return -EINVAL; + } + + result = addrinfo_helper(family, type, proto, host, port, flags & OSMO_SOCK_F_BIND); + if (!result) + return -EINVAL; + + for (rp = result; rp != NULL; rp = rp->ai_next) { + sfd = socket_helper(rp, flags); + if (sfd == -1) + continue; + + if (flags & OSMO_SOCK_F_CONNECT) { + rc = connect(sfd, rp->ai_addr, rp->ai_addrlen); + if (rc != 0 && errno != EINPROGRESS) { + close(sfd); + continue; + } + } else { + if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { + rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, + &on, sizeof(on)); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt socket:" + " %s:%u: %s\n", + host, port, strerror(errno)); + close(sfd); + continue; + } + } + if (bind(sfd, rp->ai_addr, rp->ai_addrlen) == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, "unable to bind socket:" + "%s:%u: %s\n", + host, port, strerror(errno)); + close(sfd); + continue; + } + } + break; + } + freeaddrinfo(result); + + if (rp == NULL) { + LOGP(DLGLOBAL, LOGL_ERROR, "no suitable addr found for: %s:%u\n", + host, port); + return -ENODEV; + } + + if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { + rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); + if (rc < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, + "cannot setsockopt socket: %s:%u: %s\n", host, + port, strerror(errno)); + close(sfd); + sfd = -1; + } + } + + rc = osmo_sock_init_tail(sfd, type, flags); + if (rc < 0) { + close(sfd); + sfd = -1; + } + + return sfd; +} + +/*! fill \ref osmo_fd for a give sfd + * \param[out] ofd file descriptor (will be filled in) + * \param[in] sfd socket file descriptor + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function fills the \a ofd structure. + */ +static inline int osmo_fd_init_ofd(struct osmo_fd *ofd, int sfd, unsigned int flags) +{ + int rc; + + if (sfd < 0) + return sfd; + + ofd->fd = sfd; + ofd->when = OSMO_FD_READ; + + /* if we're doing a non-blocking connect, the completion will be signaled + * by marking the fd as WRITE-able. So in this exceptional case, we're + * also interested in when the socket becomes write-able */ + if ((flags & (OSMO_SOCK_F_CONNECT|OSMO_SOCK_F_NONBLOCK)) == + (OSMO_SOCK_F_CONNECT|OSMO_SOCK_F_NONBLOCK)) { + ofd->when |= OSMO_FD_WRITE; + } + + rc = osmo_fd_register(ofd); + if (rc < 0) { + close(sfd); + return rc; + } + + return sfd; +} + +/*! Initialize a socket and fill \ref osmo_fd + * \param[out] ofd file descriptor (will be filled in) + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] host remote host name or IP address in string form + * \param[in] port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function creates (and optionall binds/connects) a socket using + * \ref osmo_sock_init, but also fills the \a ofd structure. + */ +int osmo_sock_init_ofd(struct osmo_fd *ofd, int family, int type, int proto, + const char *host, uint16_t port, unsigned int flags) +{ + return osmo_fd_init_ofd(ofd, osmo_sock_init(family, type, proto, host, port, flags), flags); +} + +/*! Initialize a socket and fill \ref osmo_fd + * \param[out] ofd file descriptor (will be filled in) + * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] local_host local host name or IP address in string form + * \param[in] local_port local port number in host byte order + * \param[in] remote_host remote host name or IP address in string form + * \param[in] remote_port remote port number in host byte order + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function creates (and optionall binds/connects) a socket using + * \ref osmo_sock_init2, but also fills the \a ofd structure. + */ +int osmo_sock_init2_ofd(struct osmo_fd *ofd, int family, int type, int proto, + const char *local_host, uint16_t local_port, + const char *remote_host, uint16_t remote_port, unsigned int flags) +{ + return osmo_fd_init_ofd(ofd, osmo_sock_init2(family, type, proto, local_host, + local_port, remote_host, remote_port, flags), flags); +} + +int osmo_sock_init_osa_ofd(struct osmo_fd *ofd, int type, int proto, + const struct osmo_sockaddr *local, + const struct osmo_sockaddr *remote, unsigned int flags) +{ + return osmo_fd_init_ofd(ofd, osmo_sock_init_osa(type, proto, local, remote, flags), flags); +} + +/*! Initialize a socket and fill \ref sockaddr + * \param[out] ss socket address (will be filled in) + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function creates (and optionall binds/connects) a socket using + * \ref osmo_sock_init, but also fills the \a ss structure. + */ +int osmo_sock_init_sa(struct sockaddr *ss, uint16_t type, + uint8_t proto, unsigned int flags) +{ + char host[NI_MAXHOST]; + uint16_t port; + struct sockaddr_in *sin; + struct sockaddr_in6 *sin6; + int s, sa_len; + + /* determine port and host from ss */ + switch (ss->sa_family) { + case AF_INET: + sin = (struct sockaddr_in *) ss; + sa_len = sizeof(struct sockaddr_in); + port = ntohs(sin->sin_port); + break; + case AF_INET6: + sin6 = (struct sockaddr_in6 *) ss; + sa_len = sizeof(struct sockaddr_in6); + port = ntohs(sin6->sin6_port); + break; + default: + return -EINVAL; + } + + s = getnameinfo(ss, sa_len, host, NI_MAXHOST, + NULL, 0, NI_NUMERICHOST); + if (s != 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "getnameinfo failed:" + " %s\n", strerror(errno)); + return s; + } + + return osmo_sock_init(ss->sa_family, type, proto, host, port, flags); +} + +#ifdef HAVE_LIBSCTP +/*! Add addresses to the multi-address (SCTP) socket active binding set + * \param[in] sfd The multi-address (SCTP) socket + * \param[in] addrs array of char pointers (strings), each containing local host name or IP address in string form + * \param[in] addrs_cnt length of addrs_hosts (in items) + * \returns 0 on success; negative on error + * + * This function only supports SCTP sockets so far, and hence it should be + * called only on socket file descriptions referencing that kind of sockets. + */ +int osmo_sock_multiaddr_add_local_addr(int sfd, const char **addrs, size_t addrs_cnt) +{ + struct osmo_sockaddr osa; + socklen_t slen = sizeof(osa); + uint16_t sfd_family; + uint16_t type = SOCK_STREAM ;/* Fixme: we assume fd is SOCK_STREAM */ + uint8_t proto = IPPROTO_SCTP; /* Fixme: we assume fd is IPPROTO_SCTP */ + struct addrinfo *res[OSMO_SOCK_MAX_ADDRS]; + uint16_t port; + struct sockaddr_in6 addrs_buf[OSMO_SOCK_MAX_ADDRS]; + unsigned int i; + int rc; + bool res_has_v4addr = false, res_has_v6addr = false; + + rc = getsockname(sfd, &osa.u.sa, &slen); + if (rc < 0) + return rc; /* TODO: log error? */ + sfd_family = osa.u.sa.sa_family; + port = osmo_sockaddr_port(&osa.u.sa); + + if (sfd_family != AF_INET && sfd_family != AF_INET6) + return -EINVAL; + + rc = addrinfo_helper_multi(res, AF_UNSPEC, type, proto, addrs, + addrs_cnt, port, true); + if (rc < 0) + return -EINVAL; + + addrinfo_has_v4v6addr((const struct addrinfo **)res, addrs_cnt, + &res_has_v4addr, &res_has_v6addr); + if (sfd_family == AF_INET && !res_has_v4addr) { + rc = -EINVAL; + goto ret_free; + } + + uint16_t new_addr_family; + if (sfd_family == AF_INET) + new_addr_family = AF_INET; + else if (sfd_family == AF_INET6 && !res_has_v4addr) + new_addr_family = AF_INET6; + else + new_addr_family = AF_UNSPEC; + rc = addrinfo_to_sockaddr(new_addr_family, (const struct addrinfo **)res, + addrs, addrs_cnt, + (uint8_t *)addrs_buf, sizeof(addrs_buf)); + if (rc < 0) { + rc = -ENODEV; + goto ret_free; + } + + rc = sctp_bindx(sfd, (struct sockaddr *)addrs_buf, addrs_cnt, SCTP_BINDX_ADD_ADDR); + if (rc == -1) { + int err = errno; + char strbuf[512]; + multiaddr_snprintf(strbuf, sizeof(strbuf), addrs, addrs_cnt); + LOGP(DLGLOBAL, LOGL_NOTICE, "Unable to bind socket to new addresses: %s:%u: %s\n", + strbuf, port, strerror(err)); + rc = -ENODEV; + goto ret_free; + } + +ret_free: + for (i = 0; i < addrs_cnt; i++) + freeaddrinfo(res[i]); + return rc; +} + +/*! Remove addresses from the multi-address (SCTP) socket active binding set + * \param[in] sfd The multi-address (SCTP) socket + * \param[in] addrs array of char pointers (strings), each containing local host name or IP address in string form + * \param[in] addrs_cnt length of addrs_hosts (in items) + * \returns 0 on success; negative on error + * + * This function only supports SCTP sockets so far, and hence it should be + * called only on socket file descriptions referencing that kind of sockets. + */ +int osmo_sock_multiaddr_del_local_addr(int sfd, const char **addrs, size_t addrs_cnt) +{ + struct osmo_sockaddr osa; + socklen_t slen = sizeof(osa); + uint16_t sfd_family; + uint16_t type = SOCK_STREAM ;/* Fixme: we assume fd is SOCK_STREAM */ + uint8_t proto = IPPROTO_SCTP; /* Fixme: we assume fd is IPPROTO_SCTP */ + struct addrinfo *res[OSMO_SOCK_MAX_ADDRS]; + uint16_t port; + struct sockaddr_in6 addrs_buf[OSMO_SOCK_MAX_ADDRS]; + unsigned int i; + int rc; + bool res_has_v4addr = false, res_has_v6addr = false; + + rc = getsockname(sfd, &osa.u.sa, &slen); + if (rc < 0) + return rc; /* TODO: log error? */ + sfd_family = osa.u.sa.sa_family; + port = osmo_sockaddr_port(&osa.u.sa); + + if (sfd_family != AF_INET && sfd_family != AF_INET6) + return -EINVAL; + + rc = addrinfo_helper_multi(res, AF_UNSPEC, type, proto, addrs, + addrs_cnt, port, true); + if (rc < 0) + return -EINVAL; + + addrinfo_has_v4v6addr((const struct addrinfo **)res, addrs_cnt, + &res_has_v4addr, &res_has_v6addr); + if (sfd_family == AF_INET && !res_has_v4addr) { + rc = -EINVAL; + goto ret_free; + } + + uint16_t del_addr_family; + if (sfd_family == AF_INET) + del_addr_family = AF_INET; + else if (sfd_family == AF_INET6 && !res_has_v4addr) + del_addr_family = AF_INET6; + else + del_addr_family = AF_UNSPEC; + rc = addrinfo_to_sockaddr(del_addr_family, (const struct addrinfo **)res, + addrs, addrs_cnt, + (uint8_t *)addrs_buf, sizeof(addrs_buf)); + if (rc < 0) { + rc = -ENODEV; + goto ret_free; + } + + rc = sctp_bindx(sfd, (struct sockaddr *)addrs_buf, addrs_cnt, SCTP_BINDX_REM_ADDR); + if (rc == -1) { + int err = errno; + char strbuf[512]; + multiaddr_snprintf(strbuf, sizeof(strbuf), addrs, addrs_cnt); + LOGP(DLGLOBAL, LOGL_NOTICE, "Unable to unbind socket from addresses: %s:%u: %s\n", + strbuf, port, strerror(err)); + rc = -ENODEV; + goto ret_free; + } + +ret_free: + for (i = 0; i < addrs_cnt; i++) + freeaddrinfo(res[i]); + return rc; +} +#endif /* HAVE_LIBSCTP */ + +static int sockaddr_equal(const struct sockaddr *a, + const struct sockaddr *b, unsigned int len) +{ + struct sockaddr_in *sin_a, *sin_b; + struct sockaddr_in6 *sin6_a, *sin6_b; + + if (a->sa_family != b->sa_family) + return 0; + + switch (a->sa_family) { + case AF_INET: + sin_a = (struct sockaddr_in *)a; + sin_b = (struct sockaddr_in *)b; + if (!memcmp(&sin_a->sin_addr, &sin_b->sin_addr, + sizeof(struct in_addr))) + return 1; + break; + case AF_INET6: + sin6_a = (struct sockaddr_in6 *)a; + sin6_b = (struct sockaddr_in6 *)b; + if (!memcmp(&sin6_a->sin6_addr, &sin6_b->sin6_addr, + sizeof(struct in6_addr))) + return 1; + break; + } + return 0; +} + +/* linux has a default route: +local 127.0.0.0/8 dev lo proto kernel scope host src 127.0.0.1 +*/ +static int sockaddr_is_local_routed(const struct sockaddr *a) +{ +#if __linux__ + if (a->sa_family != AF_INET) + return 0; + + uint32_t address = ((struct sockaddr_in *)a)->sin_addr.s_addr; /* already BE */ + uint32_t eightmask = htonl(0xff000000); /* /8 mask */ + uint32_t local_prefix_127 = htonl(0x7f000000); /* 127.0.0.0 */ + + if ((address & eightmask) == local_prefix_127) + return 1; +#endif + return 0; +} + +/*! Determine if the given address is a local address + * \param[in] addr Socket Address + * \param[in] addrlen Length of socket address in bytes + * \returns 1 if address is local, 0 otherwise. + */ +int osmo_sockaddr_is_local(struct sockaddr *addr, unsigned int addrlen) +{ + struct ifaddrs *ifaddr, *ifa; + + if (sockaddr_is_local_routed(addr)) + return 1; + + if (getifaddrs(&ifaddr) == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, "getifaddrs:" + " %s\n", strerror(errno)); + return -EIO; + } + + for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { + if (!ifa->ifa_addr) + continue; + if (sockaddr_equal(ifa->ifa_addr, addr, addrlen)) { + freeifaddrs(ifaddr); + return 1; + } + } + + freeifaddrs(ifaddr); + return 0; +} + +/*! Determine if the given address is an ANY address ("0.0.0.0", "::"). Port is not checked. + * \param[in] addr Socket Address + * \param[in] addrlen Length of socket address in bytes + * \returns 1 if address is ANY, 0 otherwise. -1 is address family not supported/detected. + */ +int osmo_sockaddr_is_any(const struct osmo_sockaddr *addr) +{ + switch (addr->u.sa.sa_family) { + case AF_INET6: { + struct in6_addr ip6_any = IN6ADDR_ANY_INIT; + return memcmp(&addr->u.sin6.sin6_addr, + &ip6_any, sizeof(ip6_any)) == 0; + } + case AF_INET: + return addr->u.sin.sin_addr.s_addr == INADDR_ANY; + default: + return -1; + } +} + +/*! Convert sockaddr_in to IP address as char string and port as uint16_t. + * \param[out] addr String buffer to write IP address to, or NULL. + * \param[out] addr_len Size of \a addr. + * \param[out] port Pointer to uint16_t to write the port number to, or NULL. + * \param[in] sin Sockaddr to convert. + * \returns the required string buffer size, like osmo_strlcpy(), or 0 if \a addr is NULL. + */ +size_t osmo_sockaddr_in_to_str_and_uint(char *addr, unsigned int addr_len, uint16_t *port, + const struct sockaddr_in *sin) +{ + if (port) + *port = ntohs(sin->sin_port); + + if (addr) + return osmo_strlcpy(addr, inet_ntoa(sin->sin_addr), addr_len); + + return 0; +} + +/*! Convert sockaddr to IP address as char string and port as uint16_t. + * \param[out] addr String buffer to write IP address to, or NULL. + * \param[out] addr_len Size of \a addr. + * \param[out] port Pointer to uint16_t to write the port number to, or NULL. + * \param[in] sa Sockaddr to convert. + * \returns the required string buffer size, like osmo_strlcpy(), or 0 if \a addr is NULL. + */ +unsigned int osmo_sockaddr_to_str_and_uint(char *addr, unsigned int addr_len, uint16_t *port, + const struct sockaddr *sa) +{ + + const struct sockaddr_in6 *sin6; + + switch (sa->sa_family) { + case AF_INET: + return osmo_sockaddr_in_to_str_and_uint(addr, addr_len, port, + (const struct sockaddr_in *)sa); + case AF_INET6: + sin6 = (const struct sockaddr_in6 *)sa; + if (port) + *port = ntohs(sin6->sin6_port); + if (addr && inet_ntop(sa->sa_family, &sin6->sin6_addr, addr, addr_len)) + return strlen(addr); + break; + } + return 0; +} + +/*! inet_ntop() wrapper for a struct sockaddr. + * \param[in] sa source sockaddr to get the address from. + * \param[out] dst string buffer of at least INET6_ADDRSTRLEN size. + * \returns returns a non-null pointer to dst. NULL is returned if there was an + * error, with errno set to indicate the error. + */ +const char *osmo_sockaddr_ntop(const struct sockaddr *sa, char *dst) +{ + const struct osmo_sockaddr *osa = (const struct osmo_sockaddr *)sa; + return inet_ntop(osa->u.sa.sa_family, + osa->u.sa.sa_family == AF_INET6 ? + (const void *)&osa->u.sin6.sin6_addr : + (const void *)&osa->u.sin.sin_addr, + dst, INET6_ADDRSTRLEN); +} + +/*! Get sockaddr port content (in host byte order) + * \param[in] sa source sockaddr to get the port from. + * \returns returns the sockaddr port in host byte order + */ +uint16_t osmo_sockaddr_port(const struct sockaddr *sa) +{ + const struct osmo_sockaddr *osa = (const struct osmo_sockaddr *)sa; + switch (osa->u.sa.sa_family) { + case AF_INET6: + return ntohs(osa->u.sin6.sin6_port); + case AF_INET: + return ntohs(osa->u.sin.sin_port); + } + return 0; +} + +/*! Set sockaddr port content (to network byte order). + * \param[out] sa sockaddr to set the port of. + * \param[in] port port nr to set. + */ +void osmo_sockaddr_set_port(struct sockaddr *sa, uint16_t port) +{ + struct osmo_sockaddr *osa = (struct osmo_sockaddr *)sa; + switch (osa->u.sa.sa_family) { + case AF_INET6: + osa->u.sin6.sin6_port = htons(port); + return; + case AF_INET: + osa->u.sin.sin_port = htons(port); + return; + } +} + +static unsigned int in6_addr_netmask_to_prefixlen(const struct in6_addr *netmask) +{ + #if defined(__linux__) + #define ADDRFIELD(i) s6_addr32[i] + #else + #define ADDRFIELD(i) __u6_addr.__u6_addr32[i] + #endif + + unsigned int i, j, prefix = 0; + + for (j = 0; j < 4; j++) { + uint32_t bits = netmask->ADDRFIELD(j); + uint8_t *b = (uint8_t *)&bits; + for (i = 0; i < 4; i++) { + while (b[i] & 0x80) { + prefix++; + b[i] = b[i] << 1; + } + } + } + + #undef ADDRFIELD + + return prefix; +} + +static unsigned int in_addr_netmask_to_prefixlen(const struct in_addr *netmask) +{ + uint32_t bits = netmask->s_addr; + uint8_t *b = (uint8_t *)&bits; + unsigned int i, prefix = 0; + + for (i = 0; i < 4; i++) { + while (b[i] & 0x80) { + prefix++; + b[i] = b[i] << 1; + } + } + return prefix; +} + +/*! Convert netmask to prefix length representation + * \param[in] netmask sockaddr containing a netmask (consecutive list of 1-bit followed by consecutive list of 0-bit) + * \returns prefix length representation of the netmask (count of 1-bit from the start of the netmask), negative on error. + */ +int osmo_sockaddr_netmask_to_prefixlen(const struct osmo_sockaddr *netmask) +{ + switch (netmask->u.sa.sa_family) { + case AF_INET6: + return in6_addr_netmask_to_prefixlen(&netmask->u.sin6.sin6_addr); + case AF_INET: + return in_addr_netmask_to_prefixlen(&netmask->u.sin.sin_addr); + default: + return -ENOTSUP; + } +} + +/*! Convert an IP address string (and port number) into a 'struct osmo_sockaddr'. + * \param[out] osa_out caller-allocated osmo_sockaddr storage + * \param[in] ipstr IP[v4,v6] address in string format + * \param[in] port port number (host byte order) + * \returns 0 on success; negative on error. */ +int osmo_sockaddr_from_str_and_uint(struct osmo_sockaddr *osa_out, const char *ipstr, uint16_t port) +{ + struct addrinfo *ai = addrinfo_helper(AF_UNSPEC, 0, 0, ipstr, port, true); + + if (!ai) + return -EIO; + + if (ai->ai_addrlen > sizeof(*osa_out)) + return -ENOSPC; + + memcpy(&osa_out->u.sa, ai->ai_addr, ai->ai_addrlen); + freeaddrinfo(ai); + + return 0; +} + +/*! Initialize a unix domain socket (including bind/connect) + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] socket_path path to identify the socket + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function creates a new unix domain socket, \a + * type and \a proto and optionally binds or connects it, depending on + * the value of \a flags parameter. + */ +#if defined(__clang__) && defined(SUN_LEN) +__attribute__((no_sanitize("undefined"))) +#endif +int osmo_sock_unix_init(uint16_t type, uint8_t proto, + const char *socket_path, unsigned int flags) +{ + struct sockaddr_un local; + int sfd, rc; + unsigned int namelen; + + if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == + (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) + return -EINVAL; + + local.sun_family = AF_UNIX; + /* When an AF_UNIX socket is bound, sun_path should be NUL-terminated. See unix(7) man page. */ + if (osmo_strlcpy(local.sun_path, socket_path, sizeof(local.sun_path)) >= sizeof(local.sun_path)) { + LOGP(DLGLOBAL, LOGL_ERROR, "Socket path exceeds maximum length of %zd bytes: %s\n", + sizeof(local.sun_path), socket_path); + return -ENOSPC; + } + +#if defined(BSD44SOCKETS) || defined(__UNIXWARE__) + local.sun_len = strlen(local.sun_path); +#endif +#if defined(BSD44SOCKETS) || defined(SUN_LEN) + namelen = SUN_LEN(&local); +#else + namelen = strlen(local.sun_path) + + offsetof(struct sockaddr_un, sun_path); +#endif + + sfd = socket(AF_UNIX, type, proto); + if (sfd < 0) + return -errno; + + if (flags & OSMO_SOCK_F_CONNECT) { + rc = connect(sfd, (struct sockaddr *)&local, namelen); + if (rc < 0) + goto err; + } else { + unlink(local.sun_path); + rc = bind(sfd, (struct sockaddr *)&local, namelen); + if (rc < 0) + goto err; + } + + rc = socket_helper_tail(sfd, flags); + if (rc < 0) + return rc; + + rc = osmo_sock_init_tail(sfd, type, flags); + if (rc < 0) { + close(sfd); + sfd = rc; + } + + return sfd; +err: + close(sfd); + return -errno; +} + +/*! Initialize a unix domain socket and fill \ref osmo_fd + * \param[out] ofd file descriptor (will be filled in) + * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM + * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP + * \param[in] socket_path path to identify the socket + * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT + * \returns socket fd on success; negative on error + * + * This function creates (and optionally binds/connects) a socket + * using osmo_sock_unix_init, but also fills the ofd structure. + */ +int osmo_sock_unix_init_ofd(struct osmo_fd *ofd, uint16_t type, uint8_t proto, + const char *socket_path, unsigned int flags) +{ + return osmo_fd_init_ofd(ofd, osmo_sock_unix_init(type, proto, socket_path, flags), flags); +} + +/*! Get the IP and/or port number on socket in separate string buffers. + * \param[in] fd file descriptor of socket + * \param[out] ip IP address (will be filled in when not NULL) + * \param[in] ip_len length of the ip buffer + * \param[out] port number (will be filled in when not NULL) + * \param[in] port_len length of the port buffer + * \param[in] local (true) or remote (false) name will get looked at + * \returns 0 on success; negative otherwise + */ +int osmo_sock_get_ip_and_port(int fd, char *ip, size_t ip_len, char *port, size_t port_len, bool local) +{ + struct sockaddr_storage sa; + socklen_t len = sizeof(sa); + char ipbuf[INET6_ADDRSTRLEN], portbuf[6]; + int rc; + + rc = local ? getsockname(fd, (struct sockaddr*)&sa, &len) : getpeername(fd, (struct sockaddr*)&sa, &len); + if (rc < 0) + return rc; + + rc = getnameinfo((const struct sockaddr*)&sa, len, ipbuf, sizeof(ipbuf), + portbuf, sizeof(portbuf), + NI_NUMERICHOST | NI_NUMERICSERV); + if (rc < 0) + return rc; + + if (ip) + strncpy(ip, ipbuf, ip_len); + if (port) + strncpy(port, portbuf, port_len); + return 0; +} + +#ifdef HAVE_LIBSCTP +/*! Get multiple IP addresses and/or port number on socket in separate string buffers + * \param[in] fd file descriptor of socket. + * \param[out] ip_proto IPPROTO of the socket, eg: IPPROTO_SCTP. + * \param[out] ip Pointer to memory holding consecutive buffers of size ip_len. + * \param[out] ip_cnt length ip array pointer. on return it contains the number of addresses found. + * \param[in] ip_len length of each of the string buffer in the the ip array. + * \param[out] port number (will be filled in when not NULL). + * \param[in] port_len length of the port buffer. + * \param[in] local (true) or remote (false) name will get looked at. + * \returns 0 on success; negative otherwise. + * + * Upon return, ip_cnt can be set to a higher value than the one set by the + * caller. This can be used by the caller to find out the required array length + * and then obtaining by calling the function twice. Only up to ip_cnt addresses + * are filed in, as per the value provided by the caller. + * + * Usage example retrieving all (up to OSMO_SOCK_MAX_ADDRS, 32) bound IP addresses and bound port: + * char hostbuf[OSMO_SOCK_MAX_ADDRS][INET6_ADDRSTRLEN]; + * size_t num_hostbuf = ARRAY_SIZE(hostbuf); + * char portbuf[6]; + * rc = osmo_sock_multiaddr_get_ip_and_port(fd, IPPROTO_SCTP, &hostbuf[0][0], &num_hostbuf, + * sizeof(hostbuf[0]), portbuf, sizeof(portbuf), true); + * if (rc < 0) + * goto error; + * if (num_hostbuf > ARRAY_SIZE(hostbuf)) + * goto not_enough_buffers; + */ +int osmo_sock_multiaddr_get_ip_and_port(int fd, int ip_proto, char *ip, size_t *ip_cnt, size_t ip_len, + char *port, size_t port_len, bool local) +{ + struct sockaddr *addrs = NULL; + unsigned int n_addrs, i; + void *addr_buf; + int rc; + + switch (ip_proto) { + case IPPROTO_SCTP: + break; /* continue below */ + default: + if (*ip_cnt == 0) { + *ip_cnt = 1; + return 0; + } + *ip_cnt = 1; + return osmo_sock_get_ip_and_port(fd, ip, ip_len, port, port_len, local); + } + + rc = local ? sctp_getladdrs(fd, 0, &addrs) : sctp_getpaddrs(fd, 0, &addrs); + if (rc < 0) + return rc; + if (rc == 0) + return -ENOTCONN; + + n_addrs = rc; + addr_buf = (void *)addrs; + for (i = 0; i < n_addrs; i++) { + struct sockaddr *sa_addr = (struct sockaddr *)addr_buf; + size_t addrlen; + + if (i >= *ip_cnt) + break; + + switch (sa_addr->sa_family) { + case AF_INET: + addrlen = sizeof(struct sockaddr_in); + break; + case AF_INET6: + addrlen = sizeof(struct sockaddr_in6); + break; + default: + rc = -EINVAL; + goto free_addrs_ret; + } + + rc = getnameinfo(sa_addr, addrlen, &ip[i * ip_len], ip_len, + port, port_len, + NI_NUMERICHOST | NI_NUMERICSERV); + if (rc < 0) + goto free_addrs_ret; + addr_buf += addrlen; + } + + *ip_cnt = n_addrs; + rc = 0; +free_addrs_ret: + local ? sctp_freeladdrs(addrs) : sctp_freepaddrs(addrs); + return rc; +} +#endif + +/*! Get local IP address on socket + * \param[in] fd file descriptor of socket + * \param[out] ip IP address (will be filled in) + * \param[in] len length of the output buffer + * \returns 0 on success; negative otherwise + */ +int osmo_sock_get_local_ip(int fd, char *ip, size_t len) +{ + return osmo_sock_get_ip_and_port(fd, ip, len, NULL, 0, true); +} + +/*! Get local port on socket + * \param[in] fd file descriptor of socket + * \param[out] port number (will be filled in) + * \param[in] len length of the output buffer + * \returns 0 on success; negative otherwise + */ +int osmo_sock_get_local_ip_port(int fd, char *port, size_t len) +{ + return osmo_sock_get_ip_and_port(fd, NULL, 0, port, len, true); +} + +/*! Get remote IP address on socket + * \param[in] fd file descriptor of socket + * \param[out] ip IP address (will be filled in) + * \param[in] len length of the output buffer + * \returns 0 on success; negative otherwise + */ +int osmo_sock_get_remote_ip(int fd, char *ip, size_t len) +{ + return osmo_sock_get_ip_and_port(fd, ip, len, NULL, 0, false); +} + +/*! Get remote port on socket + * \param[in] fd file descriptor of socket + * \param[out] port number (will be filled in) + * \param[in] len length of the output buffer + * \returns 0 on success; negative otherwise + */ +int osmo_sock_get_remote_ip_port(int fd, char *port, size_t len) +{ + return osmo_sock_get_ip_and_port(fd, NULL, 0, port, len, false); +} + +/*! Get address/port information on socket in dyn-alloc string like "(r=1.2.3.4:5<->l=6.7.8.9:10)". + * Usually, it is better to use osmo_sock_get_name2() for a static string buffer or osmo_sock_get_name_buf() for a + * caller provided string buffer, to avoid the dynamic talloc allocation. + * \param[in] ctx talloc context from which to allocate string buffer + * \param[in] fd file descriptor of socket + * \returns string identifying the connection of this socket, talloc'd from ctx. + */ +char *osmo_sock_get_name(const void *ctx, int fd) +{ + char str[OSMO_SOCK_NAME_MAXLEN]; + int rc; + rc = osmo_sock_get_name_buf(str, sizeof(str), fd); + if (rc <= 0) + return NULL; + return talloc_asprintf(ctx, "(%s)", str); +} + +#ifdef HAVE_LIBSCTP +/*! Format multiple IP addresses and/or port number into a combined string buffer + * \param[out] str Destination string buffer. + * \param[in] str_len sizeof(str), usually OSMO_SOCK_MULTIADDR_PEER_STR_MAXLEN. + * \param[out] ip Pointer to memory holding ip_cnt consecutive buffers of size ip_len. + * \param[out] ip_cnt length ip array pointer. on return it contains the number of addresses found. + * \param[in] ip_len length of each of the string buffer in the the ip array. + * \param[out] port number (will be printed in when not NULL). + * \return String length as returned by snprintf(), or negative on error. + * + * This API expects an ip array as the one filled in by + * osmo_sock_multiaddr_get_ip_and_port(), and hence it's a good companion for + * that API. + */ +int osmo_multiaddr_ip_and_port_snprintf(char *str, size_t str_len, + const char *ip, size_t ip_cnt, size_t ip_len, + const char *portbuf) +{ + struct osmo_strbuf sb = { .buf = str, .len = str_len }; + bool is_v6 = false; + unsigned int i; + + if (ip_cnt == 0) { + OSMO_STRBUF_PRINTF(sb, "NULL:%s", portbuf); + return sb.chars_needed; + } + if (ip_cnt > 1) + OSMO_STRBUF_PRINTF(sb, "("); + else if ((is_v6 = !!strchr(&ip[0], ':'))) /* IPv6, add [] to separate from port. */ + OSMO_STRBUF_PRINTF(sb, "["); + + for (i = 0; i < ip_cnt - 1; i++) + OSMO_STRBUF_PRINTF(sb, "%s|", &ip[i * ip_len]); + OSMO_STRBUF_PRINTF(sb, "%s", &ip[i * ip_len]); + + if (ip_cnt > 1) + OSMO_STRBUF_PRINTF(sb, ")"); + else if (is_v6) + OSMO_STRBUF_PRINTF(sb, "]"); + if (portbuf) + OSMO_STRBUF_PRINTF(sb, ":%s", portbuf); + + return sb.chars_needed; +} + +/*! Get address/port information on socket in provided string buffer, like "r=1.2.3.4:5<->l=6.7.8.9:10". + * This does not include braces like osmo_sock_get_name(). + * \param[out] str Destination string buffer. + * \param[in] str_len sizeof(str), usually OSMO_SOCK_MULTIADDR_NAME_MAXLEN. + * \param[in] fd File descriptor of socket. + * \param[in] fd IPPROTO of the socket, eg: IPPROTO_SCTP. + * \return String length as returned by snprintf(), or negative on error. + */ +int osmo_sock_multiaddr_get_name_buf(char *str, size_t str_len, int fd, int sk_proto) +{ + char hostbuf[OSMO_SOCK_MAX_ADDRS][INET6_ADDRSTRLEN]; + size_t num_hostbuf = ARRAY_SIZE(hostbuf); + char portbuf[6]; + struct osmo_strbuf sb = { .buf = str, .len = str_len }; + + if (fd < 0) { + osmo_strlcpy(str, "<error-bad-fd>", str_len); + return sb.chars_needed; + } + + switch (sk_proto) { + case IPPROTO_SCTP: + break; /* continue below */ + default: + return osmo_sock_get_name_buf(str, str_len, fd); + } + + /* get remote */ + OSMO_STRBUF_PRINTF(sb, "r="); + if (osmo_sock_multiaddr_get_ip_and_port(fd, sk_proto, &hostbuf[0][0], &num_hostbuf, + sizeof(hostbuf[0]), portbuf, sizeof(portbuf), false) != 0) { + OSMO_STRBUF_PRINTF(sb, "NULL"); + } else { + const bool need_more_bufs = num_hostbuf > ARRAY_SIZE(hostbuf); + if (need_more_bufs) + num_hostbuf = ARRAY_SIZE(hostbuf); + OSMO_STRBUF_APPEND(sb, osmo_multiaddr_ip_and_port_snprintf, + &hostbuf[0][0], num_hostbuf, sizeof(hostbuf[0]), portbuf); + if (need_more_bufs) + OSMO_STRBUF_PRINTF(sb, "<need-more-bufs!>"); + } + + OSMO_STRBUF_PRINTF(sb, "<->l="); + + /* get local */ + num_hostbuf = ARRAY_SIZE(hostbuf); + if (osmo_sock_multiaddr_get_ip_and_port(fd, sk_proto, &hostbuf[0][0], &num_hostbuf, + sizeof(hostbuf[0]), portbuf, sizeof(portbuf), true) != 0) { + OSMO_STRBUF_PRINTF(sb, "NULL"); + } else { + const bool need_more_bufs = num_hostbuf > ARRAY_SIZE(hostbuf); + if (need_more_bufs) + num_hostbuf = ARRAY_SIZE(hostbuf); + OSMO_STRBUF_APPEND(sb, osmo_multiaddr_ip_and_port_snprintf, + &hostbuf[0][0], num_hostbuf, sizeof(hostbuf[0]), portbuf); + if (need_more_bufs) + OSMO_STRBUF_PRINTF(sb, "<need-more-bufs!>"); + } + + return sb.chars_needed; +} +#endif + +/*! Get address/port information on socket in provided string buffer, like "r=1.2.3.4:5<->l=6.7.8.9:10". + * This does not include braces like osmo_sock_get_name(). + * \param[out] str Destination string buffer. + * \param[in] str_len sizeof(str). + * \param[in] fd File descriptor of socket. + * \return String length as returned by snprintf(), or negative on error. + */ +int osmo_sock_get_name_buf(char *str, size_t str_len, int fd) +{ + struct osmo_strbuf sb = { .buf = str, .len = str_len }; + struct osmo_sockaddr osa; + struct sockaddr_un *sun; + socklen_t len; + int rc; + + if (fd < 0) { + osmo_strlcpy(str, "<error-bad-fd>", str_len); + return -EBADF; + } + + + len = sizeof(osa.u.sas); + rc = getsockname(fd, &osa.u.sa, &len); + if (rc < 0) { + osmo_strlcpy(str, "<error-in-getsockname>", str_len); + return rc; + } + + switch (osa.u.sa.sa_family) { + case AF_INET: + case AF_INET6: + { + char hostbuf_l[INET6_ADDRSTRLEN], hostbuf_r[INET6_ADDRSTRLEN]; + char portbuf_l[6], portbuf_r[6]; + + len = sizeof(osa.u.sas); + rc = getnameinfo(&osa.u.sa, len, hostbuf_l, sizeof(hostbuf_l), + portbuf_l, sizeof(portbuf_l), + NI_NUMERICHOST | NI_NUMERICSERV); + if (rc < 0) { + osmo_strlcpy(str, "<error-in-getnameinfo>", str_len); + return rc; + } + /* Now attempt to get remote: */ + len = sizeof(osa.u.sas); + rc = getpeername(fd, &osa.u.sa, &len); + if (rc < 0) { + OSMO_STRBUF_PRINTF(sb, "r=NULL<->l=%s:%s", hostbuf_l, portbuf_l); + return sb.chars_needed; + } + len = sizeof(osa.u.sas); + rc = getnameinfo(&osa.u.sa, len, hostbuf_r, sizeof(hostbuf_r), + portbuf_r, sizeof(portbuf_r), + NI_NUMERICHOST | NI_NUMERICSERV); + if (rc < 0) { + OSMO_STRBUF_PRINTF(sb, "r=NULL<->l=%s:%s", hostbuf_l, portbuf_l); + return sb.chars_needed; + } + OSMO_STRBUF_PRINTF(sb, "r=%s:%s<->l=%s:%s", hostbuf_r, portbuf_r, hostbuf_l, portbuf_l); + return sb.chars_needed; + } + case AF_UNIX: + { + unsigned long long remote_pid; + bool have_remote_pid; +#if defined(SO_PEERCRED) + struct ucred ucred; + len = sizeof(struct ucred); + if (getsockopt(fd, SOL_SOCKET, SO_PEERCRED, &ucred, &len) == -1) { + have_remote_pid = false; + } else { + have_remote_pid = true; + remote_pid = (unsigned long long)ucred.pid; + } +#else + #pragma message "SO_PEERCRED not available" + have_remote_pid = false; +#endif + /* Make sure sun_path is NULL terminated: */ + sun = (struct sockaddr_un *)&osa.u.sa; + sun->sun_path[sizeof(sun->sun_path) - 1] = '\0'; + if (have_remote_pid) + OSMO_STRBUF_PRINTF(sb, "r=%llu<->", remote_pid); + else + OSMO_STRBUF_PRINTF(sb, "r=NULL<->"); + OSMO_STRBUF_PRINTF(sb, "l=%s:%d", sun->sun_path, fd); + return sb.chars_needed; + } + default: + osmo_strlcpy(str, "<socket-family-no-supported>", str_len); + return -ENOTSUP; + } +} + +/*! Get address/port information on socket in static string, like "r=1.2.3.4:5<->l=6.7.8.9:10". + * This does not include braces like osmo_sock_get_name(). + * \param[in] fd File descriptor of socket. + * \return Static string buffer containing the result. + */ +const char *osmo_sock_get_name2(int fd) +{ + static __thread char str[OSMO_SOCK_NAME_MAXLEN]; + osmo_sock_get_name_buf(str, sizeof(str), fd); + return str; +} + +/*! Get address/port information on socket in static string, like "r=1.2.3.4:5<->l=6.7.8.9:10". + * This does not include braces like osmo_sock_get_name(). + * \param[in] fd File descriptor of socket. + * \return Static string buffer containing the result. + */ +char *osmo_sock_get_name2_c(const void *ctx, int fd) +{ + char *str = talloc_size(ctx, OSMO_SOCK_NAME_MAXLEN); + if (!str) + return NULL; + osmo_sock_get_name_buf(str, OSMO_SOCK_NAME_MAXLEN, fd); + return str; +} + +static int sock_get_domain(int fd) +{ + int domain; +#ifdef SO_DOMAIN + socklen_t dom_len = sizeof(domain); + int rc; + + rc = getsockopt(fd, SOL_SOCKET, SO_DOMAIN, &domain, &dom_len); + if (rc < 0) + return rc; +#else + /* This of course sucks, but what shall we do on OSs like + * FreeBSD that don't seem to expose a method by which one can + * learn the address family of a socket? */ + domain = AF_INET; +#endif + return domain; +} + + +/*! Activate or de-activate local loop-back of transmitted multicast packets + * \param[in] fd file descriptor of related socket + * \param[in] enable Enable (true) or disable (false) loop-back + * \returns 0 on success; negative otherwise */ +int osmo_sock_mcast_loop_set(int fd, bool enable) +{ + int domain, loop = 0; + + if (enable) + loop = 1; + + domain = sock_get_domain(fd); + if (domain < 0) + return domain; + + switch (domain) { + case AF_INET: + return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &loop, sizeof(loop)); + case AF_INET6: + return setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &loop, sizeof(loop)); + default: + return -EINVAL; + } +} + +/*! Set the TTL of outbound multicast packets + * \param[in] fd file descriptor of related socket + * \param[in] ttl TTL of to-be-sent multicast packets + * \returns 0 on success; negative otherwise */ +int osmo_sock_mcast_ttl_set(int fd, uint8_t ttl) +{ + int domain, ttli = ttl; + + domain = sock_get_domain(fd); + if (domain < 0) + return domain; + + switch (domain) { + case AF_INET: + return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttli, sizeof(ttli)); + case AF_INET6: + return setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &ttli, sizeof(ttli)); + default: + return -EINVAL; + } +} + +/*! Set the network device to which we should bind the multicast socket + * \param[in] fd file descriptor of related socket + * \param[in] ifname name of network interface to user for multicast + * \returns 0 on success; negative otherwise */ +int osmo_sock_mcast_iface_set(int fd, const char *ifname) +{ + unsigned int ifindex; + struct ip_mreqn mr; + + /* first, resolve interface name to ifindex */ + ifindex = if_nametoindex(ifname); + if (ifindex == 0) + return -errno; + + /* next, configure kernel to use that ifindex for this sockets multicast traffic */ + memset(&mr, 0, sizeof(mr)); + mr.imr_ifindex = ifindex; + return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &mr, sizeof(mr)); +} + + +/*! Enable/disable receiving all multicast packets, even for non-subscribed groups + * \param[in] fd file descriptor of related socket + * \param[in] enable Enable or Disable receiving of all packets + * \returns 0 on success; negative otherwise */ +int osmo_sock_mcast_all_set(int fd, bool enable) +{ + int domain, all = 0; + + if (enable) + all = 1; + + domain = sock_get_domain(fd); + if (domain < 0) + return domain; + + switch (domain) { + case AF_INET: +#ifdef IP_MULTICAST_ALL + return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_ALL, &all, sizeof(all)); +#endif + case AF_INET6: + /* there seems no equivalent ?!? */ + default: + return -EINVAL; + } +} + +/* FreeBSD calls the socket option differently */ +#if !defined(IPV6_ADD_MEMBERSHIP) && defined(IPV6_JOIN_GROUP) +#define IPV6_ADD_MEMBERSHIP IPV6_JOIN_GROUP +#endif + +/*! Subscribe to the given IP multicast group + * \param[in] fd file descriptor of related scoket + * \param[in] grp_addr ASCII representation of the multicast group address + * \returns 0 on success; negative otherwise */ +int osmo_sock_mcast_subscribe(int fd, const char *grp_addr) +{ + int rc, domain; + struct ip_mreq mreq; + struct ipv6_mreq mreq6; + struct in6_addr i6a; + + domain = sock_get_domain(fd); + if (domain < 0) + return domain; + + switch (domain) { + case AF_INET: + memset(&mreq, 0, sizeof(mreq)); + mreq.imr_multiaddr.s_addr = inet_addr(grp_addr); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + return setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); +#ifdef IPV6_ADD_MEMBERSHIP + case AF_INET6: + memset(&mreq6, 0, sizeof(mreq6)); + rc = inet_pton(AF_INET6, grp_addr, (void *)&i6a); + if (rc < 0) + return -EINVAL; + mreq6.ipv6mr_multiaddr = i6a; + return setsockopt(fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq6, sizeof(mreq6)); +#endif + default: + return -EINVAL; + } +} + +/*! Determine the matching local IP-address for a given remote IP-Address. + * \param[out] local_ip caller provided memory for resulting local IP-address + * \param[in] remote_ip remote IP-address + * \returns 0 on success; negative otherwise + * + * The function accepts IPv4 and IPv6 address strings. The caller must provide + * at least INET6_ADDRSTRLEN bytes for local_ip if an IPv6 is expected as + * as result. For IPv4 addresses the required amount is INET_ADDRSTRLEN. */ +int osmo_sock_local_ip(char *local_ip, const char *remote_ip) +{ + int sfd; + int rc; + struct addrinfo addrinfo_hint; + struct addrinfo *addrinfo = NULL; + struct sockaddr_storage local_addr; + struct sockaddr_in *sin; + struct sockaddr_in6 *sin6; + socklen_t local_addr_len; + uint16_t family; + + /* Find out the address family (AF_INET or AF_INET6?) */ + memset(&addrinfo_hint, '\0', sizeof(addrinfo_hint)); + addrinfo_hint.ai_family = AF_UNSPEC; + addrinfo_hint.ai_flags = AI_NUMERICHOST; + rc = getaddrinfo(remote_ip, NULL, &addrinfo_hint, &addrinfo); + if (rc) + return -EINVAL; + family = addrinfo->ai_family; + freeaddrinfo(addrinfo); + + /* Connect a dummy socket to trick the kernel into determining the + * ip-address of the interface that would be used if we would send + * out an actual packet */ + sfd = osmo_sock_init2(family, SOCK_DGRAM, IPPROTO_UDP, NULL, 0, remote_ip, 0, OSMO_SOCK_F_CONNECT); + if (sfd < 0) + return -EINVAL; + + /* Request the IP address of the interface that the kernel has + * actually choosen. */ + memset(&local_addr, 0, sizeof(local_addr)); + local_addr_len = sizeof(local_addr); + rc = getsockname(sfd, (struct sockaddr *)&local_addr, &local_addr_len); + close(sfd); + if (rc < 0) + return -EINVAL; + + switch (local_addr.ss_family) { + case AF_INET: + sin = (struct sockaddr_in*)&local_addr; + if (!inet_ntop(AF_INET, &sin->sin_addr, local_ip, INET_ADDRSTRLEN)) + return -EINVAL; + break; + case AF_INET6: + sin6 = (struct sockaddr_in6*)&local_addr; + if (!inet_ntop(AF_INET6, &sin6->sin6_addr, local_ip, INET6_ADDRSTRLEN)) + return -EINVAL; + break; + default: + return -EINVAL; + } + + return 0; +} + +/*! Determine the matching local address for a given remote address. + * \param[out] local_ip caller provided memory for resulting local address + * \param[in] remote_ip remote address + * \returns 0 on success; negative otherwise + */ +int osmo_sockaddr_local_ip(struct osmo_sockaddr *local_ip, const struct osmo_sockaddr *remote_ip) +{ + int sfd; + int rc; + socklen_t local_ip_len; + + sfd = osmo_sock_init_osa(SOCK_DGRAM, IPPROTO_UDP, NULL, remote_ip, OSMO_SOCK_F_CONNECT); + if (sfd < 0) + return -EINVAL; + + memset(local_ip, 0, sizeof(*local_ip)); + local_ip_len = sizeof(*local_ip); + rc = getsockname(sfd, (struct sockaddr *)local_ip, &local_ip_len); + close(sfd); + + return rc; +} + +/*! Copy the addr part, the IP address octets in network byte order, to a buffer. + * Useful for encoding network protocols. + * \param[out] dst Write octets to this buffer. + * \param[in] dst_maxlen Space available in buffer. + * \param[in] os Sockaddr to copy IP of. + * \return nr of octets written on success, negative on error. + */ +int osmo_sockaddr_to_octets(uint8_t *dst, size_t dst_maxlen, const struct osmo_sockaddr *os) +{ + const void *addr; + size_t len; + switch (os->u.sa.sa_family) { + case AF_INET: + addr = &os->u.sin.sin_addr; + len = sizeof(os->u.sin.sin_addr); + break; + case AF_INET6: + addr = &os->u.sin6.sin6_addr; + len = sizeof(os->u.sin6.sin6_addr); + break; + default: + return -ENOTSUP; + } + if (dst_maxlen < len) + return -ENOSPC; + memcpy(dst, addr, len); + return len; +} + +/*! Copy the addr part, the IP address octets in network byte order, from a buffer. + * Useful for decoding network protocols. + * \param[out] os Write IP address to this sockaddr. + * \param[in] src Source buffer to read IP address octets from. + * \param[in] src_len Number of octets to copy. + * \return number of octets read on success, negative on error. + */ +int osmo_sockaddr_from_octets(struct osmo_sockaddr *os, const void *src, size_t src_len) +{ + void *addr; + size_t len; + *os = (struct osmo_sockaddr){0}; + switch (src_len) { + case sizeof(os->u.sin.sin_addr): + os->u.sa.sa_family = AF_INET; + addr = &os->u.sin.sin_addr; + len = sizeof(os->u.sin.sin_addr); + break; + case sizeof(os->u.sin6.sin6_addr): + os->u.sin6.sin6_family = AF_INET6; + addr = &os->u.sin6.sin6_addr; + len = sizeof(os->u.sin6.sin6_addr); + break; + default: + return -ENOTSUP; + } + memcpy(addr, src, len); + return len; +} + +/*! Compare two osmo_sockaddr. + * \param[in] a + * \param[in] b + * \return 0 if a and b are equal. Otherwise it follows memcmp() + */ +int osmo_sockaddr_cmp(const struct osmo_sockaddr *a, + const struct osmo_sockaddr *b) +{ + if (a == b) + return 0; + if (!a) + return 1; + if (!b) + return -1; + + if (a->u.sa.sa_family != b->u.sa.sa_family) { + return OSMO_CMP(a->u.sa.sa_family, b->u.sa.sa_family); + } + + switch (a->u.sa.sa_family) { + case AF_INET: + return memcmp(&a->u.sin, &b->u.sin, sizeof(struct sockaddr_in)); + case AF_INET6: + return memcmp(&a->u.sin6, &b->u.sin6, sizeof(struct sockaddr_in6)); + default: + /* fallback to memcmp for remaining AF over the full osmo_sockaddr length */ + return memcmp(a, b, sizeof(struct osmo_sockaddr)); + } +} + +/*! string-format a given osmo_sockaddr address + * \param[in] sockaddr the osmo_sockaddr to print + * \return pointer to the string on success; NULL on error + */ +const char *osmo_sockaddr_to_str(const struct osmo_sockaddr *sockaddr) +{ + /* INET6_ADDRSTRLEN contains already a null termination, + * adding '[' ']' ':' '16 bit port' */ + static __thread char buf[INET6_ADDRSTRLEN + 8]; + return osmo_sockaddr_to_str_buf(buf, sizeof(buf), sockaddr); +} + +/*! string-format a given osmo_sockaddr address into a user-supplied buffer. + * Same as osmo_sockaddr_to_str_buf() but returns a would-be length in snprintf() style. + * \param[in] buf user-supplied output buffer + * \param[in] buf_len size of the user-supplied output buffer in bytes + * \param[in] sockaddr the osmo_sockaddr to print + * \return number of characters that would be written if the buffer is large enough, like snprintf(). + */ +int osmo_sockaddr_to_str_buf2(char *buf, size_t buf_len, const struct osmo_sockaddr *sockaddr) +{ + struct osmo_strbuf sb = { .buf = buf, .len = buf_len }; + uint16_t port = 0; + + if (!sockaddr) { + OSMO_STRBUF_PRINTF(sb, "NULL"); + return sb.chars_needed; + } + + switch (sockaddr->u.sa.sa_family) { + case AF_INET: + OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_and_uint, &port, &sockaddr->u.sa); + if (port) + OSMO_STRBUF_PRINTF(sb, ":%u", port); + break; + case AF_INET6: + OSMO_STRBUF_PRINTF(sb, "["); + OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_and_uint, &port, &sockaddr->u.sa); + OSMO_STRBUF_PRINTF(sb, "]"); + if (port) + OSMO_STRBUF_PRINTF(sb, ":%u", port); + break; + default: + OSMO_STRBUF_PRINTF(sb, "unsupported family %d", sockaddr->u.sa.sa_family); + break; + } + + return sb.chars_needed; +} + +/*! string-format a given osmo_sockaddr address into a talloc allocated buffer. + * Like osmo_sockaddr_to_str_buf2() but returns a talloc allocated string. + * \param[in] ctx talloc context to allocate from, e.g. OTC_SELECT. + * \param[in] sockaddr the osmo_sockaddr to print. + * \return human readable string. + */ +char *osmo_sockaddr_to_str_c(void *ctx, const struct osmo_sockaddr *sockaddr) +{ + OSMO_NAME_C_IMPL(ctx, 64, "ERROR", osmo_sockaddr_to_str_buf2, sockaddr) +} + +/*! string-format a given osmo_sockaddr address into a user-supplied buffer. + * Like osmo_sockaddr_to_str_buf2() but returns buf, or NULL if too short. + * \param[in] buf user-supplied output buffer + * \param[in] buf_len size of the user-supplied output buffer in bytes + * \param[in] sockaddr the osmo_sockaddr to print + * \return pointer to the string on success; NULL on error + */ +char *osmo_sockaddr_to_str_buf(char *buf, size_t buf_len, + const struct osmo_sockaddr *sockaddr) +{ + int chars_needed = osmo_sockaddr_to_str_buf2(buf, buf_len, sockaddr); + if (chars_needed >= buf_len) + return NULL; + return buf; +} + +/*! Set the DSCP (differentiated services code point) of a socket. + * \param[in] dscp DSCP value in range 0..63 + * \returns 0 on success; negative on error. */ +int osmo_sock_set_dscp(int fd, uint8_t dscp) +{ + struct sockaddr_storage local_addr; + socklen_t local_addr_len = sizeof(local_addr); + uint8_t tos; + socklen_t tos_len = sizeof(tos); + int tclass; + socklen_t tclass_len = sizeof(tclass); + int rc; + + /* DSCP is a 6-bit value stored in the upper 6 bits of the 8-bit TOS */ + if (dscp > 63) + return -EINVAL; + + rc = getsockname(fd, (struct sockaddr *)&local_addr, &local_addr_len); + if (rc < 0) + return rc; + + switch (local_addr.ss_family) { + case AF_INET: + /* read the original value */ + rc = getsockopt(fd, IPPROTO_IP, IP_TOS, &tos, &tos_len); + if (rc < 0) + return rc; + /* mask-in the DSCP into the upper 6 bits */ + tos &= 0x03; + tos |= dscp << 2; + /* and write it back to the kernel */ + rc = setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)); + break; + case AF_INET6: + /* read the original value */ + rc = getsockopt(fd, IPPROTO_IPV6, IPV6_TCLASS, &tclass, &tclass_len); + if (rc < 0) + return rc; + /* mask-in the DSCP into the upper 6 bits */ + tclass &= 0x03; + tclass |= dscp << 2; + /* and write it back to the kernel */ + rc = setsockopt(fd, IPPROTO_IPV6, IPV6_TCLASS, &tclass, sizeof(tclass)); + break; + case AF_UNSPEC: + default: + LOGP(DLGLOBAL, LOGL_ERROR, "No DSCP support for socket family %u\n", + local_addr.ss_family); + rc = -1; + break; + } + + return rc; +} + +/*! Set the priority value of a socket. + * \param[in] prio priority value. Values outside 0..6 require CAP_NET_ADMIN. + * \returns 0 on success; negative on error. */ +int osmo_sock_set_priority(int fd, int prio) +{ + /* and write it back to the kernel */ + return setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &prio, sizeof(prio)); +} + +#ifdef HAVE_LIBSCTP +/*! Fill in array of struct sctp_paddrinfo with each of the remote addresses of an SCTP socket + * \param[in] fd file descriptor of SCTP socket + * \param[out] pinfo Pointer to memory holding an array of struct sctp_paddrinfo (pinfo_cnt length). + * \param[in,out] pinfo_cnt length of pinfo array (in elements). On return it contains the number of addresses found. + * \returns 0 on success; negative otherwise + * + * Upon return, pinfo_cnt can be set to a higher value than the one set by the + * caller. This can be used by the caller to find out the required array length + * and then obtaining by calling the function twice. Only up to pinfo_cnt addresses + * are filled in, as per the value provided by the caller. + * + * Usage example retrieving struct sctp_paddrinfo for all (up to OSMO_SOCK_MAX_ADDRS, 32) remote IP addresses: + * struct sctp_paddrinfo pinfo[OSMO_SOCK_MAX_ADDRS]; + * size_t pinfo_cnt = ARRAY_SIZE(pinfo); + * rc = osmo_sock_sctp_get_peer_addr_info(fd, &pinfo[0], &num_hostbuf, pinfo_cnt); + * if (rc < 0) + * goto error; + * if (pinfo_cnt > ARRAY_SIZE(hostbuf)) + * goto not_enough_buffers; + */ +int osmo_sock_sctp_get_peer_addr_info(int fd, struct sctp_paddrinfo *pinfo, size_t *pinfo_cnt) +{ + struct sockaddr *addrs = NULL; + unsigned int n_addrs, i; + void *addr_buf; + int rc; + socklen_t optlen; + + rc = sctp_getpaddrs(fd, 0, &addrs); + + if (rc < 0) + return rc; + if (rc == 0) + return -ENOTCONN; + + n_addrs = rc; + addr_buf = (void *)addrs; + for (i = 0; i < n_addrs; i++) { + struct sockaddr *sa_addr = (struct sockaddr *)addr_buf; + size_t addrlen; + + switch (sa_addr->sa_family) { + case AF_INET: + addrlen = sizeof(struct sockaddr_in); + break; + case AF_INET6: + addrlen = sizeof(struct sockaddr_in6); + break; + default: + rc = -EINVAL; + goto free_addrs_ret; + } + + if (i >= *pinfo_cnt) { + addr_buf += addrlen; + continue; + } + + memset(&pinfo[i], 0, sizeof(pinfo[0])); + memcpy(&pinfo[i].spinfo_address, sa_addr, addrlen); + optlen = sizeof(pinfo[0]); + rc = getsockopt(fd, SOL_SCTP, SCTP_GET_PEER_ADDR_INFO, &pinfo[i], &optlen); + if (rc < 0) + goto free_addrs_ret; + + addr_buf += addrlen; + } + + *pinfo_cnt = n_addrs; + rc = 0; +free_addrs_ret: + sctp_freepaddrs(addrs); + return rc; +} +#endif + +#endif /* HAVE_SYS_SOCKET_H */ + +/*! @} */ diff --git a/src/core/soft_uart.c b/src/core/soft_uart.c new file mode 100644 index 00000000..f969ab70 --- /dev/null +++ b/src/core/soft_uart.c @@ -0,0 +1,518 @@ +/*! \file soft_uart.c + * Software UART implementation. */ +/* + * (C) 2022 by Harald Welte <laforge@gnumonks.org> + * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <stdbool.h> +#include <stdint.h> +#include <errno.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/soft_uart.h> + +/*! Rx/Tx flow state of a soft-UART */ +enum suart_flow_state { + SUART_FLOW_ST_IDLE, /*!< waiting for a start bit or Tx data */ + SUART_FLOW_ST_DATA, /*!< receiving/transmitting data bits */ + SUART_FLOW_ST_PARITY, /*!< receiving/transmitting parity bits */ + SUART_FLOW_ST_STOP, /*!< receiving/transmitting stop bits */ +}; + +/*! Internal state of a soft-UART */ +struct osmo_soft_uart { + struct osmo_soft_uart_cfg cfg; + const char *name; + /* modem status (bitmask of OSMO_SUART_STATUS_F_*) */ + unsigned int status; + struct { + bool running; + uint8_t bit_count; + uint8_t shift_reg; + struct msgb *msg; + ubit_t parity_bit; /* 0 (even) / 1 (odd) */ + unsigned int flags; + struct osmo_timer_list timer; + enum suart_flow_state flow_state; + } rx; + struct { + bool running; + uint8_t bit_count; + uint8_t shift_reg; + ubit_t parity_bit; /* 0 (even) / 1 (odd) */ + enum suart_flow_state flow_state; + } tx; +}; + +/*! Default soft-UART configuration (8-N-1) */ +const struct osmo_soft_uart_cfg osmo_soft_uart_default_cfg = { + .num_data_bits = 8, + .num_stop_bits = 1, + .parity_mode = OSMO_SUART_PARITY_NONE, + .rx_buf_size = 1024, + .rx_timeout_ms = 100, + .flow_ctrl_mode = OSMO_SUART_FLOW_CTRL_NONE, +}; + +/************************************************************************* + * Receiver + *************************************************************************/ + +/*! Flush the receive buffer, passing ownership of the msgb to the .rx_cb(). + * \param[in] suart soft-UART instance holding the receive buffer. */ +void osmo_soft_uart_flush_rx(struct osmo_soft_uart *suart) +{ + if (suart->rx.msg && msgb_length(suart->rx.msg)) { + osmo_timer_del(&suart->rx.timer); + if (suart->cfg.rx_cb) { + suart->cfg.rx_cb(suart->cfg.priv, suart->rx.msg, suart->rx.flags); + /* call-back has taken ownership of msgb, no need to free() here */ + suart->rx.msg = msgb_alloc_c(suart, suart->cfg.rx_buf_size, "soft_uart_rx"); + } else { + msgb_reset(suart->rx.msg); + } + } +} + +/* one character was received; add to receive buffer and notify user, if needed */ +static void suart_rx_ch(struct osmo_soft_uart *suart, uint8_t ch) +{ + unsigned int msg_len; + + OSMO_ASSERT(suart->rx.msg); + msgb_put_u8(suart->rx.msg, ch); + msg_len = msgb_length(suart->rx.msg); + + if (msg_len >= suart->cfg.rx_buf_size || suart->rx.flags) { + /* either the buffer is full, or we hit a parity and/or a framing error */ + osmo_soft_uart_flush_rx(suart); + } else if (msg_len == 1) { + /* first character in new message: start timer */ + osmo_timer_schedule(&suart->rx.timer, suart->cfg.rx_timeout_ms / 1000, + (suart->cfg.rx_timeout_ms % 1000) * 1000); + } +} + +/* receive a single bit */ +static inline void suart_rx_bit(struct osmo_soft_uart *suart, const ubit_t bit) +{ + switch (suart->rx.flow_state) { + case SUART_FLOW_ST_IDLE: + if (bit == 0) { /* start bit condition */ + suart->rx.flow_state = SUART_FLOW_ST_DATA; + suart->rx.flags = 0x00; + suart->rx.shift_reg = 0; + suart->rx.bit_count = 0; + suart->rx.parity_bit = 0; + } + break; + case SUART_FLOW_ST_DATA: + suart->rx.bit_count++; + suart->rx.shift_reg >>= 1; + if (bit != 0) { + suart->rx.parity_bit = !suart->rx.parity_bit; /* flip */ + suart->rx.shift_reg |= 0x80; + } + if (suart->rx.bit_count >= suart->cfg.num_data_bits) { + /* we have accumulated enough data bits */ + if (suart->cfg.parity_mode != OSMO_SUART_PARITY_NONE) + suart->rx.flow_state = SUART_FLOW_ST_PARITY; + else + suart->rx.flow_state = SUART_FLOW_ST_STOP; + /* align the register if needed */ + if (suart->cfg.num_data_bits < 8) + suart->rx.shift_reg >>= (8 - suart->cfg.num_data_bits); + } + break; + case SUART_FLOW_ST_PARITY: + switch (suart->cfg.parity_mode) { + case OSMO_SUART_PARITY_EVEN: + /* number of 1-bits (in both data and parity) shall be even */ + if (suart->rx.parity_bit != bit) + suart->rx.flags |= OSMO_SUART_F_PARITY_ERROR; + break; + case OSMO_SUART_PARITY_ODD: + /* number of 1-bits (in both data and parity) shall be odd */ + if (suart->rx.parity_bit == bit) + suart->rx.flags |= OSMO_SUART_F_PARITY_ERROR; + break; + case OSMO_SUART_PARITY_MARK: + /* parity bit must always be 1 */ + if (bit != 1) + suart->rx.flags |= OSMO_SUART_F_PARITY_ERROR; + break; + case OSMO_SUART_PARITY_SPACE: + /* parity bit must always be 0 */ + if (bit != 0) + suart->rx.flags |= OSMO_SUART_F_PARITY_ERROR; + break; + case OSMO_SUART_PARITY_NONE: /* shall not happen */ + default: + OSMO_ASSERT(0); + } + + suart->rx.flow_state = SUART_FLOW_ST_STOP; + break; + case SUART_FLOW_ST_STOP: + suart->rx.bit_count++; + if (bit != 1) + suart->rx.flags |= OSMO_SUART_F_FRAMING_ERROR; + + if (suart->rx.bit_count >= (suart->cfg.num_data_bits + suart->cfg.num_stop_bits)) { + /* we have accumulated enough stop bits */ + suart_rx_ch(suart, suart->rx.shift_reg); + suart->rx.flow_state = SUART_FLOW_ST_IDLE; + } + break; + } +} + +/* receive timer expiration: flush rx-buffer to user call-back */ +static void suart_rx_timer_cb(void *data) +{ + struct osmo_soft_uart *suart = data; + osmo_soft_uart_flush_rx(suart); +} + +/*! Feed a number of unpacked bits into the soft-UART receiver. + * \param[in] suart soft-UART instance to feed bits into. + * \param[in] ubits pointer to the unpacked bits. + * \param[in] n_ubits number of unpacked bits to be fed. + * \returns 0 on success; negative on error. + * -EAGAIN indicates that the receiver is disabled. */ +int osmo_soft_uart_rx_ubits(struct osmo_soft_uart *suart, const ubit_t *ubits, size_t n_ubits) +{ + if (!suart->rx.running) + return -EAGAIN; + for (size_t i = 0; i < n_ubits; i++) + suart_rx_bit(suart, ubits[i]); + return 0; +} + +/************************************************************************* + * Transmitter + *************************************************************************/ + +/* pull a single bit out of the UART transmitter */ +static inline ubit_t suart_tx_bit(struct osmo_soft_uart *suart, struct msgb *msg) +{ + ubit_t tx_bit = 1; + + switch (suart->tx.flow_state) { + case SUART_FLOW_ST_IDLE: + if (msg && msgb_length(msg) > 0) { /* if we have pending data */ + suart->tx.shift_reg = msgb_pull_u8(msg); + suart->tx.flow_state = SUART_FLOW_ST_DATA; + suart->tx.bit_count = 0; + suart->tx.parity_bit = 0; + tx_bit = 0; + } + break; + case SUART_FLOW_ST_DATA: + tx_bit = suart->tx.shift_reg & 1; + suart->tx.parity_bit ^= tx_bit; + suart->tx.shift_reg >>= 1; + suart->tx.bit_count++; + if (suart->tx.bit_count >= suart->cfg.num_data_bits) { + /* we have transmitted all data bits */ + if (suart->cfg.parity_mode != OSMO_SUART_PARITY_NONE) + suart->tx.flow_state = SUART_FLOW_ST_PARITY; + else + suart->tx.flow_state = SUART_FLOW_ST_STOP; + } + break; + case SUART_FLOW_ST_PARITY: + switch (suart->cfg.parity_mode) { + case OSMO_SUART_PARITY_EVEN: + /* number of 1-bits (in both data and parity) shall be even */ + tx_bit = suart->tx.parity_bit; + break; + case OSMO_SUART_PARITY_ODD: + /* number of 1-bits (in both data and parity) shall be odd */ + tx_bit = !suart->tx.parity_bit; + break; + case OSMO_SUART_PARITY_MARK: + /* parity bit must always be 1 */ + tx_bit = 1; + break; + case OSMO_SUART_PARITY_SPACE: + /* parity bit must always be 0 */ + tx_bit = 0; + break; + case OSMO_SUART_PARITY_NONE: + default: /* shall not happen */ + OSMO_ASSERT(0); + } + + suart->tx.flow_state = SUART_FLOW_ST_STOP; + break; + case SUART_FLOW_ST_STOP: + suart->tx.bit_count++; + if (suart->tx.bit_count >= (suart->cfg.num_data_bits + suart->cfg.num_stop_bits)) { + /* we have transmitted all stop bits, we're done */ + suart->tx.flow_state = SUART_FLOW_ST_IDLE; + } + break; + } + + return tx_bit; +} + +/* pull pending bits out of the UART */ +static size_t suart_tx_pending(struct osmo_soft_uart *suart, ubit_t *ubits, size_t n_ubits) +{ + size_t i; + + for (i = 0; i < n_ubits; i++) { + if (suart->tx.flow_state == SUART_FLOW_ST_IDLE) + break; + ubits[i] = suart_tx_bit(suart, NULL); + } + + return i; +} + +/*! Pull a number of unpacked bits out of the soft-UART transmitter. + * \param[in] suart soft-UART instance to pull the bits from. + * \param[out] ubits pointer to a buffer where to store pulled bits. + * \param[in] n_ubits number of unpacked bits to be pulled. + * \returns number of bits pulled (may be less than n_ubits); negative on error. + * -EAGAIN indicates that the transmitter is disabled. */ +int osmo_soft_uart_tx_ubits(struct osmo_soft_uart *suart, ubit_t *ubits, size_t n_ubits) +{ + const struct osmo_soft_uart_cfg *cfg = &suart->cfg; + size_t n_frame_bits, n_chars; + struct msgb *msg = NULL; + + if (OSMO_UNLIKELY(n_ubits == 0)) + return -EINVAL; + + if (!suart->tx.running) + return -EAGAIN; + + switch (suart->cfg.flow_ctrl_mode) { + case OSMO_SUART_FLOW_CTRL_DTR_DSR: + /* if DSR is de-asserted, Tx pending bits and suspend */ + if (~suart->status & OSMO_SUART_STATUS_F_DSR) + return suart_tx_pending(suart, ubits, n_ubits); + /* else: keep transmitting as usual */ + break; + case OSMO_SUART_FLOW_CTRL_RTS_CTS: + /* if CTS is de-asserted, Tx pending bits and suspend */ + if (~suart->status & OSMO_SUART_STATUS_F_CTS) + return suart_tx_pending(suart, ubits, n_ubits); + /* else: keep transmitting as usual */ + break; + case OSMO_SUART_FLOW_CTRL_NONE: + default: + break; + } + + /* calculate UART frame size for the effective config */ + n_frame_bits = 1 + cfg->num_data_bits + cfg->num_stop_bits; + if (cfg->parity_mode != OSMO_SUART_PARITY_NONE) + n_frame_bits += 1; + + /* calculate the number of characters we can fit into n_ubits */ + n_chars = n_ubits / n_frame_bits; + if (n_chars == 0) { + /* we can transmit at least one character */ + if (suart->tx.flow_state == SUART_FLOW_ST_IDLE) + n_chars = 1; + } + + if (n_chars > 0) { + /* allocate a Tx buffer msgb */ + msg = msgb_alloc_c(suart, n_chars, "soft_uart_tx"); + OSMO_ASSERT(msg != NULL); + + /* call the .tx_cb() to populate the Tx buffer */ + OSMO_ASSERT(cfg->tx_cb != NULL); + suart->cfg.tx_cb(cfg->priv, msg); + } + + for (size_t i = 0; i < n_ubits; i++) + ubits[i] = suart_tx_bit(suart, msg); + msgb_free(msg); + + return n_ubits; +} + +/*! Get the modem status bitmask of the given soft-UART. + * \param[in] suart soft-UART instance to get the modem status. + * \returns bitmask of OSMO_SUART_STATUS_F_*. */ +unsigned int osmo_soft_uart_get_status(const struct osmo_soft_uart *suart) +{ + return suart->status; +} + +/*! Set the modem status bitmask of the given soft-UART. + * \param[in] suart soft-UART instance to set the modem status. + * \param[in] status bitmask of OSMO_SUART_STATUS_F_*. + * \returns 0 on success; negative on error. */ +int osmo_soft_uart_set_status(struct osmo_soft_uart *suart, unsigned int status) +{ + const struct osmo_soft_uart_cfg *cfg = &suart->cfg; + + if (cfg->status_change_cb != NULL) { + if (suart->status != status) + cfg->status_change_cb(cfg->priv, status); + } + + suart->status = status; + return 0; +} + +/*! Activate/deactivate a modem status line of the given soft-UART. + * \param[in] suart soft-UART instance to update the modem status. + * \param[in] line a modem status line, one of OSMO_SUART_STATUS_F_*. + * \param[in] active activate (true) or deactivate (false) the line. */ +void osmo_soft_uart_set_status_line(struct osmo_soft_uart *suart, + enum osmo_soft_uart_status line, + bool active) +{ + unsigned int status = suart->status; + + if (active) /* assert the given line */ + status |= line; + else /* de-assert the given line */ + status &= ~line; + + osmo_soft_uart_set_status(suart, status); +} + + +/************************************************************************* + * Management / Initialization + *************************************************************************/ + +/*! Allocate a soft-UART instance. + * \param[in] ctx parent talloc context. + * \param[in] name name of the soft-UART instance. + * \param[in] cfg initial configuration of the soft-UART instance. + * \returns pointer to allocated soft-UART instance; NULL on error. */ +struct osmo_soft_uart *osmo_soft_uart_alloc(void *ctx, const char *name, + const struct osmo_soft_uart_cfg *cfg) +{ + struct osmo_soft_uart *suart = talloc_zero(ctx, struct osmo_soft_uart); + if (!suart) + return NULL; + suart->name = talloc_strdup(suart, name); + + OSMO_ASSERT(cfg != NULL); + suart->cfg = *cfg; + + return suart; +} + +/*! Release memory taken by the given soft-UART. + * \param[in] suart soft-UART instance to be free()d. */ +void osmo_soft_uart_free(struct osmo_soft_uart *suart) +{ + if (suart == NULL) + return; + + osmo_timer_del(&suart->rx.timer); + msgb_free(suart->rx.msg); + + talloc_free((void *)suart->name); + talloc_free(suart); +} + +/*! Change soft-UART configuration to the user-provided config. + * \param[in] suart soft-UART instance to be re-configured. + * \param[in] cfg the user-provided config to be applied. + * \returns 0 on success; negative on error. */ +int osmo_soft_uart_configure(struct osmo_soft_uart *suart, const struct osmo_soft_uart_cfg *cfg) +{ + /* consistency checks on the configuration */ + if (cfg->num_data_bits > 8 || cfg->num_data_bits == 0) + return -EINVAL; + if (cfg->num_stop_bits == 0) + return -EINVAL; + if (cfg->parity_mode < 0 || cfg->parity_mode >= _OSMO_SUART_PARITY_NUM) + return -EINVAL; + if (cfg->rx_buf_size == 0) + return -EINVAL; + + if (suart->cfg.rx_buf_size > cfg->rx_buf_size || + suart->cfg.rx_timeout_ms > cfg->rx_timeout_ms) { + osmo_soft_uart_flush_rx(suart); + } + + suart->cfg = *cfg; + + osmo_timer_setup(&suart->rx.timer, suart_rx_timer_cb, suart); + + return 0; +} + +/*! Get a name for the given soft-UART instance. + * \param[in] suart soft-UART instance to get the name from. + * \returns name of the given soft-UART instance. */ +const char *osmo_soft_uart_get_name(const struct osmo_soft_uart *suart) +{ + return suart->name; +} + +/*! Set a new name for the given soft-UART instance. + * \param[in] suart soft-UART instance to set the name for. + * \param[in] name the new name. */ +void osmo_soft_uart_set_name(struct osmo_soft_uart *suart, const char *name) +{ + osmo_talloc_replace_string(suart, (char **)&suart->name, name); +} + +/*! Enable/disable receiver of the given soft-UART. + * \param[in] suart soft-UART instance to be re-configured. + * \param[in] enable enable/disable state of the receiver. + * \returns 0 on success; negative on error. */ +int osmo_soft_uart_set_rx(struct osmo_soft_uart *suart, bool enable) +{ + if (!enable && suart->rx.running) { + osmo_soft_uart_flush_rx(suart); + suart->rx.running = false; + suart->rx.flow_state = SUART_FLOW_ST_IDLE; + } else if (enable && !suart->rx.running) { + if (!suart->rx.msg) + suart->rx.msg = msgb_alloc_c(suart, suart->cfg.rx_buf_size, "soft_uart_rx"); + suart->rx.running = true; + suart->rx.flow_state = SUART_FLOW_ST_IDLE; + } + + return 0; +} + +/*! Enable/disable transmitter of the given soft-UART. + * \param[in] suart soft-UART instance to be re-configured. + * \param[in] enable enable/disable state of the transmitter. + * \returns 0 on success; negative on error. */ +int osmo_soft_uart_set_tx(struct osmo_soft_uart *suart, bool enable) +{ + if (!enable && suart->tx.running) { + suart->tx.running = false; + suart->tx.flow_state = SUART_FLOW_ST_IDLE; + } else if (enable && !suart->tx.running) { + suart->tx.running = true; + suart->tx.flow_state = SUART_FLOW_ST_IDLE; + } + + return 0; +} diff --git a/src/core/stat_item.c b/src/core/stat_item.c new file mode 100644 index 00000000..804972bf --- /dev/null +++ b/src/core/stat_item.c @@ -0,0 +1,463 @@ +/*! \file stat_item.c + * utility routines for keeping statistical values */ +/* + * (C) 2009-2010 by Harald Welte <laforge@gnumonks.org> + * (C) 2015 by sysmocom - s.f.m.c. GmbH + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +/*! \addtogroup osmo_stat_item + * @{ + * + * This osmo_stat_item module adds instrumentation capabilities to + * gather measurement and statistical values in a similar fashion to + * what we have as \ref osmo_counter_group. + * + * As opposed to counters, osmo_stat_item do not increment but consist + * of a configurable-sized FIFO, which can store not only the current + * (most recent) value, but also historic values. + * + * The only supported value type is an int32_t. + * + * Getting values from osmo_stat_item is usually done at a high level + * through the stats API (stats.c). It uses item->stats_next_id to + * store what has been sent to all enabled reporters. It is also + * possible to read from osmo_stat_item directly, without modifying + * its state, by storing next_id outside of osmo_stat_item. + * + * Each value stored in the FIFO of an osmo_stat_item has an associated + * value_id. The value_id is increased with each value, so (until the + * counter wraps) more recent values will have higher values. + * + * When a new value is set, the oldest value in the FIFO gets silently + * overwritten. Lost values are skipped when getting values from the + * item. + * + */ + +/* Struct overview: + * + * Group and item descriptions: + * Each group description exists once as osmo_stat_item_group_desc, + * each such group description lists N osmo_stat_item_desc, i.e. describes N stat items. + * + * Actual stats: + * The global osmo_stat_item_groups llist contains all group instances, each points at a group description. + * This list mixes all types of groups in a single llist, where each instance points at its group desc and has an index. + * There are one or more instances of each group, each storing stats for a distinct object (for example, one description + * for a BTS group, and any number of BTS instances with independent stats). A group is identified by a group index nr + * and possibly also a given name for that particular index (e.g. in osmo-mgw, a group instance is named + * "virtual-trunk-0" and can be looked up by that name instead of its more or less arbitrary group index number). + * + * Each group instance contains one osmo_stat_item instance per global stat item description. + * Each osmo_stat_item keeps track of the values for the current reporting period (min, last, max, sum, n), + * and also stores the set of values reported at the end of the previous reporting period. + * + * const osmo_stat_item_group_desc foo + * +-- group_name_prefix = "foo" + * +-- item_desc[] (array of osmo_stat_item_desc) + * +-- osmo_stat_item_desc bar + * | +-- name = "bar" + * | +-- description + * | +-- unit + * | +-- default_value + * | + * +-- osmo_stat_item_desc: baz + * +-- ... + * + * const osmo_stat_item_group_desc moo + * +-- group_name_prefix = "moo" + * +-- item_desc[] + * +-- osmo_stat_item_desc goo + * | +-- name = "goo" + * | +-- description + * | +-- unit + * | +-- default_value + * | + * +-- osmo_stat_item_desc: loo + * +-- ... + * + * osmo_stat_item_groups (llist of osmo_stat_item_group) + * | + * +-- group: foo[0] + * | +-- desc --> osmo_stat_item_group_desc foo + * | +-- idx = 0 + * | +-- name = NULL (no given name for this group instance) + * | +-- items[] + * | | + * | [0] --> osmo_stat_item instance for "bar" + * | | +-- desc --> osmo_stat_item_desc bar (see above) + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | | + * | [1] --> osmo_stat_item instance for "baz" + * | | +-- desc --> osmo_stat_item_desc baz + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | . + * | : + * | + * +-- group: foo[1] + * | +-- desc --> osmo_stat_item_group_desc foo + * | +-- idx = 1 + * | +-- name = "special-foo" (instance can be looked up by this index-name) + * | +-- items[] + * | | + * | [0] --> osmo_stat_item instance for "bar" + * | | +-- desc --> osmo_stat_item_desc bar + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | | + * | [1] --> osmo_stat_item instance for "baz" + * | | +-- desc --> osmo_stat_item_desc baz + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | . + * | : + * | + * +-- group: moo[0] + * | +-- desc --> osmo_stat_item_group_desc moo + * | +-- idx = 0 + * | +-- name = NULL + * | +-- items[] + * | | + * | [0] --> osmo_stat_item instance for "goo" + * | | +-- desc --> osmo_stat_item_desc goo + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | | + * | [1] --> osmo_stat_item instance for "loo" + * | | +-- desc --> osmo_stat_item_desc loo + * | | +-- value.{min, last, max, n, sum} + * | | +-- reported.{min, last, max, n, sum} + * | . + * | : + * . + * : + * + */ + +#include <stdint.h> +#include <string.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/stat_item.h> + +#include <stat_item_internal.h> + +/*! global list of stat_item groups */ +static LLIST_HEAD(osmo_stat_item_groups); + +/*! talloc context from which we allocate */ +static void *tall_stat_item_ctx; + +/*! Allocate a new group of counters according to description. + * Allocate a group of stat items described in \a desc from talloc context \a ctx, + * giving the new group the index \a idx. + * \param[in] ctx \ref talloc context + * \param[in] desc Statistics item group description + * \param[in] idx Index of new stat item group + */ +struct osmo_stat_item_group *osmo_stat_item_group_alloc(void *ctx, + const struct osmo_stat_item_group_desc *group_desc, + unsigned int idx) +{ + unsigned int group_size; + unsigned int item_idx; + struct osmo_stat_item *items; + + struct osmo_stat_item_group *group; + + group_size = sizeof(struct osmo_stat_item_group) + + group_desc->num_items * sizeof(struct osmo_stat_item *); + + if (!ctx) + ctx = tall_stat_item_ctx; + + group = talloc_zero_size(ctx, group_size); + if (!group) + return NULL; + + group->desc = group_desc; + group->idx = idx; + + items = talloc_array(group, struct osmo_stat_item, group_desc->num_items); + OSMO_ASSERT(items); + for (item_idx = 0; item_idx < group_desc->num_items; item_idx++) { + struct osmo_stat_item *item = &items[item_idx]; + const struct osmo_stat_item_desc *item_desc = &group_desc->item_desc[item_idx]; + group->items[item_idx] = item; + *item = (struct osmo_stat_item){ + .desc = item_desc, + .value = { + .n = 0, + .last = item_desc->default_value, + .min = item_desc->default_value, + .max = item_desc->default_value, + .sum = 0, + }, + }; + } + + llist_add(&group->list, &osmo_stat_item_groups); + return group; +} + +/*! Free the memory for the specified group of stat items */ +void osmo_stat_item_group_free(struct osmo_stat_item_group *grp) +{ + if (!grp) + return; + + llist_del(&grp->list); + talloc_free(grp); +} + +/*! Get statistics item from group, identified by index idx + * \param[in] grp Rate counter group + * \param[in] idx Index of the counter to retrieve + * \returns rate counter requested + */ +struct osmo_stat_item *osmo_stat_item_group_get_item(struct osmo_stat_item_group *grp, unsigned int idx) +{ + return grp->items[idx]; +} + +/*! Set a name for the statistics item group to be used instead of index value + at report time. + * \param[in] statg Statistics item group + * \param[in] name Name identifier to assign to the statistics item group + */ +void osmo_stat_item_group_set_name(struct osmo_stat_item_group *statg, const char *name) +{ + osmo_talloc_replace_string(statg, &statg->name, name); +} + +/*! Increase the stat_item to the given value. + * This function adds a new value for the given stat_item at the end of + * the FIFO. + * \param[in] item The stat_item whose \a value we want to set + * \param[in] value The numeric value we want to store at end of FIFO + */ +void osmo_stat_item_inc(struct osmo_stat_item *item, int32_t value) +{ + osmo_stat_item_set(item, item->value.last + value); +} + +/*! Descrease the stat_item to the given value. + * This function adds a new value for the given stat_item at the end of + * the FIFO. + * \param[in] item The stat_item whose \a value we want to set + * \param[in] value The numeric value we want to store at end of FIFO + */ +void osmo_stat_item_dec(struct osmo_stat_item *item, int32_t value) +{ + osmo_stat_item_set(item, item->value.last - value); +} + +/*! Set the a given stat_item to the given value. + * This function adds a new value for the given stat_item at the end of + * the FIFO. + * \param[in] item The stat_item whose \a value we want to set + * \param[in] value The numeric value we want to store at end of FIFO + */ +void osmo_stat_item_set(struct osmo_stat_item *item, int32_t value) +{ + item->value.last = value; + if (item->value.n == 0) { + /* No values recorded yet, clamp min and max to this first value. */ + item->value.min = item->value.max = value; + /* Overwrite any cruft remaining in value.sum */ + item->value.sum = value; + item->value.n = 1; + } else { + item->value.min = OSMO_MIN(item->value.min, value); + item->value.max = OSMO_MAX(item->value.max, value); + item->value.sum += value; + item->value.n++; + } +} + +/*! Indicate that a reporting period has elapsed, and prepare the stat item for a new period of collecting min/max/avg. + * \param item Stat item to flush. + */ +void osmo_stat_item_flush(struct osmo_stat_item *item) +{ + item->reported = item->value; + + /* Indicate a new reporting period: no values have been received, but the previous value.last remains unchanged + * for the case that an entire period elapses without a new value appearing. */ + item->value.n = 0; + item->value.sum = 0; + + /* Also for the case that an entire period elapses without any osmo_stat_item_set(), put the min and max to the + * last value. As soon as one osmo_stat_item_set() occurs, these are both set to the new value (when n is still + * zero from above). */ + item->value.min = item->value.max = item->value.last; +} + +/*! Initialize the stat item module. Call this once from your program. + * \param[in] tall_ctx Talloc context from which this module allocates */ +int osmo_stat_item_init(void *tall_ctx) +{ + tall_stat_item_ctx = tall_ctx; + + return 0; +} + +/*! Search for item group based on group name and index + * \param[in] name Name of stats_item_group we want to find + * \param[in] idx Index of the group we want to find + * \returns pointer to group, if found; NULL otherwise */ +struct osmo_stat_item_group *osmo_stat_item_get_group_by_name_idx( + const char *name, const unsigned int idx) +{ + struct osmo_stat_item_group *statg; + + llist_for_each_entry(statg, &osmo_stat_item_groups, list) { + if (!statg->desc) + continue; + + if (!strcmp(statg->desc->group_name_prefix, name) && + statg->idx == idx) + return statg; + } + return NULL; +} + +/*! Search for item group based on group name and index's name. + * \param[in] name Name of stats_item_group we want to find. + * \param[in] idx_name Index of the group we want to find, by the index's name (osmo_stat_item_group->name). + * \returns pointer to group, if found; NULL otherwise. */ +struct osmo_stat_item_group *osmo_stat_item_get_group_by_name_idxname(const char *group_name, const char *idx_name) +{ + struct osmo_stat_item_group *statg; + + llist_for_each_entry(statg, &osmo_stat_item_groups, list) { + if (!statg->desc || !statg->name) + continue; + if (strcmp(statg->desc->group_name_prefix, group_name)) + continue; + if (strcmp(statg->name, idx_name)) + continue; + return statg; + } + return NULL; +} + +/*! Search for item based on group + item name + * \param[in] statg group in which to search for the item + * \param[in] name name of item to search within \a statg + * \returns pointer to item, if found; NULL otherwise */ +const struct osmo_stat_item *osmo_stat_item_get_by_name( + const struct osmo_stat_item_group *statg, const char *name) +{ + int i; + const struct osmo_stat_item_desc *item_desc; + + if (!statg->desc) + return NULL; + + for (i = 0; i < statg->desc->num_items; i++) { + item_desc = &statg->desc->item_desc[i]; + + if (!strcmp(item_desc->name, name)) { + return statg->items[i]; + } + } + return NULL; +} + +/*! Iterate over all items in group, call user-supplied function on each + * \param[in] statg stat_item group over whose items to iterate + * \param[in] handle_item Call-back function, aborts if rc < 0 + * \param[in] data Private data handed through to \a handle_item + */ +int osmo_stat_item_for_each_item(struct osmo_stat_item_group *statg, + osmo_stat_item_handler_t handle_item, void *data) +{ + int rc = 0; + int i; + + for (i = 0; i < statg->desc->num_items; i++) { + struct osmo_stat_item *item = statg->items[i]; + rc = handle_item(statg, item, data); + if (rc < 0) + return rc; + } + + return rc; +} + +/*! Iterate over all stat_item groups in system, call user-supplied function on each + * \param[in] handle_group Call-back function, aborts if rc < 0 + * \param[in] data Private data handed through to \a handle_group + */ +int osmo_stat_item_for_each_group(osmo_stat_item_group_handler_t handle_group, void *data) +{ + struct osmo_stat_item_group *statg; + int rc = 0; + + llist_for_each_entry(statg, &osmo_stat_item_groups, list) { + rc = handle_group(statg, data); + if (rc < 0) + return rc; + } + + return rc; +} + +/*! Get the last (freshest) value. */ +int32_t osmo_stat_item_get_last(const struct osmo_stat_item *item) +{ + return item->value.last; +} + +/*! Remove all values of a stat item + * \param[in] item stat item to reset + */ +void osmo_stat_item_reset(struct osmo_stat_item *item) +{ + item->value.sum = 0; + item->value.n = 0; + item->value.last = item->value.min = item->value.max = item->desc->default_value; +} + +/*! Reset all osmo stat items in a group + * \param[in] statg stat item group to reset + */ +void osmo_stat_item_group_reset(struct osmo_stat_item_group *statg) +{ + int i; + + for (i = 0; i < statg->desc->num_items; i++) { + struct osmo_stat_item *item = statg->items[i]; + osmo_stat_item_reset(item); + } +} + +/*! Return the description for an osmo_stat_item. */ +const struct osmo_stat_item_desc *osmo_stat_item_get_desc(struct osmo_stat_item *item) +{ + return item->desc; +} + +/*! @} */ diff --git a/src/core/stat_item_internal.h b/src/core/stat_item_internal.h new file mode 100644 index 00000000..9ede8c41 --- /dev/null +++ b/src/core/stat_item_internal.h @@ -0,0 +1,35 @@ +/*! \file stat_item_internal.h + * internal definitions for the osmo_stat_item API */ +#pragma once + +/*! \addtogroup osmo_stat_item + * @{ + */ + +struct osmo_stat_item_period { + /*! Number of osmo_stat_item_set() that occurred during the reporting period, zero if none. */ + uint32_t n; + /*! Smallest value seen in a reporting period. */ + int32_t min; + /*! Most recent value passed to osmo_stat_item_set(), or the item->desc->default_value if none. */ + int32_t last; + /*! Largest value seen in a reporting period. */ + int32_t max; + /*! Sum of all values passed to osmo_stat_item_set() in the reporting period. */ + int64_t sum; +}; + +/*! data we keep for each actual item */ +struct osmo_stat_item { + /*! back-reference to the item description */ + const struct osmo_stat_item_desc *desc; + + /*! Current reporting period / current value. */ + struct osmo_stat_item_period value; + + /*! The results of the previous reporting period. According to these, the stats reporter decides whether to + * re-send values or omit an unchanged value from a report. */ + struct osmo_stat_item_period reported; +}; + +/*! @} */ diff --git a/src/stats.c b/src/core/stats.c index b5adbf29..16e6f620 100644 --- a/src/stats.c +++ b/src/core/stats.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup stats @@ -74,6 +70,7 @@ #include <errno.h> #include <stdio.h> #include <sys/types.h> +#include <inttypes.h> #ifdef HAVE_SYS_SOCKET_H #include <sys/socket.h> @@ -85,24 +82,37 @@ #include <osmocom/core/logging.h> #include <osmocom/core/rate_ctr.h> #include <osmocom/core/stat_item.h> -#include <osmocom/core/timer.h> +#include <osmocom/core/select.h> #include <osmocom/core/counter.h> #include <osmocom/core/msgb.h> +#include <osmocom/core/stats_tcp.h> + +#ifdef HAVE_SYSTEMTAP +/* include the generated probes header and put markers in code */ +#include "probes.h" +#define TRACE(probe) probe +#define TRACE_ENABLED(probe) probe ## _ENABLED() +#else +/* Wrap the probe to allow it to be removed when no systemtap available */ +#define TRACE(probe) +#define TRACE_ENABLED(probe) (0) +#endif /* HAVE_SYSTEMTAP */ + +#include <stat_item_internal.h> #define STATS_DEFAULT_INTERVAL 5 /* secs */ #define STATS_DEFAULT_BUFLEN 256 -static LLIST_HEAD(osmo_stats_reporter_list); +LLIST_HEAD(osmo_stats_reporter_list); static void *osmo_stats_ctx = NULL; static int is_initialised = 0; -static int32_t current_stat_item_index = 0; static struct osmo_stats_config s_stats_config = { .interval = STATS_DEFAULT_INTERVAL, }; struct osmo_stats_config *osmo_stats_config = &s_stats_config; -static struct osmo_timer_list osmo_stats_timer; +static struct osmo_fd osmo_stats_timer = { .fd = -1 }; static int osmo_stats_reporter_log_send_counter(struct osmo_stats_reporter *srep, const struct rate_ctr_group *ctrg, @@ -140,23 +150,61 @@ static int update_srep_config(struct osmo_stats_reporter *srep) return rc; } -static void osmo_stats_timer_cb(void *data) +static int osmo_stats_timer_cb(struct osmo_fd *ofd, unsigned int what) { - int interval = osmo_stats_config->interval; + uint64_t expire_count; + int rc; + + /* check that the timer has actually expired */ + if (!(what & OSMO_FD_READ)) + return 0; + + /* read from timerfd: number of expirations of periodic timer */ + rc = read(ofd->fd, (void *) &expire_count, sizeof(expire_count)); + if (rc < 0 && errno == EAGAIN) + return 0; + OSMO_ASSERT(rc == sizeof(expire_count)); + + if (expire_count > 1) + LOGP(DLSTATS, LOGL_NOTICE, "Stats timer expire_count=%" PRIu64 ": We missed %" PRIu64 " timers\n", + expire_count, expire_count-1); if (!llist_empty(&osmo_stats_reporter_list)) osmo_stats_report(); - osmo_timer_schedule(&osmo_stats_timer, interval, 0); + return 0; } -static int start_timer() +static int start_timer(void) { + int rc; + int interval = osmo_stats_config->interval; + if (!is_initialised) return -ESRCH; - osmo_timer_setup(&osmo_stats_timer, osmo_stats_timer_cb, NULL); - osmo_timer_schedule(&osmo_stats_timer, 0, 1); + struct timespec ts_first = {.tv_sec=0, .tv_nsec=1000}; + struct timespec ts_interval = {.tv_sec=interval, .tv_nsec=0}; + + rc = osmo_timerfd_setup(&osmo_stats_timer, osmo_stats_timer_cb, NULL); + if (rc < 0) + LOGP(DLSTATS, LOGL_ERROR, "Failed to setup the timer with error code %d (fd=%d)\n", + rc, osmo_stats_timer.fd); + + if (interval == 0) { + rc = osmo_timerfd_disable(&osmo_stats_timer); + if (rc < 0) + LOGP(DLSTATS, LOGL_ERROR, "Failed to disable the timer with error code %d (fd=%d)\n", + rc, osmo_stats_timer.fd); + } else { + + rc = osmo_timerfd_schedule(&osmo_stats_timer, &ts_first, &ts_interval); + if (rc < 0) + LOGP(DLSTATS, LOGL_ERROR, "Failed to schedule the timer with error code %d (fd=%d, interval %d sec)\n", + rc, osmo_stats_timer.fd, interval); + + LOGP(DLSTATS, LOGL_INFO, "Stats timer started with interval %d sec\n", interval); + } return 0; } @@ -166,13 +214,15 @@ struct osmo_stats_reporter *osmo_stats_reporter_alloc(enum osmo_stats_reporter_t { struct osmo_stats_reporter *srep; srep = talloc_zero(osmo_stats_ctx, struct osmo_stats_reporter); - OSMO_ASSERT(srep); + if (!srep) + return NULL; + srep->type = type; if (name) srep->name = talloc_strdup(srep, name); srep->fd = -1; - llist_add(&srep->list, &osmo_stats_reporter_list); + llist_add_tail(&srep->list, &osmo_stats_reporter_list); return srep; } @@ -186,15 +236,17 @@ void osmo_stats_reporter_free(struct osmo_stats_reporter *srep) talloc_free(srep); } -/*! Initilize the stats reporting module; call this once in your program +/*! Initialize the stats reporting module; call this once in your program. * \param[in] ctx Talloc context from which stats related memory is allocated */ void osmo_stats_init(void *ctx) { osmo_stats_ctx = ctx; - osmo_stat_item_discard_all(¤t_stat_item_index); - is_initialised = 1; start_timer(); + + /* Make sure that the tcp-stats interval timer also runs at its + * preconfigured rate. The vty might change this setting later. */ + osmo_stats_tcp_set_interval(osmo_tcp_stats_config->interval); } /*! Find a stats_reporter of given \a type and \a name. @@ -326,13 +378,12 @@ int osmo_stats_reporter_set_max_class(struct osmo_stats_reporter *srep, return 0; } -/*! Set the reporting interval of a given stats_reporter (in seconds). - * \param[in] srep stats_reporter whose remote address is to be set +/*! Set the reporting interval (common for all reporters) * \param[in] interval Reporting interval in seconds * \returns 0 on success; negative on error */ int osmo_stats_set_interval(int interval) { - if (interval <= 0) + if (interval < 0) return -EINVAL; osmo_stats_config->interval = interval; @@ -342,9 +393,28 @@ int osmo_stats_set_interval(int interval) return 0; } +/*! Set the regular flush period for a given stats_reporter + * + * Send all stats even if they have not changed (i.e. force the flush) + * every N-th reporting interval. Set to 0 to disable regular flush, + * set to 1 to flush every time, set to 2 to flush every 2nd time, etc. + * \param[in] srep stats_reporter to set flush period for + * \param[in] period Reporting interval in seconds + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_set_flush_period(struct osmo_stats_reporter *srep, unsigned int period) +{ + srep->flush_period = period; + srep->flush_period_counter = 0; + /* force the flush now if it's not disabled by period=0 */ + if (period > 0) + srep->force_single_flush = 1; + + return 0; +} + /*! Set the name prefix of a given stats_reporter. * \param[in] srep stats_reporter whose name prefix is to be set - * \param[in] prefix NAme perfix to pre-pend for any reported value + * \param[in] prefix Name prefix to pre-pend for any reported value * \returns 0 on success; negative on error */ int osmo_stats_reporter_set_name_prefix(struct osmo_stats_reporter *srep, const char *prefix) { @@ -418,6 +488,8 @@ int osmo_stats_reporter_udp_open(struct osmo_stats_reporter *srep) } srep->buffer = msgb_alloc(buffer_size, "stats buffer"); + if (!srep->buffer) + goto failed; return 0; @@ -501,6 +573,8 @@ struct osmo_stats_reporter *osmo_stats_reporter_create_log(const char *name) { struct osmo_stats_reporter *srep; srep = osmo_stats_reporter_alloc(OSMO_STATS_REPORTER_LOG, name); + if (!srep) + return NULL; srep->have_net_config = 0; @@ -626,36 +700,28 @@ static int osmo_stat_item_handler( struct osmo_stat_item_group *statg, struct osmo_stat_item *item, void *sctx_) { struct osmo_stats_reporter *srep; - int32_t idx = current_stat_item_index; - int32_t value; - int have_value; - - have_value = osmo_stat_item_get_next(item, &idx, &value) > 0; - if (!have_value) - /* Send the last value in case a flush is requested */ - value = osmo_stat_item_get_last(item); - - do { - llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { - if (!srep->running) - continue; + int32_t prev_reported_value = item->reported.max; + int32_t new_value = item->value.max; - if (!have_value && !srep->force_single_flush) - continue; + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (!srep->running) + continue; - if (!osmo_stats_reporter_check_config(srep, - statg->idx, statg->desc->class_id)) - continue; + /* If the previously reported value is the same as the current value, skip resending the value. + * However, if the stats reporter is set to resend all values, do resend the current value regardless of + * repetitions. + */ + if (new_value == prev_reported_value && !srep->force_single_flush) + continue; - osmo_stats_reporter_send_item(srep, statg, - item->desc, value); - } + if (!osmo_stats_reporter_check_config(srep, + statg->idx, statg->desc->class_id)) + continue; - if (!have_value) - break; + osmo_stats_reporter_send_item(srep, statg, item->desc, new_value); + } - have_value = osmo_stat_item_get_next(item, &idx, &value) > 0; - } while (have_value); + osmo_stat_item_flush(item); return 0; } @@ -698,7 +764,7 @@ static int handle_counter(struct osmo_counter *counter, void *sctx_) /*** main reporting function ***/ -static void flush_all_reporters() +static void flush_all_reporters(void) { struct osmo_stats_reporter *srep; @@ -707,20 +773,31 @@ static void flush_all_reporters() continue; osmo_stats_reporter_send_buffer(srep); + + /* reset force_single_flush first */ srep->force_single_flush = 0; + /* and schedule a new flush if it's time for it */ + if (srep->flush_period > 0) { + srep->flush_period_counter++; + if (srep->flush_period_counter >= srep->flush_period) { + srep->force_single_flush = 1; + srep->flush_period_counter = 0; + } + } } } -int osmo_stats_report() +int osmo_stats_report(void) { /* per group actions */ + TRACE(LIBOSMOCORE_STATS_START()); osmo_counters_for_each(handle_counter, NULL); rate_ctr_for_each_group(rate_ctr_group_handler, NULL); osmo_stat_item_for_each_group(osmo_stat_item_group_handler, NULL); /* global actions */ - osmo_stat_item_discard_all(¤t_stat_item_index); flush_all_reporters(); + TRACE(LIBOSMOCORE_STATS_DONE()); return 0; } diff --git a/src/stats_statsd.c b/src/core/stats_statsd.c index c3f739e2..b27baff8 100644 --- a/src/stats_statsd.c +++ b/src/core/stats_statsd.c @@ -15,10 +15,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup stats @@ -32,6 +28,7 @@ #include <string.h> #include <stdint.h> +#include <inttypes.h> #include <errno.h> #include <osmocom/core/utils.h> @@ -57,6 +54,8 @@ struct osmo_stats_reporter *osmo_stats_reporter_create_statsd(const char *name) { struct osmo_stats_reporter *srep; srep = osmo_stats_reporter_alloc(OSMO_STATS_REPORTER_STATSD, name); + if (!srep) + return NULL; srep->have_net_config = 1; @@ -88,7 +87,7 @@ static void osmo_stats_reporter_sanitize_name(char *buf) } static int osmo_stats_reporter_statsd_send(struct osmo_stats_reporter *srep, - const char *name1, unsigned int index1, const char *name2, int64_t value, + const char *name1, const char *index1, const char *name2, int64_t value, const char *unit) { char *buf; @@ -99,24 +98,16 @@ static int osmo_stats_reporter_statsd_send(struct osmo_stats_reporter *srep, int old_len = msgb_length(srep->buffer); if (prefix) { - if (name1) { - if (index1 != 0) - fmt = "%1$s.%2$s.%6$u.%3$s:%4$d|%5$s"; - else - fmt = "%1$s.%2$s.%3$s:%4$d|%5$s"; - } else { - fmt = "%1$s.%2$0.0s%3$s:%4$d|%5$s"; - } + if (name1) + fmt = "%1$s.%2$s.%6$s.%3$s:%4$" PRId64 "|%5$s"; + else + fmt = "%1$s.%2$0.0s%3$s:%4$" PRId64 "|%5$s"; } else { prefix = ""; - if (name1) { - if (index1 != 0) - fmt = "%1$s%2$s.%6$u.%3$s:%4$d|%5$s"; - else - fmt = "%1$s%2$s.%3$s:%4$d|%5$s"; - } else { - fmt = "%1$s%2$0.0s%3$s:%4$d|%5$s"; - } + if (name1) + fmt = "%1$s%2$s.%6$s.%3$s:%4$" PRId64 "|%5$s"; + else + fmt = "%1$s%2$0.0s%3$s:%4$" PRId64 "|%5$s"; } if (srep->agg_enabled) { @@ -169,32 +160,42 @@ static int osmo_stats_reporter_statsd_send_counter(struct osmo_stats_reporter *s const struct rate_ctr_desc *desc, int64_t value, int64_t delta) { - if (ctrg) - return osmo_stats_reporter_statsd_send(srep, - ctrg->desc->group_name_prefix, - ctrg->idx, - desc->name, delta, "c"); - else - return osmo_stats_reporter_statsd_send(srep, - NULL, 0, - desc->name, delta, "c"); + char buf_idx[64]; + const char *idx_name = buf_idx; + const char *prefix; + + if (ctrg) { + prefix = ctrg->desc->group_name_prefix; + if (ctrg->name) + idx_name = ctrg->name; + else + snprintf(buf_idx, sizeof(buf_idx), "%u", ctrg->idx); + } else { + prefix = NULL; + buf_idx[0] = '0'; + buf_idx[1] = '\n'; + } + return osmo_stats_reporter_statsd_send(srep, prefix, idx_name, desc->name, delta, "c"); } static int osmo_stats_reporter_statsd_send_item(struct osmo_stats_reporter *srep, const struct osmo_stat_item_group *statg, const struct osmo_stat_item_desc *desc, int64_t value) { - if (value < 0) { - return osmo_stats_reporter_statsd_send(srep, - statg->desc->group_name_prefix, - statg->idx, - desc->name, 0, "g"); - } else { - return osmo_stats_reporter_statsd_send(srep, - statg->desc->group_name_prefix, - statg->idx, - desc->name, value, "g"); + char buf_idx[64]; + char *idx_name; + if (statg->name) + idx_name = statg->name; + else { + snprintf(buf_idx, sizeof(buf_idx), "%u", statg->idx); + idx_name = buf_idx; } + + if (value < 0) + value = 0; + + return osmo_stats_reporter_statsd_send(srep, statg->desc->group_name_prefix, + idx_name, desc->name, value, "g"); } #endif /* !EMBEDDED */ diff --git a/src/core/stats_tcp.c b/src/core/stats_tcp.c new file mode 100644 index 00000000..c6459fe8 --- /dev/null +++ b/src/core/stats_tcp.c @@ -0,0 +1,327 @@ +/* + * (C) 2021 by sysmocom - s.f.m.c. GmbH + * Author: Philipp Maier <pmaier@sysmocom.de> + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +/*! \addtogroup stats + * @{ + * \file stats_tcp.c */ + +#include "config.h" +#if !defined(EMBEDDED) + +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/socket.h> +#include <netinet/in.h> +#include <netinet/ip.h> +#include <linux/tcp.h> +#include <errno.h> +#include <pthread.h> + +#include <osmocom/core/select.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/stats.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/stats_tcp.h> + +static struct osmo_tcp_stats_config s_tcp_stats_config = { + .interval = TCP_STATS_DEFAULT_INTERVAL, +}; + +struct osmo_tcp_stats_config *osmo_tcp_stats_config = &s_tcp_stats_config; + +static struct osmo_timer_list stats_tcp_poll_timer; + +static LLIST_HEAD(stats_tcp); +static struct stats_tcp_entry *stats_tcp_entry_cur; +pthread_mutex_t stats_tcp_lock; + +struct stats_tcp_entry { + struct llist_head entry; + const struct osmo_fd *fd; + struct osmo_stat_item_group *stats_tcp; + const char *name; +}; + +enum { + STATS_TCP_UNACKED, + STATS_TCP_LOST, + STATS_TCP_RETRANS, + STATS_TCP_RTT, + STATS_TCP_RCV_RTT, + STATS_TCP_NOTSENT_BYTES, + STATS_TCP_RWND_LIMITED, + STATS_TCP_SNDBUF_LIMITED, + STATS_TCP_REORD_SEEN, +}; + +static struct osmo_stat_item_desc stats_tcp_item_desc[] = { + [STATS_TCP_UNACKED] = { "tcp:unacked", "unacknowledged packets", "", 60, 0 }, + [STATS_TCP_LOST] = { "tcp:lost", "lost packets", "", 60, 0 }, + [STATS_TCP_RETRANS] = { "tcp:retrans", "retransmitted packets", "", 60, 0 }, + [STATS_TCP_RTT] = { "tcp:rtt", "roundtrip-time", "", 60, 0 }, + [STATS_TCP_RCV_RTT] = { "tcp:rcv_rtt", "roundtrip-time (receive)", "", 60, 0 }, + [STATS_TCP_NOTSENT_BYTES] = { "tcp:notsent_bytes", "bytes not yet sent", "", 60, 0 }, + [STATS_TCP_RWND_LIMITED] = { "tcp:rwnd_limited", "time (usec) limited by receive window", "", 60, 0 }, + [STATS_TCP_SNDBUF_LIMITED] = { "tcp:sndbuf_limited", "Time (usec) limited by send buffer", "", 60, 0 }, + [STATS_TCP_REORD_SEEN] = { "tcp:reord_seen", "reordering events seen", "", 60, 0 }, +}; + +static struct osmo_stat_item_group_desc stats_tcp_desc = { + .group_name_prefix = "tcp", + .group_description = "stats tcp", + .class_id = OSMO_STATS_CLASS_GLOBAL, + .num_items = ARRAY_SIZE(stats_tcp_item_desc), + .item_desc = stats_tcp_item_desc, +}; + +static void fill_stats(struct stats_tcp_entry *stats_tcp_entry) +{ + int rc; + struct tcp_info tcp_info; + socklen_t tcp_info_len = sizeof(tcp_info); + char stat_name[256]; + + /* Do not fill in anything before the socket is connected to a remote end */ + if (osmo_sock_get_ip_and_port(stats_tcp_entry->fd->fd, NULL, 0, NULL, 0, false) != 0) + return; + + /* Gather TCP statistics and update the stats items */ + rc = getsockopt(stats_tcp_entry->fd->fd, IPPROTO_TCP, TCP_INFO, &tcp_info, &tcp_info_len); + if (rc < 0) + return; + + /* Create stats items if they do not exist yet */ + if (!stats_tcp_entry->stats_tcp) { + stats_tcp_entry->stats_tcp = + osmo_stat_item_group_alloc(stats_tcp_entry, &stats_tcp_desc, stats_tcp_entry->fd->fd); + OSMO_ASSERT(stats_tcp_entry->stats_tcp); + } + + /* Update statistics */ + if (stats_tcp_entry->name) + snprintf(stat_name, sizeof(stat_name), "%s", stats_tcp_entry->name); + else + snprintf(stat_name, sizeof(stat_name), "%s", osmo_sock_get_name2(stats_tcp_entry->fd->fd)); + osmo_stat_item_group_set_name(stats_tcp_entry->stats_tcp, stat_name); + + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_UNACKED), + tcp_info.tcpi_unacked); + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_LOST), + tcp_info.tcpi_lost); + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_RETRANS), + tcp_info.tcpi_retrans); + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_RTT), tcp_info.tcpi_rtt); + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_RCV_RTT), + tcp_info.tcpi_rcv_rtt); +#if HAVE_TCP_INFO_TCPI_NOTSENT_BYTES == 1 + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_NOTSENT_BYTES), + tcp_info.tcpi_notsent_bytes); +#else + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_NOTSENT_BYTES), -1); +#endif + +#if HAVE_TCP_INFO_TCPI_RWND_LIMITED == 1 + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_RWND_LIMITED), + tcp_info.tcpi_rwnd_limited); +#else + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_RWND_LIMITED), -1); +#endif + +#if STATS_TCP_SNDBUF_LIMITED == 1 + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_REORD_SEEN), + tcp_info.tcpi_sndbuf_limited); +#else + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_REORD_SEEN), -1); +#endif + +#if HAVE_TCP_INFO_TCPI_REORD_SEEN == 1 + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_REORD_SEEN), + tcp_info.tcpi_reord_seen); +#else + osmo_stat_item_set(osmo_stat_item_group_get_item(stats_tcp_entry->stats_tcp, STATS_TCP_REORD_SEEN), -1); +#endif + +} + +static bool is_tcp(const struct osmo_fd *fd) +{ + int rc; + struct stat fd_stat; + int so_protocol = 0; + socklen_t so_protocol_len = sizeof(so_protocol); + + /* Is this a socket? */ + rc = fstat(fd->fd, &fd_stat); + if (rc < 0) + return false; + if (!S_ISSOCK(fd_stat.st_mode)) + return false; + + /* Is it a TCP socket? */ + rc = getsockopt(fd->fd, SOL_SOCKET, SO_PROTOCOL, &so_protocol, &so_protocol_len); + if (rc < 0) + return false; + if (so_protocol == IPPROTO_TCP) + return true; + + return false; +} + +/*! Register an osmo_fd for TCP stats monitoring. + * \param[in] fd osmocom file descriptor to be registered. + * \param[in] human readbla name that is used as prefix for the related stats item. + * \returns 0 on success; negative in case of error. */ +int osmo_stats_tcp_osmo_fd_register(const struct osmo_fd *fd, const char *name) +{ + struct stats_tcp_entry *stats_tcp_entry; + + /* Only TCP sockets can be registered for monitoring, anything else will fall through. */ + if (!is_tcp(fd)) + return -EINVAL; + + /* When the osmo_fd is registered and unregistered properly there shouldn't be any leftovers from already closed + * osmo_fds in the stats_tcp list. But lets proactively make sure that any leftovers are cleaned up. */ + osmo_stats_tcp_osmo_fd_unregister(fd); + + /* Make a new list object, attach the osmo_fd... */ + stats_tcp_entry = talloc_zero(OTC_GLOBAL, struct stats_tcp_entry); + OSMO_ASSERT(stats_tcp_entry); + stats_tcp_entry->fd = fd; + stats_tcp_entry->name = talloc_strdup(stats_tcp_entry, name); + + pthread_mutex_lock(&stats_tcp_lock); + llist_add_tail(&stats_tcp_entry->entry, &stats_tcp); + pthread_mutex_unlock(&stats_tcp_lock); + + return 0; +} + +static void next_stats_tcp_entry(void) +{ + struct stats_tcp_entry *last; + + if (llist_empty(&stats_tcp)) { + stats_tcp_entry_cur = NULL; + return; + } + + last = (struct stats_tcp_entry *)llist_last_entry(&stats_tcp, struct stats_tcp_entry, entry); + + if (!stats_tcp_entry_cur || stats_tcp_entry_cur == last) + stats_tcp_entry_cur = + (struct stats_tcp_entry *)llist_first_entry(&stats_tcp, struct stats_tcp_entry, entry); + else + stats_tcp_entry_cur = + (struct stats_tcp_entry *)llist_entry(stats_tcp_entry_cur->entry.next, struct stats_tcp_entry, + entry); +} + +/*! Register an osmo_fd for TCP stats monitoring. + * \param[in] fd osmocom file descriptor to be unregistered. + * \returns 0 on success; negative in case of error. */ +int osmo_stats_tcp_osmo_fd_unregister(const struct osmo_fd *fd) +{ + struct stats_tcp_entry *stats_tcp_entry; + int rc = -EINVAL; + + pthread_mutex_lock(&stats_tcp_lock); + llist_for_each_entry(stats_tcp_entry, &stats_tcp, entry) { + if (fd->fd == stats_tcp_entry->fd->fd) { + /* In case we want to remove exactly that item which is also + * selected as the current item, we must designate either a + * different item or invalidate the current item. + */ + if (stats_tcp_entry == stats_tcp_entry_cur) { + if (llist_count(&stats_tcp) > 2) + next_stats_tcp_entry(); + else + stats_tcp_entry_cur = NULL; + } + + /* Date item from list */ + llist_del(&stats_tcp_entry->entry); + osmo_stat_item_group_free(stats_tcp_entry->stats_tcp); + talloc_free(stats_tcp_entry); + rc = 0; + break; + } + } + pthread_mutex_unlock(&stats_tcp_lock); + + return rc; +} + +static void stats_tcp_poll_timer_cb(void *data) +{ + int i; + int batch_size; + int llist_size; + + pthread_mutex_lock(&stats_tcp_lock); + + /* Make sure we do not run over the same sockets multiple times if the + * configured llist_size is larger then the actual list */ + batch_size = osmo_tcp_stats_config->batch_size; + llist_size = llist_count(&stats_tcp); + if (llist_size < batch_size) + batch_size = llist_size; + + /* Process a batch of sockets */ + for (i = 0; i < batch_size; i++) { + next_stats_tcp_entry(); + if (stats_tcp_entry_cur) + fill_stats(stats_tcp_entry_cur); + } + + pthread_mutex_unlock(&stats_tcp_lock); + + if (osmo_tcp_stats_config->interval > 0) + osmo_timer_schedule(&stats_tcp_poll_timer, osmo_tcp_stats_config->interval, 0); +} + +/*! Set the polling interval (common for all sockets) + * \param[in] interval Poll interval in seconds + * \returns 0 on success; negative on error */ +int osmo_stats_tcp_set_interval(int interval) +{ + osmo_tcp_stats_config->interval = interval; + if (osmo_tcp_stats_config->interval > 0) + osmo_timer_schedule(&stats_tcp_poll_timer, osmo_tcp_stats_config->interval, 0); + return 0; +} + +static __attribute__((constructor)) +void on_dso_load_stats_tcp(void) +{ + stats_tcp_entry_cur = NULL; + pthread_mutex_init(&stats_tcp_lock, NULL); + + osmo_tcp_stats_config->interval = TCP_STATS_DEFAULT_INTERVAL; + osmo_tcp_stats_config->batch_size = TCP_STATS_DEFAULT_BATCH_SIZE; + + osmo_timer_setup(&stats_tcp_poll_timer, stats_tcp_poll_timer_cb, NULL); +} + +#endif /* !EMBEDDED */ + +/* @} */ diff --git a/src/strrb.c b/src/core/strrb.c index 461fdecc..c5a5ed6e 100644 --- a/src/strrb.c +++ b/src/core/strrb.c @@ -31,10 +31,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup utils @@ -56,12 +52,12 @@ * This function creates and initializes a ringbuffer. * Note that the ringbuffer stores at most rb_size - 1 messages. */ -struct osmo_strrb *osmo_strrb_create(TALLOC_CTX * ctx, size_t rb_size) +struct osmo_strrb *osmo_strrb_create(void *talloc_ctx, size_t rb_size) { struct osmo_strrb *rb = NULL; unsigned int i; - rb = talloc_zero(ctx, struct osmo_strrb); + rb = talloc_zero(talloc_ctx, struct osmo_strrb); if (!rb) goto alloc_error; diff --git a/src/tdef.c b/src/core/tdef.c index 71a33158..f0c0f2e8 100644 --- a/src/tdef.c +++ b/src/core/tdef.c @@ -52,18 +52,18 @@ * By keeping separate osmo_tdef arrays, several groups of timers can be kept * separately. The VTY tests in tests/tdef/ showcase different schemes: * - * - \ref tests/vty/tdef_vty_test_config_root.c: + * - \ref tests/tdef/tdef_vty_config_root_test.c: * Keep several timer definitions in separately named groups: showcase the * osmo_tdef_vty_groups*() API. Each timer group exists exactly once. * - * - \ref tests/vty/tdef_vty_test_config_subnode.c: + * - \ref tests/tdef/tdef_vty_config_subnode_test.c: * Keep a single list of timers without separate grouping. * Put this list on a specific subnode below the CONFIG_NODE. * There could be several separate subnodes with timers like this, i.e. * continuing from this example, sets of timers could be separated by placing * timers in specific config subnodes instead of using the global group name. * - * - \ref tests/vty/tdef_vty_test_dynamic.c: + * - \ref tests/tdef/tdef_vty_dynamic_test.c: * Dynamically allocate timer definitions per each new created object. * Thus there can be an arbitrary number of independent timer definitions, one * per allocated object. @@ -93,6 +93,17 @@ static unsigned long osmo_tdef_factor(enum osmo_tdef_unit a, enum osmo_tdef_unit return 1; switch (b) { + case OSMO_TDEF_US: + switch (a) { + case OSMO_TDEF_MS: + return 1000; + case OSMO_TDEF_S: + return 1000*1000; + case OSMO_TDEF_M: + return 60*1000*1000; + default: + return 0; + } case OSMO_TDEF_MS: switch (a) { case OSMO_TDEF_S: @@ -189,8 +200,10 @@ void osmo_tdefs_reset(struct osmo_tdef *tdefs) * \param[in] tdefs Array of timer definitions, last entry must be fully zero initialized. * \param[in] T Timer number to get the value for. * \param[in] as_unit Return timeout value in this unit. - * \param[in] val_if_not_present Fallback value to return if no timeout is defined. + * \param[in] val_if_not_present Fallback value to return if no timeout is defined; if this is a negative number, a + * missing T timer definition aborts the program via OSMO_ASSERT(). * \return Timeout value in the unit given by as_unit, rounded up if necessary, or val_if_not_present. + * If val_if_not_present is negative and no T timer is defined, trigger OSMO_ASSERT() and do not return. */ unsigned long osmo_tdef_get(const struct osmo_tdef *tdefs, int T, enum osmo_tdef_unit as_unit, long val_if_not_present) { @@ -320,30 +333,41 @@ const struct osmo_tdef_state_timeout *osmo_tdef_get_state_timeout(uint32_t state */ int _osmo_tdef_fsm_inst_state_chg(struct osmo_fsm_inst *fi, uint32_t state, const struct osmo_tdef_state_timeout *timeouts_array, - const struct osmo_tdef *tdefs, unsigned long default_timeout, + const struct osmo_tdef *tdefs, long default_timeout, const char *file, int line) { const struct osmo_tdef_state_timeout *t = osmo_tdef_get_state_timeout(state, timeouts_array); - unsigned long val = 0; + unsigned long val_ms = 0; /* No timeout defined for this state? */ if (!t) return _osmo_fsm_inst_state_chg(fi, state, 0, 0, file, line); - if (t->T) - val = osmo_tdef_get(tdefs, t->T, OSMO_TDEF_S, default_timeout); + if (t->T) { + const struct osmo_tdef *tdef = osmo_tdef_get_entry((struct osmo_tdef *)tdefs, t->T); + if (tdef == NULL) { + /* emulate the old behavior: treat default_timeout as OSMO_TDEF_S */ + OSMO_ASSERT(default_timeout >= 0); + val_ms = default_timeout * 1000; + } else { + val_ms = osmo_tdef_round(tdef->val, tdef->unit, OSMO_TDEF_MS); + /* emulate the old behavior: treat OSMO_TDEF_CUSTOM as OSMO_TDEF_S */ + if (tdef->unit == OSMO_TDEF_CUSTOM) + val_ms *= 1000; + } + } if (t->keep_timer) { if (t->T) - return _osmo_fsm_inst_state_chg_keep_or_start_timer(fi, state, val, t->T, file, line); + return _osmo_fsm_inst_state_chg_keep_or_start_timer_ms(fi, state, val_ms, t->T, file, line); else return _osmo_fsm_inst_state_chg_keep_timer(fi, state, file, line); } - /* val is always initialized here, because if t->keep_timer is false, t->T must be != 0. + /* val_ms is always initialized here, because if t->keep_timer is false, t->T must be != 0. * Otherwise osmo_tdef_get_state_timeout() would have returned NULL. */ OSMO_ASSERT(t->T); - return _osmo_fsm_inst_state_chg(fi, state, val, t->T, file, line); + return _osmo_fsm_inst_state_chg_ms(fi, state, val_ms, t->T, file, line); } const struct value_string osmo_tdef_unit_names[] = { @@ -351,6 +375,7 @@ const struct value_string osmo_tdef_unit_names[] = { { OSMO_TDEF_MS, "ms" }, { OSMO_TDEF_M, "m" }, { OSMO_TDEF_CUSTOM, "custom-unit" }, + { OSMO_TDEF_US, "us" }, {} }; diff --git a/src/core/thread.c b/src/core/thread.c new file mode 100644 index 00000000..d9a98422 --- /dev/null +++ b/src/core/thread.c @@ -0,0 +1,56 @@ +/* + * (C) 2021 by sysmocom s.f.m.c. GmbH <info@sysmocom.de> + * All Rights Reserved + * + * Author: Pau Espin Pedrol <pespin@sysmocom.de> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +/*! \addtogroup thread + * @{ + * \file thread.c + */ + +/*! \file thread.c + */ + +#include "config.h" + +/* If HAVE_GETTID, then "_GNU_SOURCE" may need to be defined to use gettid() */ +#if HAVE_GETTID +#define _GNU_SOURCE +#endif +#include <unistd.h> +#include <sys/types.h> + +#include <osmocom/core/thread.h> + +/*! Wrapper around Linux's gettid() to make it easily accessible on different system versions. + * If the gettid() API cannot be found, it will use the syscall directly if + * available. If no syscall is found available, then getpid() is called as + * fallback. See 'man 2 gettid' for further and details information. + * \returns This call is always successful and returns returns the thread ID of + * the calling thread (or the process ID of the current process if + * gettid() or its syscall are unavailable in the system). + */ +pid_t osmo_gettid(void) +{ +#if HAVE_GETTID + return gettid(); +#elif defined(LINUX) && defined(__NR_gettid) + return (pid_t) syscall(__NR_gettid); +#else + #pragma message ("use pid as tid") + return getpid(); +#endif +} diff --git a/src/core/time_cc.c b/src/core/time_cc.c new file mode 100644 index 00000000..0e6879e5 --- /dev/null +++ b/src/core/time_cc.c @@ -0,0 +1,228 @@ +/*! \file foo.c + * Report the cumulative counter of time for which a flag is true as rate counter. + */ +/* Copyright (C) 2021 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * All Rights Reserved + * + * Author: Neels Hofmeyr <nhofmeyr@sysmocom.de> + * + * 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 <http://www.gnu.org/licenses/>. + * + */ + +/*! \addtogroup time_cc + * + * Report the cumulative counter of time for which a flag is true as rate counter. + * + * Useful for reporting cumulative time counters as defined in 3GPP TS 52.402, for example allAvailableSDCCHAllocated, + * allAvailableTCHAllocated, availablePDCHAllocatedTime. + * + * For a usage example, see the description of struct osmo_time_cc. + * + * @{ + * \file time_cc.c + */ +#include "config.h" +#ifdef HAVE_CLOCK_GETTIME + +#include <limits.h> +#include <time.h> + +#include <osmocom/core/tdef.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/time_cc.h> + +#define GRAN_USEC(TIME_CC) ((TIME_CC)->cfg.gran_usec ? : 1000000) +#define ROUND_THRESHOLD_USEC(TIME_CC) ((TIME_CC)->cfg.round_threshold_usec ? \ + OSMO_MIN((TIME_CC)->cfg.round_threshold_usec, GRAN_USEC(TIME_CC)) \ + : (GRAN_USEC(TIME_CC) / 2)) + +static uint64_t time_now_usec(void) +{ + struct timespec tp; + if (osmo_clock_gettime(CLOCK_MONOTONIC, &tp)) + return 0; + return (uint64_t)tp.tv_sec * 1000000 + tp.tv_nsec / 1000; +} + +static void osmo_time_cc_forget_sum(struct osmo_time_cc *tc, uint64_t now); + +static void osmo_time_cc_update_from_tdef(struct osmo_time_cc *tc, uint64_t now) +{ + bool do_forget_sum = false; + if (!tc->cfg.T_defs) + return; + if (tc->cfg.T_gran) { + uint64_t was = GRAN_USEC(tc); + tc->cfg.gran_usec = osmo_tdef_get(tc->cfg.T_defs, tc->cfg.T_gran, OSMO_TDEF_US, -1); + if (was != GRAN_USEC(tc)) + do_forget_sum = true; + } + if (tc->cfg.T_round_threshold) + tc->cfg.round_threshold_usec = osmo_tdef_get(tc->cfg.T_defs, tc->cfg.T_round_threshold, + OSMO_TDEF_US, -1); + if (tc->cfg.T_forget_sum) { + uint64_t was = tc->cfg.forget_sum_usec; + tc->cfg.forget_sum_usec = osmo_tdef_get(tc->cfg.T_defs, tc->cfg.T_forget_sum, OSMO_TDEF_US, -1); + if (tc->cfg.forget_sum_usec && was != tc->cfg.forget_sum_usec) + do_forget_sum = true; + } + + if (do_forget_sum && tc->sum) + osmo_time_cc_forget_sum(tc, now); +} + +static void osmo_time_cc_schedule_timer(struct osmo_time_cc *tc, uint64_t now); + +/*! Clear out osmo_timer and internal counting state of struct osmo_time_cc. The .cfg remains unaffected. After calling, + * the osmo_time_cc instance can be used again to accumulate state as if it had just been initialized. */ +void osmo_time_cc_cleanup(struct osmo_time_cc *tc) +{ + osmo_timer_del(&tc->timer); + *tc = (struct osmo_time_cc){ + .cfg = tc->cfg, + }; +} + +static void osmo_time_cc_start(struct osmo_time_cc *tc, uint64_t now) +{ + osmo_time_cc_cleanup(tc); + tc->start_time = now; + tc->last_counted_time = now; + osmo_time_cc_update_from_tdef(tc, now); + osmo_time_cc_schedule_timer(tc, now); +} + +static void osmo_time_cc_count_time(struct osmo_time_cc *tc, uint64_t now) +{ + uint64_t time_delta = now - tc->last_counted_time; + tc->last_counted_time = now; + if (!tc->flag_state) + return; + /* Flag is currently true, cumulate the elapsed time */ + tc->total_sum += time_delta; + tc->sum += time_delta; +} + +static void osmo_time_cc_report(struct osmo_time_cc *tc, uint64_t now) +{ + uint64_t delta; + uint64_t n; + /* We report a sum "rounded up", ahead of time. If the granularity period has not yet elapsed after the last + * reporting, do not report again yet. */ + if (tc->reported_sum > tc->sum) + return; + delta = tc->sum - tc->reported_sum; + /* elapsed full periods */ + n = delta / GRAN_USEC(tc); + /* If the delta has passed round_threshold (normally half of gran_usec), increment. */ + delta -= n * GRAN_USEC(tc); + if (delta >= ROUND_THRESHOLD_USEC(tc)) + n++; + if (!n) + return; + + /* integer sanity, since rate_ctr_add() takes an int argument. */ + if (n > INT_MAX) + n = INT_MAX; + if (tc->cfg.rate_ctr) + rate_ctr_add(tc->cfg.rate_ctr, n); + /* Store the increments of gran_usec that were counted. */ + tc->reported_sum += n * GRAN_USEC(tc); +} + +static void osmo_time_cc_forget_sum(struct osmo_time_cc *tc, uint64_t now) +{ + tc->reported_sum = 0; + tc->sum = 0; + + if (tc->last_counted_time < now) + tc->last_counted_time = now; +} + +/*! Initialize struct osmo_time_cc. Call this once before use, and before setting up the .cfg items. */ +void osmo_time_cc_init(struct osmo_time_cc *tc) +{ + *tc = (struct osmo_time_cc){0}; +} + +/*! Report state to be recorded by osmo_time_cc instance. Setting an unchanged state repeatedly has no effect. */ +void osmo_time_cc_set_flag(struct osmo_time_cc *tc, bool flag) +{ + uint64_t now = time_now_usec(); + if (!tc->start_time) + osmo_time_cc_start(tc, now); + /* No flag change == no effect */ + if (flag == tc->flag_state) + return; + /* Sum up elapsed time, report increments for that. */ + osmo_time_cc_count_time(tc, now); + osmo_time_cc_report(tc, now); + tc->flag_state = flag; + osmo_time_cc_schedule_timer(tc, now); +} + +static void osmo_time_cc_timer_cb(void *data) +{ + struct osmo_time_cc *tc = data; + uint64_t now = time_now_usec(); + + osmo_time_cc_update_from_tdef(tc, now); + + if (tc->flag_state) { + osmo_time_cc_count_time(tc, now); + osmo_time_cc_report(tc, now); + } else if (tc->cfg.forget_sum_usec && tc->sum + && (now >= tc->last_counted_time + tc->cfg.forget_sum_usec)) { + osmo_time_cc_forget_sum(tc, now); + } + osmo_time_cc_schedule_timer(tc, now); +} + +/*! Figure out the next time we should do anything, if the flag state remains unchanged. */ +static void osmo_time_cc_schedule_timer(struct osmo_time_cc *tc, uint64_t now) +{ + uint64_t next_event = UINT64_MAX; + + osmo_time_cc_update_from_tdef(tc, now); + + /* If it is required, when will the next forget_sum happen? */ + if (tc->cfg.forget_sum_usec && !tc->flag_state && tc->sum > 0) { + uint64_t next_forget_time = tc->last_counted_time + tc->cfg.forget_sum_usec; + next_event = OSMO_MIN(next_event, next_forget_time); + } + /* Next rate_ctr increment? */ + if (tc->flag_state) { + uint64_t next_inc = now + (tc->reported_sum - tc->sum) + ROUND_THRESHOLD_USEC(tc); + next_event = OSMO_MIN(next_event, next_inc); + } + + /* No event coming up? */ + if (next_event == UINT64_MAX) + return; + + if (next_event <= now) + next_event = 0; + else + next_event -= now; + + osmo_timer_setup(&tc->timer, osmo_time_cc_timer_cb, tc); + osmo_timer_del(&tc->timer); + osmo_timer_schedule(&tc->timer, next_event / 1000000, next_event % 1000000); +} + +#endif /* HAVE_CLOCK_GETTIME */ + +/*! @} */ diff --git a/src/timer.c b/src/core/timer.c index 0b2e3dd3..067bd875 100644 --- a/src/timer.c +++ b/src/core/timer.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -40,10 +36,10 @@ #include <osmocom/core/linuxlist.h> /* These store the amount of time that we wait until next timer expires. */ -static struct timeval nearest; -static struct timeval *nearest_p; +static __thread struct timeval nearest; +static __thread struct timeval *nearest_p; -static struct rb_root timer_root = RB_ROOT; +static __thread struct rb_root timer_root = RB_ROOT; static void __add_timer(struct osmo_timer_list *timer) { @@ -135,7 +131,7 @@ void osmo_timer_del(struct osmo_timer_list *timer) * This function can be used to determine whether a given timer * has alredy expired (returns 0) or is still pending (returns 1) */ -int osmo_timer_pending(struct osmo_timer_list *timer) +int osmo_timer_pending(const struct osmo_timer_list *timer) { return timer->active; } @@ -184,6 +180,31 @@ struct timeval *osmo_timers_nearest(void) return nearest_p; } +/*! Determine time between now and the nearest timer in milliseconds + * \returns number of milliseconds until nearest timer expires; -1 if no timers pending + */ +int osmo_timers_nearest_ms(void) +{ + int nearest_ms; + + if (!nearest_p) + return -1; + + nearest_ms = nearest_p->tv_sec * 1000; +#ifndef EMBEDDED + /* By adding 999 milliseconds, we ensure rounding up to the nearest + * whole millisecond. This approach prevents the return of 0 when the + * timer is still active, and it guarantees that the calling process + * does not wait for a duration shorter than the time remaining on the + * timer. */ + nearest_ms += (nearest_p->tv_usec + 999) / 1000; +#else + nearest_ms += nearest_p->tv_usec / 1000; +#endif + + return nearest_ms; +} + static void update_nearest(struct timeval *cand, struct timeval *current) { if (cand->tv_sec != LONG_MAX) { diff --git a/src/timer_clockgettime.c b/src/core/timer_clockgettime.c index 7b17fd11..6112b8a5 100644 --- a/src/timer_clockgettime.c +++ b/src/core/timer_clockgettime.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup timer diff --git a/src/timer_gettimeofday.c b/src/core/timer_gettimeofday.c index 34949465..e0212b5a 100644 --- a/src/timer_gettimeofday.c +++ b/src/core/timer_gettimeofday.c @@ -15,10 +15,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup timer diff --git a/src/core/tun.c b/src/core/tun.c new file mode 100644 index 00000000..86128190 --- /dev/null +++ b/src/core/tun.c @@ -0,0 +1,577 @@ + +/* TUN interface functions. + * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Pau Espin Pedrol <pespin@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include "config.h" + +/*! \addtogroup tun + * @{ + * tun network device (interface) convenience functions + * + * \file tundev.c + * + * Example lifecycle use of the API: + * + * struct osmo_sockaddr_str osa_str = {}; + * struct osmo_sockaddr osa = {}; + * + * // Allocate object: + * struct osmo_tundev *tundev = osmo_tundev_alloc(parent_talloc_ctx, name); + * OSMO_ASSERT(tundev); + * + * // Configure object (before opening): + * osmo_tundev_set_data_ind_cb(tun, tun_data_ind_cb); + * rc = osmo_tundev_set_dev_name(tun, "mytunnel0"); + * rc = osmo_tundev_set_netns_name(tun, "some_netns_name_or_null"); + * + * // Open the tundev object: + * rc = osmo_tundev_open(tundev); + * // The tunnel device is now created and an associatd netdev object + * // is available to operate the device: + * struct osmo_netdev *netdev = osmo_tundev_get_netdev(tundev); + * OSMO_ASSERT(netdev); + * + * // Add a local IPv4 address: + * rc = osmo_sockaddr_str_from_str2(&osa_str, "192.168.200.1"); + * rc = osmo_sockaddr_str_to_sockaddr(&osa_str, &osa.u.sas); + * rc = osmo_netdev_add_addr(netdev, &osa, 24); + * + * // Bring network interface up: + * rc = osmo_netdev_ifupdown(netdev, true); + * + * // Add default route (0.0.0.0/0): + * rc = osmo_sockaddr_str_from_str2(&osa_str, "0.0.0.0"); + * rc = osmo_sockaddr_str_to_sockaddr(&osa_str, &osa.u.sas); + * rc = osmo_netdev_add_route(netdev, &osa, 0, NULL); + * + * // Close the tunnel (asssociated netdev object becomes unavailable) + * rc = osmo_tundev_close(tundev); + * // Free the object: + * osmo_tundev_free(tundev); + */ + +#if (!EMBEDDED) + +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <errno.h> +#include <ifaddrs.h> +#include <sys/types.h> +#include <sys/time.h> +#include <sys/ioctl.h> +#include <sys/socket.h> +#include <sys/stat.h> +#include <sys/time.h> +#include <net/if.h> + +#if defined(__linux__) +#include <linux/if_tun.h> +#else +#error "Unknown platform!" +#endif + +#include <osmocom/core/utils.h> +#include <osmocom/core/select.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/write_queue.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/netns.h> +#include <osmocom/core/netdev.h> +#include <osmocom/core/tun.h> + +#define TUN_DEV_PATH "/dev/net/tun" +#define TUN_PACKET_MAX 8196 + +#define LOGTUN(tun, lvl, fmt, args ...) \ + LOGP(DLGLOBAL, lvl, "TUN(%s,if=%s/%u,ns=%s): " fmt, \ + (tun)->name, (tun)->dev_name ? : "", \ + (tun)->ifindex, (tun)->netns_name ? : "", ## args) + +struct osmo_tundev { + /* Name used to identify the osmo_tundev */ + char *name; + + /* netdev managing the tun interface: */ + struct osmo_netdev *netdev; + + /* ifindiex of the currently opened tunnel interface */ + unsigned int ifindex; + + /* Network interface name to use when setting up the tun device. + * NULL = let the system pick one. */ + char *dev_name; + /* Whether dev_name is set by user or dynamically allocated by system */ + bool dev_name_dynamic; + + /* Write queue used since tun fd is set non-blocking */ + struct osmo_wqueue wqueue; + + /* netns name where the tun interface is created (NULL = default netns) */ + char *netns_name; + + /* API user private data */ + void *priv_data; + + /* Called by tundev each time a new packet is received on the tun interface. Can be NULL. */ + osmo_tundev_data_ind_cb_t data_ind_cb; + + /* Whether the tundev is in opened state (managing the tun interface) */ + bool opened; +}; + +/* A new pkt arrived from the tun device, dispatch it to the API user */ +static int tundev_decaps(struct osmo_tundev *tundev) +{ + struct msgb *msg; + int rc; + + msg = msgb_alloc(TUN_PACKET_MAX, "tundev_rx"); + + if ((rc = read(tundev->wqueue.bfd.fd, msgb_data(msg), TUN_PACKET_MAX)) <= 0) { + LOGTUN(tundev, LOGL_ERROR, "read() failed: %s (%d)\n", strerror(errno), errno); + msgb_free(msg); + return -1; + } + msgb_put(msg, rc); + + if (tundev->data_ind_cb) + return tundev->data_ind_cb(tundev, msg); + + msgb_free(msg); + return 0; +} + +/* callback for tun device osmocom select loop integration */ +static int tundev_read_cb(struct osmo_fd *fd) +{ + struct osmo_tundev *tundev = fd->data; + return tundev_decaps(tundev); +} + +/* callback for tun device osmocom select loop integration */ +static int tundev_write_cb(struct osmo_fd *fd, struct msgb *msg) +{ + struct osmo_tundev *tundev = fd->data; + size_t pkt_len = msgb_length(msg); + + int rc; + rc = write(tundev->wqueue.bfd.fd, msgb_data(msg), pkt_len); + if (rc < 0) + LOGTUN(tundev, LOGL_ERROR, "write() failed: %s (%d)\n", strerror(errno), errno); + else if (rc < pkt_len) + LOGTUN(tundev, LOGL_ERROR, "short write() %d < %zu\n", rc, pkt_len); + return rc; +} + +static int tundev_ifupdown_ind_cb(struct osmo_netdev *netdev, bool ifupdown) +{ + struct osmo_tundev *tundev = osmo_netdev_get_priv_data(netdev); + LOGTUN(tundev, LOGL_NOTICE, "Physical link state changed: %s\n", + ifupdown ? "UP" : "DOWN"); + + /* free any backlog, both on IFUP and IFDOWN. Keep the LMI, as it makes + * sense to get one out of the door ASAP. */ + osmo_wqueue_clear(&tundev->wqueue); + return 0; +} + +static int tundev_dev_name_chg_cb(struct osmo_netdev *netdev, const char *new_dev_name) +{ + struct osmo_tundev *tundev = osmo_netdev_get_priv_data(netdev); + LOGTUN(tundev, LOGL_NOTICE, "netdev changed name: %s -> %s\n", + osmo_netdev_get_dev_name(netdev), new_dev_name); + + if (tundev->dev_name_dynamic) { + osmo_talloc_replace_string(tundev, &tundev->dev_name, new_dev_name); + } else { + /* TODO: in here we probably want to force the iface name back + * to tundev->dev_name one we have a osmo_netdev_set_ifname() API */ + osmo_talloc_replace_string(tundev, &tundev->dev_name, new_dev_name); + } + + return 0; +} + +static int tundev_mtu_chg_cb(struct osmo_netdev *netdev, uint32_t new_mtu) +{ + struct osmo_tundev *tundev = osmo_netdev_get_priv_data(netdev); + LOGTUN(tundev, LOGL_NOTICE, "netdev changed MTU: %u\n", new_mtu); + + return 0; +} + +/*! Allocate a new tundev object. + * \param[in] ctx talloc context to use as a parent when allocating the tundev object + * \param[in] name A name providen to identify the tundev object + * \returns newly allocated tundev object on success; NULL on error + */ +struct osmo_tundev *osmo_tundev_alloc(void *ctx, const char *name) +{ + struct osmo_tundev *tundev; + + tundev = talloc_zero(ctx, struct osmo_tundev); + if (!tundev) + return NULL; + + tundev->netdev = osmo_netdev_alloc(tundev, name); + if (!tundev->netdev) { + talloc_free(tundev); + return NULL; + } + osmo_netdev_set_priv_data(tundev->netdev, tundev); + osmo_netdev_set_ifupdown_ind_cb(tundev->netdev, tundev_ifupdown_ind_cb); + osmo_netdev_set_dev_name_chg_cb(tundev->netdev, tundev_dev_name_chg_cb); + osmo_netdev_set_mtu_chg_cb(tundev->netdev, tundev_mtu_chg_cb); + + tundev->name = talloc_strdup(tundev, name); + osmo_wqueue_init(&tundev->wqueue, 1000); + osmo_fd_setup(&tundev->wqueue.bfd, -1, OSMO_FD_READ, osmo_wqueue_bfd_cb, tundev, 0); + tundev->wqueue.read_cb = tundev_read_cb; + tundev->wqueue.write_cb = tundev_write_cb; + + return tundev; +} + +/*! Free an allocated tundev object. + * \param[in] tundev The tundev object to free + */ +void osmo_tundev_free(struct osmo_tundev *tundev) +{ + if (!tundev) + return; + osmo_tundev_close(tundev); + osmo_netdev_free(tundev->netdev); + talloc_free(tundev); +} + +/*! Open and configure fd of the tunnel device. + * \param[in] tundev The tundev object whose tunnel interface to open + * \param[in] flags internal linux flags to pass when creating the device (not used yet) + * \returns 0 on success; negative on error + */ +static int tundev_open_fd(struct osmo_tundev *tundev, int flags) +{ + struct ifreq ifr; + int fd, rc; + + fd = open(TUN_DEV_PATH, O_RDWR); + if (fd < 0) { + LOGTUN(tundev, LOGL_ERROR, "Cannot open " TUN_DEV_PATH ": %s\n", strerror(errno)); + return fd; + } + + memset(&ifr, 0, sizeof(ifr)); + ifr.ifr_flags = IFF_TUN | IFF_NO_PI | flags; + if (tundev->dev_name) { + /* if a TUN interface name was specified, put it in the structure; otherwise, + the kernel will try to allocate the "next" device of the specified type */ + osmo_strlcpy(ifr.ifr_name, tundev->dev_name, IFNAMSIZ); + } + + /* try to create the device */ + rc = ioctl(fd, TUNSETIFF, (void *) &ifr); + if (rc < 0) + goto close_ret; + + /* Read name back from device */ + if (!tundev->dev_name) { + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + tundev->dev_name = talloc_strdup(tundev, ifr.ifr_name); + tundev->dev_name_dynamic = true; + } + + /* Store interface index: + * (Note: there's a potential race condition here between creating the + * iface with a given name above and attempting to retrieve its ifindex based + * on that name. Someone (ie udev) could have the iface renamed in + * between here. It's a pity that TUNSETIFF doesn't copy back to us the + * newly allocated ifinidex as it does with ifname) + */ + tundev->ifindex = if_nametoindex(tundev->dev_name); + if (tundev->ifindex == 0) { + LOGTUN(tundev, LOGL_ERROR, "Unable to find ifinidex for dev %s\n", + tundev->dev_name); + rc = -ENODEV; + goto close_ret; + } + + LOGTUN(tundev, LOGL_INFO, "TUN device created\n"); + + /* set non-blocking: */ + rc = fcntl(fd, F_GETFL); + if (rc < 0) { + LOGTUN(tundev, LOGL_ERROR, "fcntl(F_GETFL) failed: %s (%d)\n", + strerror(errno), errno); + goto close_ret; + } + rc = fcntl(fd, F_SETFL, rc | O_NONBLOCK); + if (rc < 0) { + LOGTUN(tundev, LOGL_ERROR, "fcntl(F_SETFL, O_NONBLOCK) failed: %s (%d)\n", + strerror(errno), errno); + goto close_ret; + } + return fd; + +close_ret: + close(fd); + return rc; +} + +/*! Open the tunnel device owned by the tundev object. + * \param[in] tundev The tundev object to open + * \returns 0 on success; negative on error + */ +int osmo_tundev_open(struct osmo_tundev *tundev) +{ + struct osmo_netns_switch_state switch_state; + int rc; + int netns_fd = -1; + + if (tundev->opened) + return -EALREADY; + + /* temporarily switch to specified namespace to create tun device */ + if (tundev->netns_name) { + LOGTUN(tundev, LOGL_INFO, "Open tun: Switch to netns '%s'\n", + tundev->netns_name); + netns_fd = osmo_netns_open_fd(tundev->netns_name); + if (netns_fd < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "Open tun: Cannot switch to netns '%s': %s (%d)\n", + tundev->netns_name, strerror(errno), errno); + return netns_fd; + } + rc = osmo_netns_switch_enter(netns_fd, &switch_state); + if (rc < 0) { + LOGTUN(tundev, LOGL_ERROR, "Open tun: Cannot switch to netns '%s': %s (%d)\n", + tundev->netns_name, strerror(errno), errno); + goto err_close_netns_fd; + } + } + + tundev->wqueue.bfd.fd = tundev_open_fd(tundev, 0); + if (tundev->wqueue.bfd.fd < 0) { + LOGTUN(tundev, LOGL_ERROR, "Cannot open TUN device: %s\n", strerror(errno)); + rc = -ENODEV; + goto err_restore_ns; + } + + /* switch back to default namespace */ + if (tundev->netns_name) { + rc = osmo_netns_switch_exit(&switch_state); + if (rc < 0) { + LOGTUN(tundev, LOGL_ERROR, "Open tun: Cannot switch back from netns '%s': %s\n", + tundev->netns_name, strerror(errno)); + goto err_close_tun; + } + LOGTUN(tundev, LOGL_INFO, "Open tun: Back from netns '%s'\n", + tundev->netns_name); + } + + rc = osmo_netdev_set_netns_name(tundev->netdev, tundev->netns_name); + if (rc < 0) + goto err_close_tun; + rc = osmo_netdev_set_ifindex(tundev->netdev, tundev->ifindex); + if (rc < 0) + goto err_close_tun; + + rc = osmo_netdev_register(tundev->netdev); + if (rc < 0) + goto err_close_tun; + + rc = osmo_fd_register(&tundev->wqueue.bfd); + if (rc < 0) + goto err_unregister_netdev; + + tundev->opened = true; + return 0; + +err_unregister_netdev: + osmo_netdev_unregister(tundev->netdev); +err_close_tun: + close(tundev->wqueue.bfd.fd); + tundev->wqueue.bfd.fd = -1; +err_restore_ns: + if (tundev->netns_name) + osmo_netns_switch_exit(&switch_state); +err_close_netns_fd: + if (netns_fd >= 0) + close(netns_fd); + return rc; +} + +/*! Close the tunnel device owned by the tundev object. + * \param[in] tundev The tundev object to close + * \returns 0 on success; negative on error + */ +int osmo_tundev_close(struct osmo_tundev *tundev) +{ + if (!tundev->opened) + return -EALREADY; + + osmo_wqueue_clear(&tundev->wqueue); + if (tundev->wqueue.bfd.fd != -1) { + osmo_fd_unregister(&tundev->wqueue.bfd); + close(tundev->wqueue.bfd.fd); + tundev->wqueue.bfd.fd = -1; + } + + osmo_netdev_unregister(tundev->netdev); + if (tundev->dev_name_dynamic) { + TALLOC_FREE(tundev->dev_name); + tundev->dev_name_dynamic = false; + } + tundev->opened = false; + return 0; +} + +/*! Retrieve whether the tundev object is in "opened" state. + * \param[in] tundev The tundev object to check + * \returns true if in state "opened"; false otherwise + */ +bool osmo_tundev_is_open(struct osmo_tundev *tundev) +{ + return tundev->opened; +} + +/*! Set private user data pointer on the tundev object. + * \param[in] tundev The tundev object where the field is set + */ +void osmo_tundev_set_priv_data(struct osmo_tundev *tundev, void *priv_data) +{ + tundev->priv_data = priv_data; +} + +/*! Get private user data pointer from the tundev object. + * \param[in] tundev The tundev object from where to retrieve the field + * \returns The current value of the priv_data field. + */ +void *osmo_tundev_get_priv_data(struct osmo_tundev *tundev) +{ + return tundev->priv_data; +} + +/*! Set data_ind_cb callback, called when a new packet is received on the tun interface. + * \param[in] tundev The tundev object where the field is set + * \param[in] data_ind_cb the user provided function to be called when a new packet is received + */ +void osmo_tundev_set_data_ind_cb(struct osmo_tundev *tundev, osmo_tundev_data_ind_cb_t data_ind_cb) +{ + tundev->data_ind_cb = data_ind_cb; +} + +/*! Get name used to identify the tundev object. + * \param[in] tundev The tundev object from where to retrieve the field + * \returns The current value of the name used to identify the tundev object + */ +const char *osmo_tundev_get_name(const struct osmo_tundev *tundev) +{ + return tundev->name; +} + +/*! Set name used to name the tunnel interface created by the tundev object. + * \param[in] tundev The tundev object where the field is set + * \param[in] dev_name The tunnel interface name to use + * \returns 0 on success; negative on error + * + * This is used during osmo_tundev_open() time, and hence shouldn't be changed + * when the tundev object is in "opened" state. + * If left as NULL (default), the system will pick a suitable name during + * osmo_tundev_open(), and the field will be updated to the system-selected + * name, which can be retrieved later with osmo_tundev_get_dev_name(). + */ +int osmo_tundev_set_dev_name(struct osmo_tundev *tundev, const char *dev_name) +{ + if (tundev->opened) + return -EALREADY; + osmo_talloc_replace_string(tundev, &tundev->dev_name, dev_name); + tundev->dev_name_dynamic = false; + return 0; +} + +/*! Get name used to name the tunnel interface created by the tundev object + * \param[in] tundev The tundev object from where to retrieve the field + * \returns The current value of the configured tunnel interface name to use + */ +const char *osmo_tundev_get_dev_name(const struct osmo_tundev *tundev) +{ + return tundev->dev_name; +} + +/*! Set name of the network namespace to use when opening the tunnel interface + * \param[in] tundev The tundev object where the field is set + * \param[in] netns_name The network namespace to use during tunnel interface creation + * \returns 0 on success; negative on error + * + * This is used during osmo_tundev_open() time, and hence shouldn't be changed + * when the tundev object is in "opened" state. + * If left as NULL (default), the system will stay in the current network namespace. + */ +int osmo_tundev_set_netns_name(struct osmo_tundev *tundev, const char *netns_name) +{ + if (tundev->opened) + return -EALREADY; + osmo_talloc_replace_string(tundev, &tundev->netns_name, netns_name); + return 0; +} + +/*! Get name of network namespace used when opening the tunnel interface + * \param[in] tundev The tundev object from where to retrieve the field + * \returns The current value of the configured network namespace + */ +const char *osmo_tundev_get_netns_name(const struct osmo_tundev *tundev) +{ + return tundev->netns_name; +} + +/*! Get netdev managing the tunnel interface of tundev + * \param[in] tundev The tundev object from where to retrieve the field + * \returns The netdev objet managing the tun interface + */ +struct osmo_netdev *osmo_tundev_get_netdev(struct osmo_tundev *tundev) +{ + return tundev->netdev; +} + +/*! Submit a packet to the tunnel device managed by the tundev object + * \param[in] tundev The tundev object owning the tunnel device where to inject the packet + * \param[in] msg The msgb containg the packet to transfer + * \returns The current value of the configured network namespace + * + * This function takes the ownership of msg in all cases. + */ +int osmo_tundev_send(struct osmo_tundev *tundev, struct msgb *msg) +{ + int rc = osmo_wqueue_enqueue(&tundev->wqueue, msg); + if (rc < 0) { + LOGTUN(tundev, LOGL_ERROR, "Failed to enqueue the packet\n"); + msgb_free(msg); + return rc; + } + return rc; +} + + +#endif /* (!EMBEDDED) */ + +/*! @} */ diff --git a/src/use_count.c b/src/core/use_count.c index 453d2ae8..9714403d 100644 --- a/src/use_count.c +++ b/src/core/use_count.c @@ -19,10 +19,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <errno.h> @@ -107,6 +103,19 @@ int32_t osmo_use_count_by(const struct osmo_use_count *uc, const char *use) */ const char *osmo_use_count_name_buf(char *buf, size_t buf_len, const struct osmo_use_count *uc) { + osmo_use_count_to_str_buf(buf, buf_len, uc); + return buf; +} + +/*! Write a comprehensive listing of use counts to a string buffer. + * Reads like "12 (3*barring,fighting,8*kungfoo)". + * \param[inout] buf Destination buffer. + * \param[in] buf_len sizeof(buf). + * \param[in] uc Use counts to print. + * \return number of bytes that would be written, like snprintf(). + */ +int osmo_use_count_to_str_buf(char *buf, size_t buf_len, const struct osmo_use_count *uc) +{ int32_t count = osmo_use_count_total(uc); struct osmo_strbuf sb = { .buf = buf, .len = buf_len }; struct osmo_use_count_entry *e; @@ -114,6 +123,9 @@ const char *osmo_use_count_name_buf(char *buf, size_t buf_len, const struct osmo OSMO_STRBUF_PRINTF(sb, "%" PRId32 " (", count); + if (!uc->use_counts.next) + goto uninitialized; + first = true; llist_for_each_entry(e, &uc->use_counts, entry) { if (!e->count) @@ -127,8 +139,21 @@ const char *osmo_use_count_name_buf(char *buf, size_t buf_len, const struct osmo } if (first) OSMO_STRBUF_PRINTF(sb, "-"); + +uninitialized: OSMO_STRBUF_PRINTF(sb, ")"); - return buf; + return sb.chars_needed; +} + +/*! Write a comprehensive listing of use counts to a talloc allocated string buffer. + * Reads like "12 (3*barring,fighting,8*kungfoo)". + * \param[in] ctx talloc pool to allocate from. + * \param[in] uc Use counts to print. + * \return buf, always nul-terminated. + */ +char *osmo_use_count_to_str_c(void *ctx, const struct osmo_use_count *uc) +{ + OSMO_NAME_C_IMPL(ctx, 32, "ERROR", osmo_use_count_to_str_buf, uc) } /* Return a use token's use count entry -- probably you want osmo_use_count_by() instead. @@ -210,6 +235,8 @@ int _osmo_use_count_get_put(struct osmo_use_count *uc, const char *use, int32_t { struct osmo_use_count_entry *e; int32_t old_use_count; + if (!uc) + return -EINVAL; if (!change) return 0; diff --git a/src/utils.c b/src/core/utils.c index ea1de0f5..1ec940d0 100644 --- a/src/utils.c +++ b/src/core/utils.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -30,6 +26,7 @@ #include <errno.h> #include <stdio.h> #include <inttypes.h> +#include <limits.h> #include <osmocom/core/utils.h> #include <osmocom/core/bit64gen.h> @@ -150,13 +147,15 @@ uint8_t osmo_char2bcd(char c) */ int osmo_bcd2str(char *dst, size_t dst_size, const uint8_t *bcd, int start_nibble, int end_nibble, bool allow_hex) { - char *dst_end = dst + dst_size - 1; + char *dst_end; int nibble_i; int rc = 0; - if (!dst || dst_size < 1) + if (!dst || dst_size < 1 || start_nibble < 0) return -ENOMEM; + dst_end = dst + dst_size - 1; + for (nibble_i = start_nibble; nibble_i < end_nibble && dst < dst_end; nibble_i++, dst++) { uint8_t nibble = bcd[nibble_i >> 1]; if ((nibble_i & 1)) @@ -175,13 +174,72 @@ int osmo_bcd2str(char *dst, size_t dst_size, const uint8_t *bcd, int start_nibbl return OSMO_MAX(0, end_nibble - start_nibble); } +/*! Convert string to BCD. + * The given nibble offsets are interpreted in BCD order, i.e. nibble 0 is bcd[0] & 0x0f, nibble 1 is bcd[0] & 0xf0, nibble + * 3 is bcd[1] & 0x0f, etc.. + * \param[out] dst Output BCD buffer. + * \param[in] dst_size sizeof() the output string buffer. + * \param[in] digits String containing decimal or hexadecimal digits in upper or lower case. + * \param[in] start_nibble Offset to start from, in nibbles, typically 1 to skip the first (MI type) nibble. + * \param[in] end_nibble Negative to write all digits found in str, followed by 0xf nibbles to fill any started octet. + * If >= 0, stop before this offset in nibbles, e.g. to get default behavior, pass + * start_nibble + strlen(str) + ((start_nibble + strlen(str)) & 1? 1 : 0) + 1. + * \param[in] allow_hex If false, return error if there are hexadecimal digits (A-F). If true, write those to + * BCD. + * \returns The buffer size in octets that is used to place all bcd digits (including the skipped nibbles + * from 'start_nibble' and rounded up to full octets); -EINVAL on invalid digits; + * -ENOMEM if dst is NULL, if dst_size is too small to contain all nibbles, or if start_nibble is negative. + */ +int osmo_str2bcd(uint8_t *dst, size_t dst_size, const char *digits, int start_nibble, int end_nibble, bool allow_hex) +{ + const char *digit = digits; + int nibble_i; + + if (!dst || !dst_size || start_nibble < 0) + return -ENOMEM; + + if (end_nibble < 0) { + end_nibble = start_nibble + strlen(digits); + /* If the last octet is not complete, add another filler nibble */ + if (end_nibble & 1) + end_nibble++; + } + if ((unsigned int) (end_nibble / 2) > dst_size) + return -ENOMEM; + + for (nibble_i = start_nibble; nibble_i < end_nibble; nibble_i++) { + uint8_t nibble = 0xf; + int octet = nibble_i >> 1; + if (*digit) { + char c = *digit; + digit++; + if (c >= '0' && c <= '9') + nibble = c - '0'; + else if (allow_hex && c >= 'A' && c <= 'F') + nibble = 0xa + (c - 'A'); + else if (allow_hex && c >= 'a' && c <= 'f') + nibble = 0xa + (c - 'a'); + else + return -EINVAL; + } + nibble &= 0xf; + if ((nibble_i & 1)) + dst[octet] = (nibble << 4) | (dst[octet] & 0x0f); + else + dst[octet] = (dst[octet] & 0xf0) | nibble; + } + + /* floor(float(end_nibble) / 2) */ + return end_nibble / 2; +} + /*! Parse a string containing hexadecimal digits * \param[in] str string containing ASCII encoded hexadecimal digits * \param[out] b output buffer * \param[in] max_len maximum space in output buffer * \returns number of parsed octets, or -1 on error */ -int osmo_hexparse(const char *str, uint8_t *b, int max_len) +int osmo_hexparse(const char *str, uint8_t *b, unsigned int max_len) { char c; @@ -256,7 +314,7 @@ const char *osmo_hexdump_buf(char *out_buf, size_t out_buf_size, const unsigned for (i = 0; i < len; i++) { const char *delimp = delim; int len_remain = out_buf_size - (cur - out_buf) - 1; - if (len_remain < (2 + delim_len) + if (len_remain < (int) (2 + delim_len) && !(!delim_after_last && i == (len - 1) && len_remain >= 2)) break; @@ -280,11 +338,11 @@ const char *osmo_hexdump_buf(char *out_buf, size_t out_buf_size, const unsigned * \param[out] buf_len size of buf in bytes * \param[in] bits A sequence of unpacked bits * \param[in] len Length of bits - * \returns string representation in static buffer. + * \return The output buffer (buf). */ char *osmo_ubit_dump_buf(char *buf, size_t buf_len, const uint8_t *bits, unsigned int len) { - int i; + unsigned int i; if (len > buf_len-1) len = buf_len-1; @@ -347,9 +405,6 @@ char *osmo_hexdump(const unsigned char *buf, int len) * * This function will print a sequence of bytes as hexadecimal numbers, * adding one space character between each byte (e.g. "1a ef d9") - * - * The maximum size of the output buffer is 4096 bytes, i.e. the maximum - * number of input bytes that can be printed in one call is 1365! */ char *osmo_hexdump_c(const void *ctx, const unsigned char *buf, int len) { @@ -386,9 +441,6 @@ char *osmo_hexdump_nospc(const unsigned char *buf, int len) * * This function will print a sequence of bytes as hexadecimal numbers, * without any space character between each byte (e.g. "1aefd9") - * - * The maximum size of the output buffer is 4096 bytes, i.e. the maximum - * number of input bytes that can be printed in one call is 2048! */ char *osmo_hexdump_nospc_c(const void *ctx, const unsigned char *buf, int len) { @@ -409,7 +461,7 @@ char *osmo_osmo_hexdump_nospc(const unsigned char *buf, int len) __attribute__((weak, alias("osmo_hexdump_nospc"))); #endif -#include "../config.h" +#include "config.h" #ifdef HAVE_CTYPE_H #include <ctype.h> /*! Convert an entire string to lower case @@ -486,7 +538,7 @@ uint64_t osmo_decode_big_endian(const uint8_t *data, size_t data_len) /*! Generic big-endian encoding of big endian number up to 64bit * \param[in] value unsigned integer value to be stored - * \param[in] data_len number of octets + * \param[in] data_len number of octets * \returns static buffer containing big-endian stored value * * This is like osmo_store64be_ext, except that this returns a static buffer of @@ -509,20 +561,44 @@ uint8_t *osmo_encode_big_endian(uint64_t value, size_t data_len) * Copy at most \a siz bytes from \a src to \a dst, ensuring that the result is * NUL terminated. The NUL character is included in \a siz, i.e. passing the * actual sizeof(*dst) is correct. + * + * Note, a similar function that also limits the input buffer size is osmo_print_n(). */ size_t osmo_strlcpy(char *dst, const char *src, size_t siz) { size_t ret = src ? strlen(src) : 0; if (siz) { - size_t len = (ret >= siz) ? siz - 1 : ret; - if (src) + size_t len = OSMO_MIN(siz - 1, ret); + if (len) memcpy(dst, src, len); dst[len] = '\0'; } return ret; } +/*! Find first occurence of a char in a size limited string. + * Like strchr() but with a buffer size limit. + * \param[in] str String buffer to examine. + * \param[in] str_size sizeof(str). + * \param[in] c Character to look for. + * \return Pointer to the matched char, or NULL if not found. + */ +const char *osmo_strnchr(const char *str, size_t str_size, char c) +{ + const char *end = str + str_size; + const char *pos; + if (!str) + return NULL; + for (pos = str; pos < end; pos++) { + if (c == *pos) + return pos; + if (!*pos) + return NULL; + } + return NULL; +} + /*! Validate that a given string is a hex string within given size limits. * Note that each hex digit amounts to a nibble, so if checking for a hex * string to result in N bytes, pass amount of digits as 2*N. @@ -603,7 +679,7 @@ bool osmo_identifier_valid(const char *str) * To guarantee passing osmo_separated_identifiers_valid(), replace_with must not itself be an illegal character. If in * doubt, use '-'. * \param[inout] str Identifier to sanitize, must be nul terminated and in a writable buffer. - * \param[in] sep_chars Additional characters that are allowed besides osmo_identifier_illegal_chars. + * \param[in] sep_chars Additional characters that are to be replaced besides osmo_identifier_illegal_chars. * \param[in] replace_with Replace any illegal characters with this character. */ void osmo_identifier_sanitize_buf(char *str, const char *sep_chars, char replace_with) @@ -668,13 +744,19 @@ int osmo_print_n(char *buf, size_t bufsize, const char *str, size_t n) } /*! Return the string with all non-printable characters escaped. + * This internal function is the implementation for all osmo_escape_str* and osmo_quote_str* API versions. + * It provides both the legacy (non C compatible) escaping, as well as C compatible string constant syntax, + * and it provides a return value of characters-needed, to allow producing un-truncated strings in all cases. * \param[out] buf string buffer to write escaped characters to. * \param[in] bufsize sizeof(buf). * \param[in] str A string that may contain any characters. * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length (also past nul chars). + * \param[in] legacy_format If false, return C compatible string constants ("\x0f"), if true the legacy + * escaping format ("\15"). The legacy format also escapes as "\a\b\f\v", while + * the non-legacy format also escapes those as "\xNN" sequences. * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). */ -char *osmo_escape_str_buf2(char *buf, size_t bufsize, const char *str, int in_len) +static int _osmo_escape_str_buf(char *buf, size_t bufsize, const char *str, int in_len, bool legacy_format) { struct osmo_strbuf sb = { .buf = buf, .len = bufsize }; int in_pos = 0; @@ -713,22 +795,56 @@ char *osmo_escape_str_buf2(char *buf, size_t bufsize, const char *str, int in_le BACKSLASH_CASE('\r', 'r'); BACKSLASH_CASE('\t', 't'); BACKSLASH_CASE('\0', '0'); - BACKSLASH_CASE('\a', 'a'); - BACKSLASH_CASE('\b', 'b'); - BACKSLASH_CASE('\v', 'v'); - BACKSLASH_CASE('\f', 'f'); BACKSLASH_CASE('\\', '\\'); BACKSLASH_CASE('"', '"'); -#undef BACKSLASH_CASE default: - OSMO_STRBUF_PRINTF(sb, "\\%u", (unsigned char)str[in_pos]); + if (legacy_format) { + switch (str[next_unprintable]) { + BACKSLASH_CASE('\a', 'a'); + BACKSLASH_CASE('\b', 'b'); + BACKSLASH_CASE('\v', 'v'); + BACKSLASH_CASE('\f', 'f'); + default: + OSMO_STRBUF_PRINTF(sb, "\\%u", (unsigned char)str[in_pos]); + break; + } + break; + } + + OSMO_STRBUF_PRINTF(sb, "\\x%02x", (unsigned char)str[in_pos]); break; } in_pos ++; +#undef BACKSLASH_CASE } done: + return sb.chars_needed; +} + +/*! Return the string with all non-printable characters escaped. + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length (also past nul chars). + * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). + */ +int osmo_escape_str_buf3(char *buf, size_t bufsize, const char *str, int in_len) +{ + return _osmo_escape_str_buf(buf, bufsize, str, in_len, false); +} + +/*! Return the string with all non-printable characters escaped. + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length (also past nul chars). + * \return The output buffer (buf). + */ +char *osmo_escape_str_buf2(char *buf, size_t bufsize, const char *str, int in_len) +{ + _osmo_escape_str_buf(buf, bufsize, str, in_len, true); return buf; } @@ -750,31 +866,63 @@ const char *osmo_escape_str(const char *str, int in_len) */ char *osmo_escape_str_c(const void *ctx, const char *str, int in_len) { - char *buf = talloc_size(ctx, in_len+1); - if (!buf) - return NULL; - return osmo_escape_str_buf2(buf, in_len+1, str, in_len); + /* The string will be at least as long as in_len, but some characters might need escaping. + * These extra bytes should catch most usual escaping situations, avoiding a second run in OSMO_NAME_C_IMPL. */ + OSMO_NAME_C_IMPL(ctx, in_len + 16, "ERROR", _osmo_escape_str_buf, str, in_len, true); } -/*! Like osmo_escape_str_buf2(), but returns double-quotes around a string, or "NULL" for a NULL string. - * This allows passing any char* value and get its C representation as string. - * The function signature is suitable for OSMO_STRBUF_APPEND_NOLEN(). +/*! Return a quoted and escaped representation of the string. + * This internal function is the implementation for all osmo_quote_str* API versions. + * It provides both the legacy (non C compatible) escaping, as well as C compatible string constant syntax, + * and it provides a return value of characters-needed, to allow producing un-truncated strings in all cases. * \param[out] buf string buffer to write escaped characters to. * \param[in] bufsize sizeof(buf). * \param[in] str A string that may contain any characters. - * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length (also past nul chars). + * \param[in] legacy_format If false, return C compatible string constants ("\x0f"), if true the legacy + * escaping format ("\15"). The legacy format also escapes as "\a\b\f\v", while + * the non-legacy format also escapes those as "\xNN" sequences. * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). */ -char *osmo_quote_str_buf2(char *buf, size_t bufsize, const char *str, int in_len) +static size_t _osmo_quote_str_buf(char *buf, size_t bufsize, const char *str, int in_len, bool legacy_format) { struct osmo_strbuf sb = { .buf = buf, .len = bufsize }; if (!str) OSMO_STRBUF_PRINTF(sb, "NULL"); else { OSMO_STRBUF_PRINTF(sb, "\""); - OSMO_STRBUF_APPEND_NOLEN(sb, osmo_escape_str_buf2, str, in_len); + OSMO_STRBUF_APPEND(sb, _osmo_escape_str_buf, str, in_len, legacy_format); OSMO_STRBUF_PRINTF(sb, "\""); } + return sb.chars_needed; +} + +/*! Like osmo_escape_str_buf3(), but returns double-quotes around a string, or "NULL" for a NULL string. + * This allows passing any char* value and get its C representation as string. + * The function signature is suitable for OSMO_STRBUF_APPEND_NOLEN(). + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length. + * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). + */ +int osmo_quote_str_buf3(char *buf, size_t bufsize, const char *str, int in_len) +{ + return _osmo_quote_str_buf(buf, bufsize, str, in_len, false); +} + +/*! Like osmo_escape_str_buf2(), but returns double-quotes around a string, or "NULL" for a NULL string. + * This allows passing any char* value and get its C representation as string. + * The function signature is suitable for OSMO_STRBUF_APPEND_NOLEN(). + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length. + * \return The output buffer (buf). + */ +char *osmo_quote_str_buf2(char *buf, size_t bufsize, const char *str, int in_len) +{ + _osmo_quote_str_buf(buf, bufsize, str, in_len, true); return buf; } @@ -792,7 +940,7 @@ const char *osmo_quote_str_buf(const char *str, int in_len, char *buf, size_t bu return "NULL"; if (!buf || !bufsize) return "(error)"; - osmo_quote_str_buf2(buf, bufsize, str, in_len); + _osmo_quote_str_buf(buf, bufsize, str, in_len, true); return buf; } @@ -804,32 +952,78 @@ const char *osmo_quote_str_buf(const char *str, int in_len, char *buf, size_t bu */ const char *osmo_quote_str(const char *str, int in_len) { - return osmo_quote_str_buf(str, in_len, namebuf, sizeof(namebuf)); + _osmo_quote_str_buf(namebuf, sizeof(namebuf), str, in_len, true); + return namebuf; } /*! Like osmo_quote_str_buf() but returns the result in a dynamically-allocated buffer. - * The static buffer is shared with get_value_string() and osmo_escape_str(). * \param[in] str A string that may contain any characters. * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length. * \returns dynamically-allocated buffer containing a quoted and escaped representation. */ char *osmo_quote_str_c(const void *ctx, const char *str, int in_len) { - size_t len = in_len == -1 ? strlen(str) : in_len; - char *buf; + /* The string will be at least as long as in_len, but some characters might need escaping. + * These extra bytes should catch most usual escaping situations, avoiding a second run in OSMO_NAME_C_IMPL. */ + OSMO_NAME_C_IMPL(ctx, in_len + 16, "ERROR", _osmo_quote_str_buf, str, in_len, true); +} - /* account for two quote characters + terminating NUL */ - len += 3; +/*! Return the string with all non-printable characters escaped. + * In contrast to osmo_escape_str_buf2(), this returns the needed buffer size suitable for OSMO_STRBUF_APPEND(), and + * this escapes characters in a way compatible with C string constant syntax. + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length (also past nul chars). + * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). + */ +size_t osmo_escape_cstr_buf(char *buf, size_t bufsize, const char *str, int in_len) +{ + return _osmo_escape_str_buf(buf, bufsize, str, in_len, false); +} - /* some minimum length for things like "NULL" or "(error)" */ - if (len < 32) - len = 32; +/*! Return the string with all non-printable characters escaped, in dynamically-allocated buffer. + * In contrast to osmo_escape_str_c(), this escapes characters in a way compatible with C string constant syntax, and + * allocates sufficient memory in all cases. + * \param[in] str A string that may contain any characters. + * \param[in] len Pass -1 to print until nul char, or >= 0 to force a length. + * \returns dynamically-allocated buffer, containing an escaped representation. + */ +char *osmo_escape_cstr_c(void *ctx, const char *str, int in_len) +{ + /* The string will be at least as long as in_len, but some characters might need escaping. + * These extra bytes should catch most usual escaping situations, avoiding a second run in OSMO_NAME_C_IMPL. */ + OSMO_NAME_C_IMPL(ctx, in_len + 16, "ERROR", _osmo_escape_str_buf, str, in_len, false); +} - buf = talloc_size(ctx, len); - if (!buf) - return NULL; +/*! Like osmo_escape_str_buf2(), but returns double-quotes around a string, or "NULL" for a NULL string. + * This allows passing any char* value and get its C representation as string. + * The function signature is suitable for OSMO_STRBUF_APPEND_NOLEN(). + * In contrast to osmo_escape_str_buf2(), this returns the needed buffer size suitable for OSMO_STRBUF_APPEND(), and + * this escapes characters in a way compatible with C string constant syntax. + * \param[out] buf string buffer to write escaped characters to. + * \param[in] bufsize sizeof(buf). + * \param[in] str A string that may contain any characters. + * \param[in] in_len Pass -1 to print until nul char, or >= 0 to force a length. + * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). + */ +size_t osmo_quote_cstr_buf(char *buf, size_t bufsize, const char *str, int in_len) +{ + return _osmo_quote_str_buf(buf, bufsize, str, in_len, false); +} - return osmo_quote_str_buf2(buf, len, str, in_len); +/*! Return the string quoted and with all non-printable characters escaped, in dynamically-allocated buffer. + * In contrast to osmo_quote_str_c(), this escapes characters in a way compatible with C string constant syntax, and + * allocates sufficient memory in all cases. + * \param[in] str A string that may contain any characters. + * \param[in] len Pass -1 to print until nul char, or >= 0 to force a length. + * \returns dynamically-allocated buffer, containing a quoted and escaped representation. + */ +char *osmo_quote_cstr_c(void *ctx, const char *str, int in_len) +{ + /* The string will be at least as long as in_len plus two quotes, but some characters might need escaping. + * These extra bytes should catch most usual escaping situations, avoiding a second run in OSMO_NAME_C_IMPL. */ + OSMO_NAME_C_IMPL(ctx, in_len + 16, "ERROR", _osmo_quote_str_buf, str, in_len, false); } /*! perform an integer square root operation on unsigned 32bit integer. @@ -1019,6 +1213,51 @@ char osmo_luhn(const char* in, int in_len) return (sum * 9) % 10 + '0'; } +/*! Remove up to N chars from the end of an osmo_strbuf. + * |--char-count---| - - chars_needed - - | + * |<---------drop----------| + */ +void osmo_strbuf_drop_tail(struct osmo_strbuf *sb, size_t n_chars) +{ + size_t drop_n; + if (sb->pos <= sb->buf) + return; + drop_n = OSMO_MIN(sb->chars_needed, n_chars); + sb->chars_needed -= drop_n; + /* chars_needed was reduced by n_chars, which may have been entirely behind the end of a full buffer, within the + * hypothetical chars_needed. Modify the buffer tail pos only if the buffer is not or longer full now. */ + if (sb->chars_needed >= OSMO_STRBUF_CHAR_COUNT(*sb)) + return; + sb->pos = sb->buf + sb->chars_needed; + *sb->pos = '\0'; +} + +/*! Let osmo_strbuf know that n_chars characters (excluding nul) were written to the end of the buffer. + * If sb is nonempty, the n_chars are assumed to have been written to sb->pos. If sb is still empty and pos == NULL, the + * n_chars are assumed to have been written to the start of the buffer. + * Advance sb->pos and sb->chars_needed by at most n_chars, or up to sb->len - 1. + * Ensure nul termination. */ +void osmo_strbuf_added_tail(struct osmo_strbuf *sb, size_t n_chars) +{ + /* On init of an osmo_strbuf, sb->pos == NULL, which is defined as semantically identical to pointing at the + * start of the buffer. A caller may just write to the buffer and call osmo_strbuf_added_tail(), in which case + * still pos == NULL. pos != NULL happens as soon as the first OSMO_STRBUF_*() API has acted on the strbuf. */ + if (!sb->pos) + sb->pos = sb->buf; + sb->chars_needed += n_chars; + /* first get remaining space, not counting trailing nul; but safeguard against empty buffer */ + size_t n_added = OSMO_STRBUF_REMAIN(*sb); + if (n_added) + n_added--; + /* do not add more than fit in sb->len, still ensuring nul termination */ + n_added = OSMO_MIN(n_added, n_chars); + if (n_added) + sb->pos += n_added; + /* when a strbuf is full, sb->pos may point after the final nul, so nul terminate only when pos is valid. */ + if (sb->pos < sb->buf + sb->len) + *sb->pos = '\0'; +} + /*! Compare start of a string. * This is an optimisation of 'strstr(str, startswith_str) == str' because it doesn't search through the entire string. * \param str (Longer) string to compare. @@ -1033,4 +1272,315 @@ bool osmo_str_startswith(const char *str, const char *startswith_str) return strncmp(str, startswith_str, strlen(startswith_str)) == 0; } +/*! Convert a string of a floating point number to a signed int, with a decimal factor (fixed-point precision). + * For example, with precision=3, convert "-1.23" to -1230. In other words, the float value is multiplied by + * 10 to-the-power-of precision to obtain the returned integer. + * The usable range of digits is -INT64_MAX .. INT64_MAX -- note, not INT64_MIN! The value of INT64_MIN is excluded to + * reduce implementation complexity. See also utils_test.c. + * The advantage over using sscanf("%f") is guaranteed precision: float or double types may apply rounding in the + * conversion result. osmo_float_str_to_int() and osmo_int_to_float_str_buf() guarantee true results when converting + * back and forth between string and int. + * \param[out] val Returned integer value. + * \param[in] str String of a float, like '-12.345'. + * \param[in] precision Fixed-point precision, or * \returns 0 on success, negative on error. + */ +int osmo_float_str_to_int(int64_t *val, const char *str, unsigned int precision) +{ + const char *point; + char *endptr; + const char *p; + int64_t sign = 1; + int64_t integer = 0; + int64_t decimal = 0; + int64_t precision_factor; + int64_t integer_max; + int64_t decimal_max; + unsigned int i; + + OSMO_ASSERT(val); + *val = 0; + + if (!str) + return -EINVAL; + if (str[0] == '-') { + str = str + 1; + sign = -1; + } else if (str[0] == '+') { + str = str + 1; + } + if (!str[0]) + return -EINVAL; + + /* Validate entire string as purely digits and at most one decimal dot. If not doing this here in advance, + * parsing digits might stop early because of precision cut-off and miss validation of input data. */ + point = NULL; + for (p = str; *p; p++) { + if (*p == '.') { + if (point) + return -EINVAL; + point = p; + } else if (!isdigit((unsigned char)*p)) + return -EINVAL; + } + + /* Parse integer part if there is one. If the string starts with a point, there's nothing to parse for the + * integer part. */ + if (!point || point > str) { + errno = 0; + integer = strtoll(str, &endptr, 10); + if ((errno == ERANGE && (integer == LLONG_MAX || integer == LLONG_MIN)) + || (errno != 0 && integer == 0)) + return -ERANGE; + + if ((point && endptr != point) + || (!point && *endptr)) + return -EINVAL; + } + + /* Parse the fractional part if there is any, and if the precision is nonzero (if we even care about fractional + * digits) */ + if (precision && point && point[1] != '\0') { + /* limit the number of digits parsed to 'precision'. + * If 'precision' is larger than the 19 digits representable in int64_t, skip some, to pick up lower + * magnitude digits. */ + unsigned int skip_digits = (precision < 20) ? 0 : precision - 20; + char decimal_str[precision + 1]; + osmo_strlcpy(decimal_str, point+1, precision+1); + + /* fill with zeros to make exactly 'precision' digits */ + for (i = strlen(decimal_str); i < precision; i++) + decimal_str[i] = '0'; + decimal_str[precision] = '\0'; + + for (i = 0; i < skip_digits; i++) { + /* When skipping digits because precision > nr-of-digits-in-int64_t, they must be zero; + * if there is a nonzero digit above the precision, it's -ERANGE. */ + if (decimal_str[i] != '0') + return -ERANGE; + } + errno = 0; + decimal = strtoll(decimal_str + skip_digits, &endptr, 10); + if ((errno == ERANGE && (decimal == LLONG_MAX || decimal == LLONG_MIN)) + || (errno != 0 && decimal == 0)) + return -ERANGE; + + if (*endptr) + return -EINVAL; + } + + if (precision > 18) { + /* Special case of returning more digits than fit in int64_t range, e.g. + * osmo_float_str_to_int("0.0000000012345678901234567", precision=25) -> 12345678901234567. */ + precision_factor = 0; + integer_max = 0; + decimal_max = INT64_MAX; + } else { + /* Do not surpass the resulting int64_t range. Depending on the amount of precision, the integer part + * and decimal part have specific ranges they must comply to. */ + precision_factor = 1; + for (i = 0; i < precision; i++) + precision_factor *= 10; + integer_max = INT64_MAX / precision_factor; + if (integer == integer_max) + decimal_max = INT64_MAX % precision_factor; + else + decimal_max = INT64_MAX; + } + + if (integer > integer_max) + return -ERANGE; + if (decimal > decimal_max) + return -ERANGE; + + *val = sign * (integer * precision_factor + decimal); + return 0; +} + +/*! Convert an integer to a floating point string using a decimal quotient (fixed-point precision). + * For example, with precision = 3, convert -1230 to "-1.23". + * The usable range of digits is -INT64_MAX .. INT64_MAX -- note, not INT64_MIN! The value of INT64_MIN is excluded to + * reduce implementation complexity. See also utils_test.c. + * The advantage over using printf("%.6g") is guaranteed precision: float or double types may apply rounding in the + * conversion result. osmo_float_str_to_int() and osmo_int_to_float_str_buf() guarantee true results when converting + * back and forth between string and int. + * The resulting string omits trailing zeros in the fractional part (like "%g" would) but never applies rounding. + * \param[out] buf Buffer to write string to. + * \param[in] buflen sizeof(buf). + * \param[in] val Value to convert to float. + * \returns number of chars that would be written, like snprintf(). + */ +int osmo_int_to_float_str_buf(char *buf, size_t buflen, int64_t val, unsigned int precision) +{ + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + unsigned int i; + unsigned int w; + int64_t precision_factor; + if (val < 0) { + OSMO_STRBUF_PRINTF(sb, "-"); + if (val == INT64_MIN) { + OSMO_STRBUF_PRINTF(sb, "ERR"); + return sb.chars_needed; + } + val = -val; + } + + if (precision > 18) { + /* Special case of returning more digits than fit in int64_t range, e.g. + * osmo_int_to_float_str(12345678901234567, precision=25) -> "0.0000000012345678901234567". */ + if (!val) { + OSMO_STRBUF_PRINTF(sb, "0"); + return sb.chars_needed; + } + OSMO_STRBUF_PRINTF(sb, "0."); + for (i = 19; i < precision; i++) + OSMO_STRBUF_PRINTF(sb, "0"); + precision = 19; + } else { + precision_factor = 1; + for (i = 0; i < precision; i++) + precision_factor *= 10; + + OSMO_STRBUF_PRINTF(sb, "%" PRId64, val / precision_factor); + val %= precision_factor; + if (!val) + return sb.chars_needed; + OSMO_STRBUF_PRINTF(sb, "."); + } + + /* print fractional part, skip trailing zeros */ + w = precision; + while (!(val % 10)) { + val /= 10; + w--; + } + OSMO_STRBUF_PRINTF(sb, "%0*" PRId64, w, val); + return sb.chars_needed; +} + +/*! Convert an integer with a factor of a million to a floating point string. + * For example, convert -1230000 to "-1.23". + * \param[in] ctx Talloc ctx to allocate string buffer from. + * \param[in] val Value to convert to float. + * \returns resulting string, dynamically allocated. + */ +char *osmo_int_to_float_str_c(void *ctx, int64_t val, unsigned int precision) +{ + OSMO_NAME_C_IMPL(ctx, 16, "ERROR", osmo_int_to_float_str_buf, val, precision) +} + +/*! Convert a string of a number to int64_t, including all common strtoll() validity checks. + * It's not so trivial to call strtoll() and properly verify that the input string was indeed a valid number string. + * \param[out] result Buffer for the resulting integer number, or NULL if the caller is only interested in the + * validation result (returned rc). + * \param[in] str The string to convert. + * \param[in] base The integer base, i.e. 10 for decimal numbers or 16 for hexadecimal, as in strtoll(). + * \param[in] min_val The smallest valid number expected in the string. + * \param[in] max_val The largest valid number expected in the string. + * \return 0 on success, -EOVERFLOW if the number in the string exceeds int64_t, -ENOTSUPP if the base is not supported, + * -ERANGE if the converted number exceeds the range [min_val..max_val] but is still within int64_t range, -E2BIG if + * surplus characters follow after the number, -EINVAL if the string does not contain a number. In case of -ERANGE and + * -E2BIG, the converted number is still accurately returned in result. In case of -EOVERFLOW, the returned value is + * clamped to INT64_MIN..INT64_MAX. + */ +int osmo_str_to_int64(int64_t *result, const char *str, int base, int64_t min_val, int64_t max_val) +{ + long long int val; + char *endptr; + if (result) + *result = 0; + if (!str || !*str) + return -EINVAL; + errno = 0; + val = strtoll(str, &endptr, base); + /* In case the number string exceeds long long int range, strtoll() clamps the returned value to LLONG_MIN or + * LLONG_MAX. Make sure of the same here with respect to int64_t. */ + if (val < INT64_MIN) { + if (result) + *result = INT64_MIN; + return -ERANGE; + } + if (val > INT64_MAX) { + if (result) + *result = INT64_MAX; + return -ERANGE; + } + if (result) + *result = (int64_t)val; + switch (errno) { + case 0: + break; + case ERANGE: + return -EOVERFLOW; + default: + case EINVAL: + return -ENOTSUP; + } + if (!endptr || *endptr) { + /* No chars were converted */ + if (endptr == str) + return -EINVAL; + /* Or there are surplus chars after the converted number */ + return -E2BIG; + } + if (val < min_val || val > max_val) + return -ERANGE; + return 0; +} + +/*! Convert a string of a number to int, including all common strtoll() validity checks. + * Same as osmo_str_to_int64() but using the plain int data type. + * \param[out] result Buffer for the resulting integer number, or NULL if the caller is only interested in the + * validation result (returned rc). + * \param[in] str The string to convert. + * \param[in] base The integer base, i.e. 10 for decimal numbers or 16 for hexadecimal, as in strtoll(). + * \param[in] min_val The smallest valid number expected in the string. + * \param[in] max_val The largest valid number expected in the string. + * \return 0 on success, -EOVERFLOW if the number in the string exceeds int range, -ENOTSUPP if the base is not supported, + * -ERANGE if the converted number exceeds the range [min_val..max_val] but is still within int range, -E2BIG if + * surplus characters follow after the number, -EINVAL if the string does not contain a number. In case of -ERANGE and + * -E2BIG, the converted number is still accurately returned in result. In case of -EOVERFLOW, the returned value is + * clamped to INT_MIN..INT_MAX. + */ +int osmo_str_to_int(int *result, const char *str, int base, int min_val, int max_val) +{ + int64_t val; + int rc = osmo_str_to_int64(&val, str, base, min_val, max_val); + /* In case the number string exceeds long long int range, strtoll() clamps the returned value to LLONG_MIN or + * LLONG_MAX. Make sure of the same here with respect to int. */ + if (val < INT_MIN) { + if (result) + *result = INT_MIN; + return -EOVERFLOW; + } + if (val > INT_MAX) { + if (result) + *result = INT_MAX; + return -EOVERFLOW; + } + if (result) + *result = (int)val; + return rc; +} + +/*! Replace a string using talloc and release its prior content (if any). + * This is a format string capable equivalent of osmo_talloc_replace_string(). + * \param[in] ctx Talloc context to use for allocation. + * \param[out] dst Pointer to string, will be updated with ptr to new string. + * \param[in] fmt Format string that will be copied to newly allocated string. */ +void osmo_talloc_replace_string_fmt(void *ctx, char **dst, const char *fmt, ...) +{ + char *name = NULL; + + if (fmt != NULL) { + va_list ap; + + va_start(ap, fmt); + name = talloc_vasprintf(ctx, fmt, ap); + va_end(ap); + } + + talloc_free(*dst); + *dst = name; +} + /*! @} */ diff --git a/src/write_queue.c b/src/core/write_queue.c index 3399b0f1..8fb73a67 100644 --- a/src/write_queue.c +++ b/src/core/write_queue.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <errno.h> @@ -64,16 +60,19 @@ int osmo_wqueue_bfd_cb(struct osmo_fd *fd, unsigned int what) fd->when &= ~OSMO_FD_WRITE; + msg = msgb_dequeue_count(&queue->msg_queue, &queue->current_length); /* the queue might have been emptied */ - if (!llist_empty(&queue->msg_queue)) { - --queue->current_length; - - msg = msgb_dequeue(&queue->msg_queue); + if (msg) { rc = queue->write_cb(fd, msg); - msgb_free(msg); - - if (rc == -EBADF) + if (rc == -EBADF) { + msgb_free(msg); goto err_badfd; + } else if (rc == -EAGAIN) { + /* re-enqueue the msgb to the head of the queue */ + llist_add(&msg->list, &queue->msg_queue); + queue->current_length++; + } else + msgb_free(msg); if (!llist_empty(&queue->msg_queue)) fd->when |= OSMO_FD_WRITE; @@ -100,10 +99,26 @@ void osmo_wqueue_init(struct osmo_wqueue *queue, int max_length) INIT_LLIST_HEAD(&queue->msg_queue); } +/*! Enqueue a new \ref msgb into a write queue (without logging full queue events) + * \param[in] queue Write queue to be used + * \param[in] data to-be-enqueued message buffer + * \returns 0 on success; negative on error (MESSAGE NOT FREED IN CASE OF ERROR). + */ +int osmo_wqueue_enqueue_quiet(struct osmo_wqueue *queue, struct msgb *data) +{ + if (queue->current_length >= queue->max_length) + return -ENOSPC; + + msgb_enqueue_count(&queue->msg_queue, data, &queue->current_length); + queue->bfd.when |= OSMO_FD_WRITE; + + return 0; +} + /*! Enqueue a new \ref msgb into a write queue * \param[in] queue Write queue to be used * \param[in] data to-be-enqueued message buffer - * \returns 0 on success; negative on error + * \returns 0 on success; negative on error (MESSAGE NOT FREED IN CASE OF ERROR). */ int osmo_wqueue_enqueue(struct osmo_wqueue *queue, struct msgb *data) { @@ -113,11 +128,7 @@ int osmo_wqueue_enqueue(struct osmo_wqueue *queue, struct msgb *data) return -ENOSPC; } - ++queue->current_length; - msgb_enqueue(&queue->msg_queue, data); - queue->bfd.when |= OSMO_FD_WRITE; - - return 0; + return osmo_wqueue_enqueue_quiet(queue, data); } /*! Clear a \ref osmo_wqueue @@ -136,4 +147,24 @@ void osmo_wqueue_clear(struct osmo_wqueue *queue) queue->bfd.when &= ~OSMO_FD_WRITE; } +/*! Update write queue length & drop excess messages. + * \param[in] queue linked list header of message queue + * \param[in] len new max. wqueue length + * \returns Number of messages dropped. + * + * Messages beyond the new maximum message queue size will be dropped. + */ +size_t osmo_wqueue_set_maxlen(struct osmo_wqueue *queue, unsigned int len) +{ + size_t dropped_msgs = 0; + struct msgb *msg; + queue->max_length = len; + while (queue->current_length > queue->max_length) { + msg = msgb_dequeue_count(&queue->msg_queue, &queue->current_length); + msgb_free(msg); + dropped_msgs++; + } + return dropped_msgs; +} + /*! @} */ diff --git a/src/ctrl/Makefile.am b/src/ctrl/Makefile.am index ca642869..f9e34333 100644 --- a/src/ctrl/Makefile.am +++ b/src/ctrl/Makefile.am @@ -1,9 +1,10 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=4:0:4 +LIBVERSION=8:1:8 -AM_CFLAGS = -Wall $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)/include $(TALLOC_CFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) if ENABLE_CTRL lib_LTLIBRARIES = libosmoctrl.la @@ -12,7 +13,7 @@ libosmoctrl_la_SOURCES = control_cmd.c control_if.c fsm_ctrl_commands.c libosmoctrl_la_LDFLAGS = $(LTLDFLAGS_OSMOCTRL) -version-info $(LIBVERSION) -no-undefined libosmoctrl_la_LIBADD = $(TALLOC_LIBS) \ - $(top_builddir)/src/libosmocore.la \ + $(top_builddir)/src/core/libosmocore.la \ $(top_builddir)/src/gsm/libosmogsm.la \ $(top_builddir)/src/vty/libosmovty.la @@ -21,5 +22,6 @@ libosmoctrl_la_SOURCES += control_vty.c endif EXTRA_DIST = libosmoctrl.map +EXTRA_libosmoctrl_la_DEPENDENCIES = libosmoctrl.map endif diff --git a/src/ctrl/control_cmd.c b/src/ctrl/control_cmd.c index 33496bd8..db205510 100644 --- a/src/ctrl/control_cmd.c +++ b/src/ctrl/control_cmd.c @@ -20,10 +20,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <ctype.h> @@ -35,6 +31,7 @@ #include <unistd.h> #include <osmocom/ctrl/control_cmd.h> +#include <osmocom/ctrl/control_if.h> #include <osmocom/core/msgb.h> #include <osmocom/core/talloc.h> @@ -207,13 +204,23 @@ failure: } /*! Install a given command definition at a given CTRL node. - * \param[in] node CTRL node at whihc \a cmd is to be installed + * \param[in] node CTRL node at which \a cmd is to be installed * \param[in] cmd command definition to be installed * \returns 0 on success; negative on error */ int ctrl_cmd_install(enum ctrl_node_type node, struct ctrl_cmd_element *cmd) { vector cmds_vec; + /* If this assert triggers, it means the program forgot to initialize + * the CTRL interface first by calling ctrl_handle_alloc(2)() directly + * or indirectly through ctrl_interface_setup_dynip(2)() + */ + if (!ctrl_node_vec) { + LOGP(DLCTRL, LOGL_ERROR, + "ctrl_handle must be initialized prior to installing cmds.\n"); + return -ENODEV; + } + cmds_vec = vector_lookup_ensure(ctrl_node_vec, node); if (!cmds_vec) { @@ -516,92 +523,61 @@ err: * \returns callee-allocated message buffer containing the encoded \a cmd; NULL on error */ struct msgb *ctrl_cmd_make(struct ctrl_cmd *cmd) { - struct msgb *msg; + struct msgb *msg = NULL; + char *strbuf; + size_t len; const char *type; - char *tmp; if (!cmd->id) return NULL; - msg = msgb_alloc_headroom(4096, 128, "ctrl command make"); - if (!msg) - return NULL; - type = get_value_string(ctrl_type_vals, cmd->type); switch (cmd->type) { case CTRL_TYPE_GET: if (!cmd->variable) - goto err; - - tmp = talloc_asprintf(cmd, "%s %s %s", type, cmd->id, cmd->variable); - if (!tmp) { - LOGP(DLCTRL, LOGL_ERROR, "Failed to allocate cmd.\n"); - goto err; - } - - msg->l2h = msgb_put(msg, strlen(tmp)); - memcpy(msg->l2h, tmp, strlen(tmp)); - talloc_free(tmp); + return NULL; + strbuf = talloc_asprintf(cmd, "%s %s %s", type, cmd->id, cmd->variable); break; case CTRL_TYPE_SET: if (!cmd->variable || !cmd->value) - goto err; - - tmp = talloc_asprintf(cmd, "%s %s %s %s", type, cmd->id, cmd->variable, - cmd->value); - if (!tmp) { - LOGP(DLCTRL, LOGL_ERROR, "Failed to allocate cmd.\n"); - goto err; - } - - msg->l2h = msgb_put(msg, strlen(tmp)); - memcpy(msg->l2h, tmp, strlen(tmp)); - talloc_free(tmp); + return NULL; + strbuf = talloc_asprintf(cmd, "%s %s %s %s", type, cmd->id, + cmd->variable, cmd->value); break; case CTRL_TYPE_GET_REPLY: case CTRL_TYPE_SET_REPLY: case CTRL_TYPE_TRAP: if (!cmd->variable || !cmd->reply) - goto err; - - tmp = talloc_asprintf(cmd, "%s %s %s %s", type, cmd->id, cmd->variable, - cmd->reply); - if (!tmp) { - LOGP(DLCTRL, LOGL_ERROR, "Failed to allocate cmd.\n"); - goto err; - } - - msg->l2h = msgb_put(msg, strlen(tmp)); - memcpy(msg->l2h, tmp, strlen(tmp)); - talloc_free(tmp); + return NULL; + strbuf = talloc_asprintf(cmd, "%s %s %s %s", type, cmd->id, + cmd->variable, cmd->reply); break; case CTRL_TYPE_ERROR: if (!cmd->reply) - goto err; - - tmp = talloc_asprintf(cmd, "%s %s %s", type, cmd->id, - cmd->reply); - if (!tmp) { - LOGP(DLCTRL, LOGL_ERROR, "Failed to allocate cmd.\n"); - goto err; - } - - msg->l2h = msgb_put(msg, strlen(tmp)); - memcpy(msg->l2h, tmp, strlen(tmp)); - talloc_free(tmp); + return NULL; + strbuf = talloc_asprintf(cmd, "%s %s %s", type, cmd->id, cmd->reply); break; default: LOGP(DLCTRL, LOGL_NOTICE, "Unknown command type %i\n", cmd->type); - goto err; - break; + return NULL; } - return msg; + if (!strbuf) { + LOGP(DLCTRL, LOGL_ERROR, "Failed to allocate cmd.\n"); + goto ret; + } + len = strlen(strbuf); -err: - msgb_free(msg); - return NULL; + msg = msgb_alloc_headroom(len + 128, 128, "ctrl ERROR command make"); + if (!msg) + goto ret; + msg->l2h = msgb_put(msg, len); + memcpy(msg->l2h, strbuf, len); + +ret: + talloc_free(strbuf); + return msg; } /*! Build a deferred control command state and keep it the per-connection list of deferred commands. @@ -672,7 +648,7 @@ int ctrl_cmd_def_send(struct ctrl_cmd_def *cd) cmd->type = CTRL_TYPE_ERROR; } - rc = ctrl_cmd_send(&cmd->ccon->write_queue, cmd); + rc = ctrl_cmd_send2(cmd->ccon, cmd); talloc_free(cmd); llist_del(&cd->list); diff --git a/src/ctrl/control_if.c b/src/ctrl/control_if.c index ce2e3676..c265c3a9 100644 --- a/src/ctrl/control_if.c +++ b/src/ctrl/control_if.c @@ -20,10 +20,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include "config.h" @@ -35,6 +31,7 @@ #include <string.h> #include <time.h> #include <unistd.h> +#include <limits.h> #include <arpa/inet.h> @@ -50,9 +47,11 @@ #include <osmocom/ctrl/control_cmd.h> #include <osmocom/ctrl/control_if.h> +#include <osmocom/ctrl/control_vty.h> #include <osmocom/core/msgb.h> #include <osmocom/core/rate_ctr.h> +#include <osmocom/core/stat_item.h> #include <osmocom/core/select.h> #include <osmocom/core/counter.h> #include <osmocom/core/talloc.h> @@ -82,26 +81,22 @@ static LLIST_HEAD(ctrl_lookup_helpers); * \returns 1 on success; 0 in case of error */ int ctrl_parse_get_num(vector vline, int i, long *num) { - char *token, *tmp; + char *token; + int64_t val; if (i >= vector_active(vline)) return 0; token = vector_slot(vline, i); - errno = 0; - if (token[0] == '\0') - return 0; - - *num = strtol(token, &tmp, 10); - if (tmp[0] != '\0' || errno != 0) + if (osmo_str_to_int64(&val, token, 10, LONG_MIN, LONG_MAX)) return 0; - + *num = (long)val; return 1; } /*! Send a CTRL command to all connections. * \param[in] ctrl global control handle - * \param[in] cmd command to send to all connections in \ctrl + * \param[in] cmd command to send to all connections in ctrl * \returns number of times the command has been sent */ int ctrl_cmd_send_to_all(struct ctrl_handle *ctrl, struct ctrl_cmd *cmd) { @@ -111,18 +106,28 @@ int ctrl_cmd_send_to_all(struct ctrl_handle *ctrl, struct ctrl_cmd *cmd) llist_for_each_entry(ccon, &ctrl->ccon_list, list_entry) { if (ccon == cmd->ccon) continue; - if (ctrl_cmd_send(&ccon->write_queue, cmd)) + if (ctrl_cmd_send2(ccon, cmd)) ret++; } return ret; } -/*! Encode a CTRL command and append it to the given write queue +/*! Encode a CTRL command and append it to the given ctrl_connection * \param[inout] queue write queue to which encoded \a cmd shall be appended * \param[in] cmd decoded command representation * \returns 0 in case of success; negative on error */ int ctrl_cmd_send(struct osmo_wqueue *queue, struct ctrl_cmd *cmd) { + struct ctrl_connection *ccon = container_of(queue, struct ctrl_connection, write_queue); + return ctrl_cmd_send2(ccon, cmd); +} + +/*! Encode a CTRL command and append it to the given ctrl_connection + * \param[inout] queue write queue to which encoded \a cmd shall be appended + * \param[in] cmd decoded command representation + * \returns 0 in case of success; negative on error */ +int ctrl_cmd_send2(struct ctrl_connection *ccon, struct ctrl_cmd *cmd) +{ int ret; struct msgb *msg; @@ -135,7 +140,7 @@ int ctrl_cmd_send(struct osmo_wqueue *queue, struct ctrl_cmd *cmd) ipa_prepend_header_ext(msg, IPAC_PROTO_EXT_CTRL); ipa_prepend_header(msg, IPAC_PROTO_OSMO); - ret = osmo_wqueue_enqueue(queue, msg); + ret = osmo_wqueue_enqueue(&ccon->write_queue, msg); if (ret != 0) { LOGP(DLCTRL, LOGL_ERROR, "Failed to enqueue the command.\n"); msgb_free(msg); @@ -221,6 +226,10 @@ int ctrl_cmd_handle(struct ctrl_handle *ctrl, struct ctrl_cmd *cmd, if (cmd->type == CTRL_TYPE_SET_REPLY || cmd->type == CTRL_TYPE_GET_REPLY) { + if (ctrl->reply_cb) { + ctrl->reply_cb(ctrl, cmd, data); + return CTRL_CMD_HANDLED; + } if (strncmp(cmd->reply, "OK", 2) == 0) { LOGP(DLCTRL, LOGL_DEBUG, "%s <%s> for %s is OK\n", get_value_string(ctrl_type_vals, cmd->type), @@ -465,7 +474,7 @@ int ctrl_handle_msg(struct ctrl_handle *ctrl, struct ctrl_connection *ccon, stru send_reply: /* There is a reply or error that should be reported back to the sender. */ - ctrl_cmd_send(&ccon->write_queue, cmd); + ctrl_cmd_send2(ccon, cmd); just_free: talloc_free(cmd); return 0; @@ -485,8 +494,14 @@ static int control_write_cb(struct osmo_fd *bfd, struct msgb *msg) control_close_conn(ccon); return -EBADF; } - if (rc != msg->len) + if (rc < 0) { LOGP(DLCTRL, LOGL_ERROR, "Failed to write message to the CTRL connection.\n"); + return 0; + } + if (rc < msg->len) { + msgb_pull(msg, rc); + return -EAGAIN; + } return 0; } @@ -510,6 +525,7 @@ struct ctrl_connection *osmo_ctrl_conn_alloc(void *ctx, void *data) INIT_LLIST_HEAD(&ccon->def_cmds); ccon->write_queue.bfd.data = data; + ccon->write_queue.bfd.fd = -1; ccon->write_queue.write_cb = control_write_cb; ccon->write_queue.read_cb = handle_control_read; @@ -584,12 +600,12 @@ static uint64_t get_rate_ctr_value(const struct rate_ctr *ctr, int intv, const c } } -static int get_rate_ctr_group_idx(const struct rate_ctr_group *ctrg, int intv, struct ctrl_cmd *cmd) +static int get_rate_ctr_group_idx(struct rate_ctr_group *ctrg, int intv, struct ctrl_cmd *cmd) { unsigned int i; for (i = 0; i < ctrg->desc->num_ctr; i++) { ctrl_cmd_reply_printf(cmd, "%s %"PRIu64";", ctrg->desc->ctr_desc[i].name, - get_rate_ctr_value(&ctrg->ctr[i], intv, ctrg->desc->group_name_prefix)); + get_rate_ctr_value(rate_ctr_group_get_ctr(ctrg, i), intv, ctrg->desc->group_name_prefix)); if (!cmd->reply) { cmd->reply = "OOM"; return CTRL_CMD_ERROR; @@ -718,6 +734,100 @@ static int verify_rate_ctr(struct ctrl_cmd *cmd, const char *value, void *data) return 0; } +/* Expose all stat_item groups on CTRL, as read-only variables of the form: + * stat_item.(last|...).group_name.N.item_name + * stat_item.(last|...).group_name.by_name.group_idx_name.item_name + */ +CTRL_CMD_DEFINE_RO(stat_item, "stat_item *"); +static int get_stat_item(struct ctrl_cmd *cmd, void *data) +{ + char *dup; + char *saveptr; + char *value_type; + char *group_name; + char *group_idx_str; + char *item_name; + char *tmp; + int32_t val; + struct osmo_stat_item_group *statg; + const struct osmo_stat_item *stat_item; + + /* cmd will be freed in control_if.c after handling here, so no need to free the dup string. */ + dup = talloc_strdup(cmd, cmd->variable); + if (!dup) { + cmd->reply = "OOM"; + return CTRL_CMD_ERROR; + } + + /* Split off the first "stat_item." part */ + tmp = strtok_r(dup, ".", &saveptr); + if (!tmp || strcmp(tmp, "stat_item") != 0) + goto format_error; + + /* Split off the "last." part (validated further below) */ + value_type = strtok_r(NULL, ".", &saveptr); + if (!value_type) + goto format_error; + + /* Split off the "group_name." part */ + group_name = strtok_r(NULL, ".", &saveptr); + if (!group_name) + goto format_error; + + /* Split off the "N." part */ + group_idx_str = strtok_r(NULL, ".", &saveptr); + if (!group_idx_str) + goto format_error; + if (strcmp(group_idx_str, "by_name") == 0) { + /* The index is not given by "N" but by "by_name.foo". Get the "foo" idx-name */ + group_idx_str = strtok_r(NULL, ".", &saveptr); + statg = osmo_stat_item_get_group_by_name_idxname(group_name, group_idx_str); + } else { + int idx; + if (osmo_str_to_int(&idx, group_idx_str, 10, 0, INT_MAX)) + goto format_error; + statg = osmo_stat_item_get_group_by_name_idx(group_name, idx); + } + if (!statg) { + cmd->reply = "Stat group with given name and index not found"; + return CTRL_CMD_ERROR; + } + + /* Split off the "item_name" part */ + item_name = strtok_r(NULL, ".", &saveptr); + if (!item_name) + goto format_error; + stat_item = osmo_stat_item_get_by_name(statg, item_name); + if (!stat_item) { + cmd->reply = "No such stat item found"; + return CTRL_CMD_ERROR; + } + + tmp = strtok_r(NULL, "", &saveptr); + if (tmp) { + cmd->reply = "Garbage after stat item name"; + return CTRL_CMD_ERROR; + } + + if (!strcmp(value_type, "last")) + val = osmo_stat_item_get_last(stat_item); + else + goto format_error; + + cmd->reply = talloc_asprintf(cmd, "%"PRIu32, val); + if (!cmd->reply) { + cmd->reply = "OOM"; + return CTRL_CMD_ERROR; + } + + return CTRL_CMD_REPLY; + +format_error: + cmd->reply = "Stat item must be of form stat_item.type.group_name.N.item_name," + " e.g. 'stat_item.last.bsc.0.msc_num:connected'"; + return CTRL_CMD_ERROR; +} + /* counter */ CTRL_CMD_DEFINE(counter, "counter *"); static int get_counter(struct ctrl_cmd *cmd, void *data) @@ -779,7 +889,7 @@ static int verify_counter(struct ctrl_cmd *cmd, const char *value, void *data) struct ctrl_handle *ctrl_interface_setup(void *data, uint16_t port, ctrl_cmd_lookup lookup) { - return ctrl_interface_setup_dynip(data, "127.0.0.1", port, lookup); + return ctrl_interface_setup2(data, port, lookup, 0); } static int ctrl_initialized = 0; @@ -805,6 +915,9 @@ static int ctrl_init(unsigned int node_count) ret = ctrl_cmd_install(CTRL_NODE_ROOT, &cmd_rate_ctr); if (ret) goto err_vec; + ret = ctrl_cmd_install(CTRL_NODE_ROOT, &cmd_stat_item); + if (ret) + goto err_vec; ret = ctrl_cmd_install(CTRL_NODE_ROOT, &cmd_counter); if (ret) goto err_vec; @@ -928,12 +1041,24 @@ struct ctrl_handle *ctrl_interface_setup_dynip(void *data, return ctrl_interface_setup_dynip2(data, bind_addr, port, lookup, 0); } +/*! Initializes CTRL interface using the configured bind addr/port. + * \param[in] data Pointer which will be made available to each set_..() get_..() verify_..() control command function + * \param[in] default_port TCP port number to bind to if not explicitly configured + * \param[in] lookup Lookup function pointer, can be NULL + * \param[in] node_count Number of CTRL nodes to allocate, 0 for default. + */ +struct ctrl_handle *ctrl_interface_setup2(void *data, uint16_t default_port, ctrl_cmd_lookup lookup, + unsigned int node_count) +{ + return ctrl_interface_setup_dynip2(data, ctrl_vty_get_bind_addr(), ctrl_vty_get_bind_port(default_port), lookup, + node_count); +} /*! Install a lookup helper function for control nodes * This function is used by e.g. library code to install lookup helpers * for additional nodes in the control interface. * \param[in] lookup The lookup helper function - * \retuns - on success; negative on error. + * \returns - on success; negative on error. */ int ctrl_lookup_register(ctrl_cmd_lookup lookup) { diff --git a/src/ctrl/control_vty.c b/src/ctrl/control_vty.c index ef988892..a7ebddc2 100644 --- a/src/ctrl/control_vty.c +++ b/src/ctrl/control_vty.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdlib.h> @@ -30,16 +26,20 @@ static void *ctrl_vty_ctx = NULL; static const char *ctrl_vty_bind_addr = NULL; +/* Port the CTRL should bind to: -1 means not configured */ +static int ctrl_bind_port = -1; DEFUN(cfg_ctrl_bind_addr, cfg_ctrl_bind_addr_cmd, - "bind A.B.C.D", + "bind A.B.C.D [<0-65535>]", "Set bind address to listen for Control connections\n" - "Local IP address (default 127.0.0.1)\n") + "Local IP address (default 127.0.0.1)\n" + "Local TCP port number\n") { talloc_free((char*)ctrl_vty_bind_addr); ctrl_vty_bind_addr = NULL; ctrl_vty_bind_addr = talloc_strdup(ctrl_vty_ctx, argv[0]); + ctrl_bind_port = argc > 1 ? atoi(argv[1]) : -1; return CMD_SUCCESS; } @@ -50,6 +50,11 @@ const char *ctrl_vty_get_bind_addr(void) return ctrl_vty_bind_addr; } +uint16_t ctrl_vty_get_bind_port(uint16_t default_port) +{ + return ctrl_bind_port >= 0 ? ctrl_bind_port : default_port; +} + static struct cmd_node ctrl_node = { L_CTRL_NODE, "%s(config-ctrl)# ", @@ -82,10 +87,10 @@ static int config_write_ctrl(struct vty *vty) int ctrl_vty_init(void *ctx) { ctrl_vty_ctx = ctx; - install_element(CONFIG_NODE, &cfg_ctrl_cmd); + install_lib_element(CONFIG_NODE, &cfg_ctrl_cmd); install_node(&ctrl_node, config_write_ctrl); - install_element(L_CTRL_NODE, &cfg_ctrl_bind_addr_cmd); + install_lib_element(L_CTRL_NODE, &cfg_ctrl_bind_addr_cmd); return 0; } diff --git a/src/ctrl/libosmoctrl.map b/src/ctrl/libosmoctrl.map index f995467b..3418e620 100644 --- a/src/ctrl/libosmoctrl.map +++ b/src/ctrl/libosmoctrl.map @@ -15,6 +15,7 @@ ctrl_cmd_parse; ctrl_cmd_parse2; ctrl_cmd_parse3; ctrl_cmd_send; +ctrl_cmd_send2; ctrl_cmd_send_to_all; ctrl_cmd_send_trap; ctrl_cmd_trap; @@ -22,12 +23,14 @@ ctrl_handle_alloc; /* could be removed? */ ctrl_handle_alloc2; /* could be removed? */ ctrl_handle_msg; /* only used in unit test */ ctrl_interface_setup; +ctrl_interface_setup2; ctrl_interface_setup_dynip; ctrl_interface_setup_dynip2; ctrl_lookup_register; ctrl_parse_get_num; ctrl_type_vals; ctrl_vty_get_bind_addr; +ctrl_vty_get_bind_port; ctrl_vty_init; osmo_ctrl_conn_alloc; diff --git a/src/gb/Makefile.am b/src/gb/Makefile.am index e14c11c1..2f7ad5eb 100644 --- a/src/gb/Makefile.am +++ b/src/gb/Makefile.am @@ -1,26 +1,43 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=9:1:0 +LIBVERSION=16:0:2 -AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -AM_CFLAGS = -Wall ${GCC_FVISIBILITY_HIDDEN} -fno-strict-aliasing $(TALLOC_CFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall -fno-strict-aliasing \ + $(TALLOC_CFLAGS) \ + $(NULL) # FIXME: this should eventually go into a milenage/Makefile.am -noinst_HEADERS = common_vty.h gb_internal.h +noinst_HEADERS = common_vty.h gb_internal.h gprs_bssgp_internal.h gprs_ns2_internal.h if ENABLE_GB lib_LTLIBRARIES = libosmogb.la -libosmogb_la_LDFLAGS = $(LTLDFLAGS_OSMOGB) -version-info $(LIBVERSION) +libosmogb_la_LDFLAGS = \ + $(LTLDFLAGS_OSMOGB) \ + -version-info $(LIBVERSION) \ + -no-undefined \ + $(NULL) libosmogb_la_LIBADD = $(TALLOC_LIBS) \ - $(top_builddir)/src/libosmocore.la \ + $(top_builddir)/src/core/libosmocore.la \ $(top_builddir)/src/vty/libosmovty.la \ $(top_builddir)/src/gsm/libosmogsm.la libosmogb_la_SOURCES = gprs_ns.c gprs_ns_frgre.c gprs_ns_vty.c gprs_ns_sns.c \ - gprs_bssgp.c gprs_bssgp_util.c gprs_bssgp_vty.c \ - gprs_bssgp_bss.c common_vty.c + gprs_bssgp.c gprs_bssgp_util.c gprs_bssgp_vty.c gprs_bssgp_rim.c \ + gprs_bssgp_bss.c \ + gprs_ns2.c gprs_ns2_udp.c gprs_ns2_frgre.c gprs_ns2_fr.c gprs_ns2_vc_fsm.c gprs_ns2_sns.c \ + gprs_ns2_message.c gprs_ns2_vty.c \ + gprs_bssgp2.c bssgp_bvc_fsm.c \ + common_vty.c frame_relay.c + +# convenience library for testing with access to all non-static symbols +noinst_LTLIBRARIES = libosmogb-test.la +libosmogb_test_la_LIBADD = $(libosmogb_la_LIBADD) +libosmogb_test_la_SOURCES= $(libosmogb_la_SOURCES) + endif EXTRA_DIST = libosmogb.map +EXTRA_libosmogb_la_DEPENDENCIES = libosmogb.map diff --git a/src/gb/bssgp_bvc_fsm.c b/src/gb/bssgp_bvc_fsm.c new file mode 100644 index 00000000..3a36c7dc --- /dev/null +++ b/src/gb/bssgp_bvc_fsm.c @@ -0,0 +1,857 @@ +/* BSSGP per-BVC Finite State Machine */ + +/* (C) 2020 Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <string.h> +#include <stdio.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/tdef.h> + +#include <osmocom/gsm/gsm48.h> +#include <osmocom/gsm/tlv.h> + +#include <osmocom/gprs/gprs_msgb.h> +#include <osmocom/gprs/gprs_bssgp.h> +#include <osmocom/gprs/gprs_bssgp2.h> +#include <osmocom/gprs/bssgp_bvc_fsm.h> + +#include "common_vty.h" + +#define S(x) (1 << (x)) + +/* TODO: Those are not made cofnigurable via a VTY yet */ +struct osmo_tdef bssgp_bvc_fsm_tdefs[] = { + { + .T = 1, + .default_val = 5, + .min_val = 1, + .max_val = 30, + .unit = OSMO_TDEF_S, + .desc = "Guards the BSSGP BVC (un)blocking procedure", + }, { + .T = 2, + .default_val = 10, + .min_val = 1, + .max_val = 120, + .unit = OSMO_TDEF_S, + .desc = "Guards the BSSGP BVC reset procedure", + }, { + .T = 3, + .default_val = 500, + .min_val = 100, + .max_val = 10000, + .unit = OSMO_TDEF_MS, + .desc = "Guards the BSSGP SUSPEND procedure", + }, { + .T = 4, + .default_val = 500, + .min_val = 100, + .max_val = 10000, + .unit = OSMO_TDEF_MS, + .desc = "Guards the BSSGP RESUME procedure", + }, { + .T = 5, + .default_val = 15, + .min_val = 1, + .max_val = 30, + .unit = OSMO_TDEF_S, + .desc = "Guards the BSSGP Radio Access Capability Update procedure", + }, + {} +}; + +#define T1 1 +#define T2 2 + +/* We cannot use osmo_tdef_fsm_* as it makes hard-coded assumptions that + * each new/target state will always use the same timer and timeout - or + * a timeout at all */ +#define T1_SECS osmo_tdef_get(bssgp_bvc_fsm_tdefs, 1, OSMO_TDEF_S, 5) +#define T2_SECS osmo_tdef_get(bssgp_bvc_fsm_tdefs, 2, OSMO_TDEF_S, 10) + +/* forward declaration */ +static struct osmo_fsm bssgp_bvc_fsm; + +static const struct value_string ptp_bvc_event_names[] = { + { BSSGP_BVCFSM_E_RX_BLOCK, "RX-BVC-BLOCK" }, + { BSSGP_BVCFSM_E_RX_BLOCK_ACK, "RX-BVC-BLOCK-ACK" }, + { BSSGP_BVCFSM_E_RX_UNBLOCK, "RX-BVC-UNBLOCK" }, + { BSSGP_BVCFSM_E_RX_UNBLOCK_ACK, "RX-BVC-UNBLOCK-ACK" }, + { BSSGP_BVCFSM_E_RX_RESET, "RX-BVC-RESET" }, + { BSSGP_BVCFSM_E_RX_RESET_ACK, "RX-BVC-RESET-ACK" }, + { BSSGP_BVCFSM_E_RX_FC_BVC, "RX-FLOW-CONTROL-BVC" }, + { BSSGP_BVCFSM_E_RX_FC_BVC_ACK, "RX-FLOW-CONTROL-BVC-ACK" }, + { BSSGP_BVCFSM_E_REQ_BLOCK, "REQ-BLOCK" }, + { BSSGP_BVCFSM_E_REQ_UNBLOCK, "REQ-UNBLOCK" }, + { BSSGP_BVCFSM_E_REQ_RESET, "REQ-RESET" }, + { BSSGP_BVCFSM_E_REQ_FC_BVC, "REQ-FLOW-CONTROL-BVC" }, + { 0, NULL } +}; + +struct bvc_fsm_priv { + /* NS-instance; defining the scope for NSEI below */ + struct gprs_ns2_inst *nsi; + + /* NSEI of the underlying NS Entity */ + uint16_t nsei; + /* Maximum size of the BSSGP PDU */ + uint16_t max_pdu_len; + + /* BVCI of this BVC */ + uint16_t bvci; + + /* are we the SGSN (true) or the BSS (false) */ + bool role_sgsn; + + /* BSS side: are we locally marked blocked? */ + bool locally_blocked; + uint8_t block_cause; + + /* cause value of the last outbound BVC-RESET (for re-transmissions) */ + uint8_t last_reset_cause; + + struct { + /* Bit 0..7: Features; Bit 8..15: Extended Features */ + uint32_t advertised; + uint32_t received; + uint32_t negotiated; + /* only used if BSSGP_XFEAT_GBIT is negotiated */ + enum bssgp_fc_granularity fc_granularity; + } features; + + /* Cell Identification used by BSS when + * transmitting BVC-RESET / BVC-RESET-ACK, or those received + * from BSS in SGSN role */ + struct gprs_ra_id ra_id; + uint16_t cell_id; + + /* call-backs provided by the user */ + const struct bssgp_bvc_fsm_ops *ops; + /* private data pointer passed to each call-back invocation */ + void *ops_priv; +}; + +static int fi_tx_ptp(struct osmo_fsm_inst *fi, struct msgb *msg) +{ + struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_bssgph(msg); + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + LOGPFSM(fi, "Tx BSSGP %s\n", osmo_tlv_prot_msg_name(&osmo_pdef_bssgp, bgph->pdu_type)); + + return bssgp2_nsi_tx_ptp(bfp->nsi, bfp->nsei, bfp->bvci, msg, 0); +} + +static int fi_tx_sig(struct osmo_fsm_inst *fi, struct msgb *msg) +{ + struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_bssgph(msg); + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + LOGPFSM(fi, "Tx BSSGP %s\n", osmo_tlv_prot_msg_name(&osmo_pdef_bssgp, bgph->pdu_type)); + + return bssgp2_nsi_tx_sig(bfp->nsi, bfp->nsei, msg, 0); +} + +/* helper function to transmit BVC-RESET with right combination of conditional/optional IEs */ +static void _tx_bvc_reset(struct osmo_fsm_inst *fi, uint8_t cause) +{ + struct bvc_fsm_priv *bfp = fi->priv; + const uint8_t *features = NULL; + const uint8_t *features_ext = NULL; + uint8_t _features[2] = { + (bfp->features.advertised >> 0) & 0xff, + (bfp->features.advertised >> 8) & 0xff, + }; + struct msgb *tx; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + /* transmit BVC-RESET to peer; RA-ID only present for PTP from BSS */ + if (bfp->bvci == 0) { + features = &_features[0]; + features_ext = &_features[1]; + } + tx = bssgp2_enc_bvc_reset(bfp->bvci, cause, + bfp->bvci && !bfp->role_sgsn ? &bfp->ra_id : NULL, + bfp->cell_id, features, features_ext); + fi_tx_sig(fi, tx); +} + +/* helper function to transmit BVC-RESET-ACK with right combination of conditional/optional IEs */ +static void _tx_bvc_reset_ack(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + const uint8_t *features = NULL; + const uint8_t *features_ext = NULL; + uint8_t _features[2] = { + (bfp->features.advertised >> 0) & 0xff, + (bfp->features.advertised >> 8) & 0xff, + }; + struct msgb *tx; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + /* transmit BVC-RESET-ACK to peer; RA-ID only present for PTP from BSS -> SGSN */ + if (bfp->bvci == 0) { + features = &_features[0]; + features_ext = &_features[1]; + } + tx = bssgp2_enc_bvc_reset_ack(bfp->bvci, bfp->bvci && !bfp->role_sgsn ? &bfp->ra_id : NULL, + bfp->cell_id, features, features_ext); + fi_tx_sig(fi, tx); +} + +/* helper function to transmit BVC-STATUS with right combination of conditional/optional IEs */ +static void _tx_status(struct osmo_fsm_inst *fi, enum gprs_bssgp_cause cause, const struct msgb *rx) +{ + struct bvc_fsm_priv *bfp = fi->priv; + struct msgb *tx; + uint16_t *bvci = NULL; + + /* GSM 08.18, 10.4.14.1: The BVCI must be included if (and only if) the + * cause is either "BVCI blocked" or "BVCI unknown" */ + if (cause == BSSGP_CAUSE_UNKNOWN_BVCI || cause == BSSGP_CAUSE_BVCI_BLOCKED) + bvci = &bfp->bvci; + + tx = bssgp2_enc_status(cause, bvci, rx, bfp->max_pdu_len); + + if (msgb_bvci(rx) == 0) + fi_tx_sig(fi, tx); + else + fi_tx_ptp(fi, tx); +} + +/* Update the features by bit-wise AND of advertised + received features */ +static void update_negotiated_features(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + bfp->features.received = 0; + + if (TLVP_PRES_LEN(tp, BSSGP_IE_FEATURE_BITMAP, 1)) + bfp->features.received |= *TLVP_VAL(tp, BSSGP_IE_FEATURE_BITMAP); + + if (TLVP_PRES_LEN(tp, BSSGP_IE_EXT_FEATURE_BITMAP, 1)) + bfp->features.received |= (*TLVP_VAL(tp, BSSGP_IE_EXT_FEATURE_BITMAP) << 8); + + bfp->features.negotiated = bfp->features.advertised & bfp->features.received; + + LOGPFSML(fi, LOGL_NOTICE, "Updating features: Advertised 0x%04x, Received 0x%04x, Negotiated 0x%04x\n", + bfp->features.advertised, bfp->features.received, bfp->features.negotiated); +} + +/* "tail" of each onenter() handler: Calling the state change notification call-back */ +static void _onenter_tail(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + if (prev_state == fi->state) + return; + + if (bfp->ops && bfp->ops->state_chg_notification) + bfp->ops->state_chg_notification(bfp->nsei, bfp->bvci, prev_state, fi->state, bfp->ops_priv); +} + +static void bssgp_bvc_fsm_null(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + /* we don't really expect anything in this state; all handled via allstate */ + OSMO_ASSERT(0); +} + +static void bssgp_bvc_fsm_blocked_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct bvc_fsm_priv *bfp = fi->priv; + /* signaling BVC can never be blocked */ + OSMO_ASSERT(bfp->bvci != 0); + _onenter_tail(fi, prev_state); +} + +static void bssgp_bvc_fsm_blocked(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct bvc_fsm_priv *bfp = fi->priv; + struct msgb *rx = NULL, *tx; + const struct tlv_parsed *tp = NULL; + uint8_t cause; + + switch (event) { + case BSSGP_BVCFSM_E_RX_BLOCK_ACK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + /* If a BVC-BLOCK-ACK PDU is received by a BSS for the signalling BVC, the PDU is ignored. */ + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-BLOCK-ACK on BVCI=0 is illegal\n"); + if (!bfp->role_sgsn) + break; + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + /* stop T1 timer */ + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_BLOCKED, 0, 0); + break; + case BSSGP_BVCFSM_E_RX_BLOCK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + cause = *TLVP_VAL(tp, BSSGP_IE_CAUSE); + LOGPFSML(fi, LOGL_NOTICE, "Rx BVC-BLOCK (cause=%s)\n", bssgp_cause_str(cause)); + /* If a BVC-BLOCK PDU is received by an SGSN for a blocked BVC, a BVC-BLOCK-ACK + * PDU shall be returned. */ + if (bfp->role_sgsn) { + /* If a BVC-BLOCK PDU is received by an SGSN for + * the signalling BVC, the PDU is ignored */ + if (bfp->bvci == 0) + break; + tx = bssgp2_enc_bvc_block_ack(bfp->bvci); + fi_tx_sig(fi, tx); + } + break; + case BSSGP_BVCFSM_E_RX_UNBLOCK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + LOGPFSML(fi, LOGL_NOTICE, "Rx BVC-UNBLOCK\n"); + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-UNBLOCK on BVCI=0 is illegal\n"); + /* If BVC-UNBLOCK PDU is received by an SGSN for the signalling BVC, the PDU is ignored.*/ + if (bfp->role_sgsn) + break; + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + if (!bfp->role_sgsn) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-UNBLOCK on BSS is illegal\n"); + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + tx = bssgp2_enc_bvc_unblock_ack(bfp->bvci); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, T1_SECS, T1); + break; + case BSSGP_BVCFSM_E_REQ_UNBLOCK: + if (bfp->role_sgsn) { + LOGPFSML(fi, LOGL_ERROR, "SGSN side cannot initiate BVC unblock\n"); + break; + } + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "BVCI 0 cannot be unblocked\n"); + break; + } + bfp->locally_blocked = false; + tx = bssgp2_enc_bvc_unblock(bfp->bvci); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, 0, 0); + break; + } +} + +/* Waiting for RESET-ACK: Receive PDUs but don't transmit */ +static void bssgp_bvc_fsm_wait_reset_ack(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct bvc_fsm_priv *bfp = fi->priv; + const struct tlv_parsed *tp = NULL; + struct msgb *rx = NULL, *tx; + uint8_t cause; + + switch (event) { + case BSSGP_BVCFSM_E_RX_RESET: + /* 48.018 Section 8.4.3: If the BSS (or SGSN) has sent a BVC-RESET PDU for a BVCI to + * the SGSN (or BSS) and is awaiting a BVC-RESET-ACK PDU in response, but instead + * receives a BVC-RESET PDU indicating the same BVCI, then this shall be interpreted + * as a BVC-RESET ACK PDU and the T2 timer shall be stopped. */ + /* fall-through */ + case BSSGP_BVCFSM_E_RX_RESET_ACK: + rx = data; + cause = bfp->last_reset_cause; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + if (bfp->bvci == 0) + update_negotiated_features(fi, tp); + if (bfp->role_sgsn && bfp->bvci != 0) + bfp->cell_id = bssgp_parse_cell_id(&bfp->ra_id, TLVP_VAL(tp, BSSGP_IE_CELL_ID)); + if (!bfp->role_sgsn && bfp->bvci != 0 && bfp->locally_blocked) { + /* initiate the blocking procedure */ + /* transmit BVC-BLOCK, transition to BLOCKED state and start re-transmit timer */ + tx = bssgp2_enc_bvc_block(bfp->bvci, bfp->block_cause); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_BLOCKED, T1_SECS, T1); + } else + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, 0, 0); + if (bfp->ops && bfp->ops->reset_ack_notification) + bfp->ops->reset_ack_notification(bfp->nsei, bfp->bvci, &bfp->ra_id, bfp->cell_id, cause, bfp->ops_priv); + break; + } +} + +static void bssgp_bvc_fsm_unblocked(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct bssgp2_flow_ctrl rx_fc, *tx_fc; + struct bvc_fsm_priv *bfp = fi->priv; + const struct tlv_parsed *tp = NULL; + struct msgb *rx = NULL, *tx; + int rc; + + switch (event) { + case BSSGP_BVCFSM_E_RX_UNBLOCK_ACK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + /* If BVC-UNBLOCK-ACK PDU is received by an BSS for the signalling BVC, the PDU is ignored. */ + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-UNBLOCK-ACK on BVCI=0 is illegal\n"); + if (!bfp->role_sgsn) + break; + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + /* stop T1 timer */ + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, 0, 0); + break; + case BSSGP_BVCFSM_E_RX_UNBLOCK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + /* If a BVC-UNBLOCK PDU is received by an SGSN for a blocked BVC, a BVC-UNBLOCK-ACK + * PDU shall be returned. */ + if (bfp->role_sgsn) { + /* If a BVC-UNBLOCK PDU is received by an SGSN for + * the signalling BVC, the PDU is ignored */ + if (bfp->bvci == 0) + break; + bssgp_tx_simple_bvci(BSSGP_PDUT_BVC_UNBLOCK_ACK, bfp->nsei, bfp->bvci, 0); + } + break; + case BSSGP_BVCFSM_E_RX_BLOCK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + LOGPFSML(fi, LOGL_NOTICE, "Rx BVC-BLOCK (cause=%s)\n", + bssgp_cause_str(*TLVP_VAL(tp, BSSGP_IE_CAUSE))); + /* If a BVC-BLOCK PDU is received by an SGSN for the signalling BVC, the PDU is ignored */ + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-BLOCK on BVCI=0 is illegal\n"); + if (bfp->role_sgsn) + break; + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + if (!bfp->role_sgsn) { + LOGPFSML(fi, LOGL_ERROR, "Rx BVC-BLOCK on BSS is illegal\n"); + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + /* transmit BVC-BLOCK-ACK, transition to BLOCKED state */ + tx = bssgp2_enc_bvc_block_ack(bfp->bvci); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_BLOCKED, 0, 0); + break; + case BSSGP_BVCFSM_E_REQ_BLOCK: + if (bfp->role_sgsn) { + LOGPFSML(fi, LOGL_ERROR, "SGSN may not initiate BVC-BLOCK\n"); + break; + } + if (bfp->bvci == 0) { + LOGPFSML(fi, LOGL_ERROR, "BVCI 0 cannot be blocked\n"); + break; + } + bfp->locally_blocked = true; + bfp->block_cause = *(uint8_t *)data; + /* transmit BVC-BLOCK, transition to BLOCKED state and start re-transmit timer */ + tx = bssgp2_enc_bvc_block(bfp->bvci, bfp->block_cause); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_BLOCKED, T1_SECS, T1); + break; + case BSSGP_BVCFSM_E_RX_FC_BVC: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + /* we assume osmo_tlv_prot_* has been used before calling here to ensure this */ + OSMO_ASSERT(bfp->role_sgsn); + rc = bssgp2_dec_fc_bvc(&rx_fc, tp); + if (rc < 0) { + _tx_status(fi, BSSGP_CAUSE_SEM_INCORR_PDU, rx); + break; + } + if (bfp->ops->rx_fc_bvc) + bfp->ops->rx_fc_bvc(bfp->nsei, bfp->bvci, &rx_fc, bfp->ops_priv); + tx = bssgp2_enc_fc_bvc_ack(rx_fc.tag); + fi_tx_ptp(fi, tx); + break; + case BSSGP_BVCFSM_E_RX_FC_BVC_ACK: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + /* we assume osmo_tlv_prot_* has been used before calling here to ensure this */ + OSMO_ASSERT(!bfp->role_sgsn); + break; + case BSSGP_BVCFSM_E_REQ_FC_BVC: + tx_fc = data; + tx = bssgp2_enc_fc_bvc(tx_fc, bfp->features.negotiated & (BSSGP_XFEAT_GBIT << 8) ? + &bfp->features.fc_granularity : NULL); + fi_tx_ptp(fi, tx); + break; + } +} + +static void bssgp_bvc_fsm_allstate(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct bvc_fsm_priv *bfp = fi->priv; + uint8_t cause; + const struct tlv_parsed *tp = NULL; + struct msgb *rx = NULL; + + switch (event) { + case BSSGP_BVCFSM_E_REQ_RESET: + bfp->locally_blocked = false; + cause = bfp->last_reset_cause = *(uint8_t *) data; + _tx_bvc_reset(fi, cause); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_WAIT_RESET_ACK, T2_SECS, T2); +#if 0 /* not sure if we really should notify the application if itself has requested the reset? */ + if (bfp->ops && bfp->ops->reset_notification) + bfp->ops->reset_notification(bfp->nsei, bfp->bvci, NULL, 0, cause, bfp->ops_priv); +#endif + break; + case BSSGP_BVCFSM_E_RX_RESET: + rx = data; + tp = (const struct tlv_parsed *) msgb_bcid(rx); + cause = *TLVP_VAL(tp, BSSGP_IE_CAUSE); + if (bfp->role_sgsn && bfp->bvci != 0) + bfp->cell_id = bssgp_parse_cell_id(&bfp->ra_id, TLVP_VAL(tp, BSSGP_IE_CELL_ID)); + LOGPFSML(fi, LOGL_NOTICE, "Rx BVC-RESET (cause=%s)\n", bssgp_cause_str(cause)); + if (bfp->bvci == 0) + update_negotiated_features(fi, tp); + _tx_bvc_reset_ack(fi); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, 0, 0); + if (bfp->ops && bfp->ops->reset_notification) { + bfp->ops->reset_notification(bfp->nsei, bfp->bvci, &bfp->ra_id, bfp->cell_id, + cause, bfp->ops_priv); + } + break; + } +} + +static int bssgp_bvc_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + struct msgb *tx; + + switch (fi->T) { + case T1: + switch (fi->state) { + case BSSGP_BVCFSM_S_BLOCKED: + /* re-transmit BVC-BLOCK */ + tx = bssgp2_enc_bvc_block(bfp->bvci, bfp->block_cause); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_BLOCKED, T1_SECS, T1); + break; + case BSSGP_BVCFSM_S_UNBLOCKED: + /* re-transmit BVC-UNBLOCK */ + tx = bssgp2_enc_bvc_unblock(bfp->bvci); + fi_tx_sig(fi, tx); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, T1_SECS, T1); + break; + } + break; + case T2: + switch (fi->state) { + case BSSGP_BVCFSM_S_WAIT_RESET_ACK: + /* re-transmit BVC-RESET */ + _tx_bvc_reset(fi, bfp->last_reset_cause); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_WAIT_RESET_ACK, T2_SECS, T2); + break; + case BSSGP_BVCFSM_S_UNBLOCKED: + /* re-transmit BVC-RESET-ACK */ + _tx_bvc_reset_ack(fi); + osmo_fsm_inst_state_chg(fi, BSSGP_BVCFSM_S_UNBLOCKED, T2_SECS, T2); + break; + } + break; + default: + OSMO_ASSERT(0); + break; + } + return 0; +} + + + +static const struct osmo_fsm_state bssgp_bvc_fsm_states[] = { + [BSSGP_BVCFSM_S_NULL] = { + /* initial state from which we must do a RESET */ + .name = "NULL", + .in_event_mask = 0, + .out_state_mask = S(BSSGP_BVCFSM_S_WAIT_RESET_ACK) | + S(BSSGP_BVCFSM_S_UNBLOCKED), + .action = bssgp_bvc_fsm_null, + }, + [BSSGP_BVCFSM_S_BLOCKED] = { + .name = "BLOCKED", + .in_event_mask = S(BSSGP_BVCFSM_E_RX_UNBLOCK) | + S(BSSGP_BVCFSM_E_RX_BLOCK) | + S(BSSGP_BVCFSM_E_RX_BLOCK_ACK) | + S(BSSGP_BVCFSM_E_REQ_UNBLOCK), + .out_state_mask = S(BSSGP_BVCFSM_S_WAIT_RESET_ACK) | + S(BSSGP_BVCFSM_S_UNBLOCKED) | + S(BSSGP_BVCFSM_S_BLOCKED), + .action = bssgp_bvc_fsm_blocked, + .onenter = bssgp_bvc_fsm_blocked_onenter, + }, + [BSSGP_BVCFSM_S_WAIT_RESET_ACK]= { + .name = "WAIT_RESET_ACK", + .in_event_mask = S(BSSGP_BVCFSM_E_RX_RESET_ACK) | + S(BSSGP_BVCFSM_E_RX_RESET), + .out_state_mask = S(BSSGP_BVCFSM_S_UNBLOCKED) | + S(BSSGP_BVCFSM_S_BLOCKED) | + S(BSSGP_BVCFSM_S_WAIT_RESET_ACK), + .action = bssgp_bvc_fsm_wait_reset_ack, + .onenter = _onenter_tail, + }, + + [BSSGP_BVCFSM_S_UNBLOCKED] = { + .name = "UNBLOCKED", + .in_event_mask = S(BSSGP_BVCFSM_E_RX_BLOCK) | + S(BSSGP_BVCFSM_E_RX_UNBLOCK) | + S(BSSGP_BVCFSM_E_RX_UNBLOCK_ACK) | + S(BSSGP_BVCFSM_E_REQ_BLOCK) | + S(BSSGP_BVCFSM_E_RX_FC_BVC) | + S(BSSGP_BVCFSM_E_RX_FC_BVC_ACK) | + S(BSSGP_BVCFSM_E_REQ_FC_BVC), + .out_state_mask = S(BSSGP_BVCFSM_S_BLOCKED) | + S(BSSGP_BVCFSM_S_WAIT_RESET_ACK) | + S(BSSGP_BVCFSM_S_UNBLOCKED), + .action = bssgp_bvc_fsm_unblocked, + .onenter = _onenter_tail, + }, +}; + +static struct osmo_fsm bssgp_bvc_fsm = { + .name = "BSSGP-BVC", + .states = bssgp_bvc_fsm_states, + .num_states = ARRAY_SIZE(bssgp_bvc_fsm_states), + .allstate_event_mask = S(BSSGP_BVCFSM_E_REQ_RESET) | + S(BSSGP_BVCFSM_E_RX_RESET), + .allstate_action = bssgp_bvc_fsm_allstate, + .timer_cb = bssgp_bvc_fsm_timer_cb, + .log_subsys = DLBSSGP, + .event_names = ptp_bvc_event_names, +}; + +static struct osmo_fsm_inst * +_bvc_fsm_alloc(void *ctx, struct gprs_ns2_inst *nsi, bool role_sgsn, uint16_t nsei, uint16_t bvci) +{ + struct osmo_fsm_inst *fi; + struct bvc_fsm_priv *bfp; + char idbuf[64]; + + /* TODO: encode our role in the id string? */ + snprintf(idbuf, sizeof(idbuf), "NSE%05u-BVC%05u", nsei, bvci); + + fi = osmo_fsm_inst_alloc(&bssgp_bvc_fsm, ctx, NULL, LOGL_INFO, idbuf); + if (!fi) + return NULL; + + bfp = talloc_zero(fi, struct bvc_fsm_priv); + if (!bfp) { + osmo_fsm_inst_free(fi); + return NULL; + } + fi->priv = bfp; + + bfp->nsi = nsi; + bfp->role_sgsn = role_sgsn; + bfp->nsei = nsei; + bfp->bvci = bvci; + bfp->max_pdu_len = UINT16_MAX; + + return fi; +} + +/*! Allocate a SIGNALING-BVC FSM for the BSS role (facing a remote SGSN). + * \param[in] ctx talloc context from which to allocate + * \param[in] nsi NS Instance on which this BVC operates + * \param[in] nsei NS Entity Identifier on which this BVC operates + * \param[in] features Feature [byte 0] and Extended Feature [byte 1] bitmap + * \returns newly-allocated FSM Instance; NULL in case of error */ +struct osmo_fsm_inst * +bssgp_bvc_fsm_alloc_sig_bss(void *ctx, struct gprs_ns2_inst *nsi, uint16_t nsei, uint32_t features) +{ + struct osmo_fsm_inst *fi = _bvc_fsm_alloc(ctx, nsi, false, nsei, 0); + struct bvc_fsm_priv *bfp; + + if (!fi) + return NULL; + + bfp = fi->priv; + bfp->features.advertised = features; + + return fi; +} + +/*! Allocate a PTP-BVC FSM for the BSS role (facing a remote SGSN). + * \param[in] ctx talloc context from which to allocate + * \param[in] nsi NS Instance on which this BVC operates + * \param[in] nsei NS Entity Identifier on which this BVC operates + * \param[in] bvci BVCI of this FSM + * \param[in] ra_id Routing Area Identity of the cell (reported to SGSN) + * \param[in] cell_id Cell Identifier of the cell (reported to SGSN) + * \returns newly-allocated FSM Instance; NULL in case of error */ +struct osmo_fsm_inst * +bssgp_bvc_fsm_alloc_ptp_bss(void *ctx, struct gprs_ns2_inst *nsi, uint16_t nsei, + uint16_t bvci, const struct gprs_ra_id *ra_id, uint16_t cell_id) +{ + struct osmo_fsm_inst *fi; + struct bvc_fsm_priv *bfp; + + OSMO_ASSERT(bvci >= 2); + OSMO_ASSERT(ra_id); + + fi = _bvc_fsm_alloc(ctx, nsi, false, nsei, bvci); + if (!fi) + return NULL; + + bfp = fi->priv; + bfp->ra_id = *ra_id; + bfp->cell_id = cell_id; + + return fi; +} + +/*! Allocate a SIGNALING-BVC FSM for the SGSN role (facing a remote BSS). + * \param[in] ctx talloc context from which to allocate + * \param[in] nsi NS Instance on which this BVC operates + * \param[in] nsei NS Entity Identifier on which this BVC operates + * \param[in] features Feature [byte 0] and Extended Feature [byte 1] bitmap + * \returns newly-allocated FSM Instance; NULL in case of error */ +struct osmo_fsm_inst * +bssgp_bvc_fsm_alloc_sig_sgsn(void *ctx, struct gprs_ns2_inst *nsi, uint16_t nsei, uint32_t features) +{ + struct osmo_fsm_inst *fi = _bvc_fsm_alloc(ctx, nsi, true, nsei, 0); + struct bvc_fsm_priv *bfp; + + if (!fi) + return NULL; + + bfp = fi->priv; + bfp->features.advertised = features; + + return fi; +} + +/*! Allocate a PTP-BVC FSM for the SGSN role (facing a remote BSS). + * \param[in] ctx talloc context from which to allocate + * \param[in] nsi NS Instance on which this BVC operates + * \param[in] nsei NS Entity Identifier on which this BVC operates + * \param[in] bvci BVCI of this FSM + * \returns newly-allocated FSM Instance; NULL in case of error */ +struct osmo_fsm_inst * +bssgp_bvc_fsm_alloc_ptp_sgsn(void *ctx, struct gprs_ns2_inst *nsi, uint16_t nsei, uint16_t bvci) +{ + struct osmo_fsm_inst *fi; + + OSMO_ASSERT(bvci >= 2); + + fi = _bvc_fsm_alloc(ctx, nsi, true, nsei, bvci); + if (!fi) + return NULL; + + return fi; +} + +/*! Set the 'operations' callbacks + private data. + * \param[in] fi FSM instance for which the data shall be set + * \param[in] ops BSSGP BVC FSM operations (call-back functions) to register + * \param[in] ops_priv opaque/private data pointer passed through to call-backs */ +void bssgp_bvc_fsm_set_ops(struct osmo_fsm_inst *fi, const struct bssgp_bvc_fsm_ops *ops, void *ops_priv) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + + bfp->ops = ops; + bfp->ops_priv = ops_priv; +} + +/*! Return if the given BVC FSM is in UNBLOCKED state. */ +bool bssgp_bvc_fsm_is_unblocked(struct osmo_fsm_inst *fi) +{ + return fi->state == BSSGP_BVCFSM_S_UNBLOCKED; +} + +/*! Determine the cause value why given BVC FSM is blocked. */ +uint8_t bssgp_bvc_fsm_get_block_cause(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + return bfp->block_cause; +} + +/*! Return the advertised features / extended features. */ +uint32_t bssgp_bvc_fsm_get_features_advertised(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + return bfp->features.advertised; +} + +/*! Return the received features / extended features. */ +uint32_t bssgp_bvc_fsm_get_features_received(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + return bfp->features.received; +} + +/*! Return the negotiated features / extended features. */ +uint32_t bssgp_bvc_fsm_get_features_negotiated(struct osmo_fsm_inst *fi) +{ + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + return bfp->features.negotiated; +} + +/*! Set the maximum size of a BSSGP PDU. + *! On the NS layer this corresponds to the size of an NS SDU in NS-UNITDATA (3GPP TS 48.016 Ch. 9.2.10) */ +void bssgp_bvc_fsm_set_max_pdu_len(struct osmo_fsm_inst *fi, uint16_t max_pdu_len) { + struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + bfp->max_pdu_len = max_pdu_len; +} + +/*! Return the maximum size of a BSSGP PDU + *! On the NS layer this corresponds to the size of an NS SDU in NS-UNITDATA (3GPP TS 48.016 Ch. 9.2.10) */ +uint16_t bssgp_bvc_fsm_get_max_pdu_len(const struct osmo_fsm_inst *fi) +{ + const struct bvc_fsm_priv *bfp = fi->priv; + + OSMO_ASSERT(fi->fsm == &bssgp_bvc_fsm); + return bfp->max_pdu_len; +} + + +static __attribute__((constructor)) void on_dso_load_bvc_fsm(void) +{ + OSMO_ASSERT(osmo_fsm_register(&bssgp_bvc_fsm) == 0); +} diff --git a/src/gb/common_vty.c b/src/gb/common_vty.c index a47294b5..ad3dea23 100644 --- a/src/gb/common_vty.c +++ b/src/gb/common_vty.c @@ -40,15 +40,21 @@ int gprs_log_filter_fn(const struct log_context *ctx, struct log_target *tar) { - const struct gprs_nsvc *nsvc = ctx->ctx[LOG_CTX_GB_NSVC]; - const struct gprs_bvc *bvc = ctx->ctx[LOG_CTX_GB_BVC]; + const void *nse = ctx->ctx[LOG_CTX_GB_NSE]; + const void *nsvc = ctx->ctx[LOG_CTX_GB_NSVC]; + const void *bvc = ctx->ctx[LOG_CTX_GB_BVC]; + + /* Filter on the NS Entity */ + if ((tar->filter_map & (1 << LOG_FLT_GB_NSE)) != 0 + && nse && (nse == tar->filter_data[LOG_FLT_GB_NSE])) + return 1; /* Filter on the NS Virtual Connection */ if ((tar->filter_map & (1 << LOG_FLT_GB_NSVC)) != 0 && nsvc && (nsvc == tar->filter_data[LOG_FLT_GB_NSVC])) return 1; - /* Filter on the NS Virtual Connection */ + /* Filter on the BSSGP Virtual Connection */ if ((tar->filter_map & (1 << LOG_FLT_GB_BVC)) != 0 && bvc && (bvc == tar->filter_data[LOG_FLT_GB_BVC])) return 1; @@ -57,4 +63,4 @@ int gprs_log_filter_fn(const struct log_context *ctx, } -int DNS, DBSSGP; +int DNS; diff --git a/src/gb/common_vty.h b/src/gb/common_vty.h index 801d2dab..8e883319 100644 --- a/src/gb/common_vty.h +++ b/src/gb/common_vty.h @@ -3,5 +3,5 @@ #include <osmocom/vty/command.h> #include <osmocom/core/logging.h> -extern int DNS, DBSSGP; +extern int DNS; diff --git a/src/gb/frame_relay.c b/src/gb/frame_relay.c new file mode 100644 index 00000000..e973b915 --- /dev/null +++ b/src/gb/frame_relay.c @@ -0,0 +1,1051 @@ +/*! \file frame_relay.c + * Implement frame relay/PVC by Q.933 + */ +/* (C) 2020 Harald Welte <laforge@gnumonks.org> + * (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <stdint.h> +#include <stdbool.h> +#include <unistd.h> +#include <errno.h> + +#include <osmocom/gprs/frame_relay.h> +#include <osmocom/core/endian.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/tdef.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/msgb.h> + +#include <osmocom/gsm/tlv.h> + +#define LOGPFRL(frl, lvl, fmt, args ...) \ + LOGP(DFR, lvl, "%s: " fmt, (frl)->name, ## args) + +#define DFR DLNS + +/* Table 4-2/Q.931 */ +enum q931_msgtype { + /* Call establishment message */ + Q931_MSGT_ALERTING = 0x01, + Q931_MSGT_CALL_PROCEEDING = 0x02, + Q931_MSGT_CONNECT = 0x07, + Q931_MSGT_CONNECT_ACK = 0x0f, + Q931_MSGT_PROGRESS = 0x03, + Q931_MSGT_SETUP = 0x05, + Q931_MSGT_SETUP_ACK = 0x0d, + /* Call information phase message */ + Q931_MSGT_RESUME = 0x26, + Q931_MSGT_RESUME_ACK = 0x2e, + Q931_MSGT_RESUME_REJ = 0x22, + Q931_MSGT_SUSPEND = 0x25, + Q931_MSGT_SUSPEND_ACK = 0x2d, + Q931_MSGT_USER_INFO = 0x20, + /* Call clearing message */ + Q931_MSGT_DISCONNECT = 0x45, + Q931_MSGT_RELEASE = 0x4d, + Q931_MSGT_RELEASE_COMPLETE = 0x5a, + Q931_MSGT_RESTART = 0x46, + Q931_MSGT_RESTART_ACK = 0x4e, + /* Miscellaneous messages */ + Q931_MSGT_SEGMENT = 0x60, + Q931_MSGT_CONGESTION_CONTROL = 0x79, + Q931_MSGT_IFORMATION = 0x7b, + Q931_MSGT_NOTIFY = 0x6e, + Q931_MSGT_STATUS = 0x7d, + Q931_MSGT_STATUS_ENQUIRY = 0x75, +}; + + +/* Figure A.1/Q.933 Report type information element */ +enum q933_type_of_report { + Q933_REPT_FULL_STATUS = 0x00, + Q933_REPT_LINK_INTEGRITY_VERIF = 0x01, + Q933_REPT_SINGLE_PVC_ASYNC_STS = 0x02, +}; + +/* Q.933 Section A.3 */ +enum q933_iei { + Q933_IEI_REPORT_TYPE = 0x51, + Q933_IEI_LINK_INT_VERIF = 0x53, + Q933_IEI_PVC_STATUS = 0x57, +}; + +/* Q.933 Section A.3.3 */ +enum q933_pvc_status { + Q933_PVC_STATUS_DLC_ACTIVE = 0x02, + Q933_PVC_STATUS_DLC_DELETE = 0x04, + Q933_PVC_STATUS_DLC_NEW = 0x08, +}; + + + +#define LAPF_UI 0x03 /* UI control word */ +#define Q931_PDISC_CC 0x08 /* protocol discriminator */ +#define LMI_Q933A_CALLREF 0x00 /* NULL call-ref */ + +/* LMI DLCI values */ +#define LMI_Q933A_DLCI 0 /* Q.933A DLCI */ +#define LMI_CISCO_DLCI 1023 /* Cisco DLCI */ + +/* maximum of supported */ +#define MAX_SUPPORTED_PVC 10 + +/* TODO: add counters since good connection */ + +/* Message header of the L3 payload of a Q.933 Annex A message */ +struct q933_a_hdr { + uint8_t prot_disc; + uint8_t call_ref; + uint8_t msg_type; +} __attribute__((packed)); + +/* Value part of the Q.933 Annex A.3.3 IE */ +struct q933_a_pvc_sts { +#if OSMO_IS_LITTLE_ENDIAN + uint8_t dlci_msb:6, + spare:1, + ext0:1; + uint8_t space1:3, + dlci_lsb:4, + ext1:1; + uint8_t reserved:1, + active:1, + delete:1, + new:1, + spare2:3, + ext2:1; + +#elif OSMO_IS_BIG_ENDIAN +/* auto-generated from the little endian part above (libosmocore/contrib/struct_endianness.py) */ + uint8_t ext0:1, spare:1, dlci_msb:6; + uint8_t ext1:1, dlci_lsb:4, space1:3; + uint8_t ext2:1, spare2:3, new:1, delete:1, active:1, reserved:1; +#endif +} __attribute__((packed)); + +/* RX Message: 14 [ 00 01 03 08 00 75 95 01 01 00 03 02 01 00 ] */ +/* RX Message: 13 [ 00 01 03 08 00 75 51 01 00 53 02 01 00 ] */ + +const struct value_string osmo_fr_role_names[] = { + { FR_ROLE_USER_EQUIPMENT, "USER" }, + { FR_ROLE_NETWORK_EQUIPMENT, "NETWORK" }, + { 0, NULL } +}; + +/* Table A.4/Q.933 */ +struct osmo_tdef fr_tdefs[] = { + { + .T=391, + .default_val = 10, + .min_val = 5, + .max_val = 30, + .desc = "Link integrity verification polling timer", + .unit = OSMO_TDEF_S, + }, { + .T=392, + .default_val = 15, + .min_val = 5, + .max_val = 30, + .desc = "Polling verification timer", + .unit = OSMO_TDEF_S, + }, + {} +}; + +static const struct tlv_definition q933_att_tlvdef = { + .def = { + [Q933_IEI_REPORT_TYPE] = { TLV_TYPE_TLV }, + [Q933_IEI_LINK_INT_VERIF] = { TLV_TYPE_TLV }, + [Q933_IEI_PVC_STATUS] = { TLV_TYPE_TLV }, + }, +}; + +static void check_link_state(struct osmo_fr_link *link, bool valid); + +static inline uint16_t q922_to_dlci(const uint8_t *hdr) +{ + return ((hdr[0] & 0xFC) << 2) | ((hdr[1] & 0xF0) >> 4); +} + + +static inline void dlci_to_q922(uint8_t *hdr, uint16_t dlci) +{ + hdr[0] = (dlci >> 2) & 0xFC; + hdr[1] = ((dlci << 4) & 0xF0) | 0x01; +} + +static void dlc_set_active(struct osmo_fr_dlc *dlc, bool active) +{ + if (active == dlc->active) + return; + + dlc->active = active; + + LOGPFRL(dlc->link, LOGL_NOTICE, "DLCI %u became %s\n", dlc->dlci, active ? "active" : "inactive"); + if (dlc->status_cb) + dlc->status_cb(dlc, dlc->cb_data, active); +} + +/* allocate a message buffer and put Q.933 Annex A headers (L2 + L3) */ +static struct msgb *q933_msgb_alloc(uint16_t dlci, uint8_t prot_disc, uint8_t msg_type) +{ + struct msgb *msg = msgb_alloc_headroom(1600+64, 64, "FR Q.933 Tx"); + struct q933_a_hdr *qh; + + if (!msg) + return NULL; + + msg->l1h = msgb_put(msg, 2); + dlci_to_q922(msg->l1h, dlci); + + /* LAPF UI control */ + msg->l2h = msgb_put(msg, 1); + *msg->l2h = LAPF_UI; + + msg->l3h = msgb_put(msg, sizeof(*qh)); + qh = (struct q933_a_hdr *) msg->l3h; + qh->prot_disc = prot_disc; + qh->call_ref = LMI_Q933A_CALLREF; + qh->msg_type = msg_type; + + return msg; +} + +/* obtain the [next] transmit sequence number */ +static uint8_t link_get_tx_seq(struct osmo_fr_link *link) +{ + /* The {user equipment, network} increments the send sequence + * counter using modulo 256. The value zero is skipped. */ + link->last_tx_seq++; + if (link->last_tx_seq == 0) + link->last_tx_seq++; + + return link->last_tx_seq; +} + +/* Append PVC Status IE according to Q.933 A.3.2 */ +static void msgb_put_link_int_verif(struct msgb *msg, struct osmo_fr_link *link) +{ + uint8_t link_int_tx[2]; + link_int_tx[0] = link_get_tx_seq(link); + link_int_tx[1] = link->last_rx_seq; + msgb_tlv_put(msg, Q933_IEI_LINK_INT_VERIF, 2, link_int_tx); +} + +static void dlc_destroy(struct osmo_fr_dlc *dlc) +{ + llist_del(&dlc->list); + talloc_free(dlc); +} + +/* Append PVC Status IE according to Q.933 A.3.3 */ +static void msgb_put_pvc_status(struct msgb *msg, struct osmo_fr_dlc *dlc) +{ + uint8_t ie[3]; + + ie[0] = (dlc->dlci >> 4) & 0x3f; + /* extension bits */ + ie[1] = 0x80 | ((dlc->dlci & 0xf) << 3); + /* extension bits */ + ie[2] = 0x80; + + /* FIXME: validate: this status should be added as long it's not yet acked by the remote */ + if (dlc->active) + ie[2] |= Q933_PVC_STATUS_DLC_ACTIVE; + + if (dlc->add) { + ie[2] |= Q933_PVC_STATUS_DLC_NEW; + /* we've reported it as new once, reset the status */ + } + + if (dlc->del) { + ie[2] |= Q933_PVC_STATUS_DLC_DELETE; + /* we've reported it as deleted once, destroy it */ + dlc_destroy(dlc); + } + + msgb_tlv_put(msg, Q933_IEI_PVC_STATUS, 3, ie); +} + +/* Send a Q.933 STATUS ENQUIRY given type over given link */ +static int tx_lmi_q933_status_enq(struct osmo_fr_link *link, uint8_t rep_type) +{ + struct msgb *resp; + + resp = q933_msgb_alloc(0, Q931_PDISC_CC, Q931_MSGT_STATUS_ENQUIRY); + if (!resp) + return -1; + resp->dst = link; + link->expected_rep = rep_type; + + /* Table A.2/Q.933 */ + msgb_tlv_put(resp, Q933_IEI_REPORT_TYPE, 1, &rep_type); + msgb_put_link_int_verif(resp, link); + + return link->tx_cb(link->cb_data, resp); +} + +/* Send a Q.933 STATUS of given type over given link */ +static int tx_lmi_q933_status(struct osmo_fr_link *link, uint8_t rep_type) +{ + struct osmo_fr_dlc *dlc; + struct msgb *resp; + + resp = q933_msgb_alloc(0, Q931_PDISC_CC, Q931_MSGT_STATUS); + if (!resp) + return -1; + + resp->dst = link; + + /* Table A.1/Q.933 */ + msgb_tlv_put(resp, Q933_IEI_REPORT_TYPE, 1, &rep_type); + switch (rep_type) { + case Q933_REPT_FULL_STATUS: + msgb_put_link_int_verif(resp, link); + llist_for_each_entry(dlc, &link->dlc_list, list) { + if (dlc->add || dlc->del) + dlc->state_send = true; + + msgb_put_pvc_status(resp, dlc); + } + break; + case Q933_REPT_LINK_INTEGRITY_VERIF: + msgb_put_link_int_verif(resp, link); + llist_for_each_entry(dlc, &link->dlc_list, list) { + if (dlc->add || dlc->del) { + msgb_put_pvc_status(resp, dlc); + dlc->state_send = true; + } + } + break; + case Q933_REPT_SINGLE_PVC_ASYNC_STS: + llist_for_each_entry(dlc, &link->dlc_list, list) + msgb_put_pvc_status(resp, dlc); + break; + } + + return link->tx_cb(link->cb_data, resp); +} + + +static void link_set_failed(struct osmo_fr_link *link) +{ + struct osmo_fr_dlc *dlc; + + LOGPFRL(link, LOGL_NOTICE, "Link failed\n"); + link->state = false; + if (link->status_cb) + link->status_cb(link, link->cb_data, link->state); + + llist_for_each_entry(dlc, &link->dlc_list, list) { + dlc_set_active(dlc, false); + } +} + +/* Q.933 */ +static int rx_lmi_q933_status_enq(struct msgb *msg, struct tlv_parsed *tp) +{ + struct osmo_fr_link *link = msg->dst; + struct osmo_fr_dlc *dlc; + const uint8_t *link_int_rx; + uint8_t rep_type; + + OSMO_ASSERT(link); + + if (link->role == FR_ROLE_USER_EQUIPMENT) { + LOGPFRL(link, LOGL_ERROR, "STATUS-ENQ aren't supported in role user\n"); + return -1; + } + + /* check for mandatory IEs */ + if (!TLVP_PRES_LEN(tp, Q933_IEI_REPORT_TYPE, 1) || + !TLVP_PRES_LEN(tp, Q933_IEI_LINK_INT_VERIF, 2)) + return -1; + + rep_type = *TLVP_VAL(tp, Q933_IEI_REPORT_TYPE); + + link_int_rx = TLVP_VAL(tp, Q933_IEI_LINK_INT_VERIF); + link->last_rx_seq = link_int_rx[0]; + + /* this is a bit of a hack. Q.933 explicitly forbids either side from ever + * sending a sequence number of '0'. Values start from '1' and are modulo 256, + * but '0' is always skipped. So if the peer is sending us a "last received + * sequence number of '0' it means it has not yet received any packets from us, + * which in turn can only mean that it has just been restarted. Let's treat + * this as "service affecting condition" and notify upper layers. This helps + * particularly in recovering from rapidly re-starting peers, where the Q.933 + * nor NS have time to actually detect the connection was lost. Se OS#4974 */ + if (link_int_rx[1] == 0) { + link_set_failed(link); + /* the network checks the receive sequence number received from + * the user equipment against its send sequence counter */ + } else if (link_int_rx[1] != link->last_tx_seq) { + check_link_state(link, false); + link->err_count++; + } else { + check_link_state(link, true); + /* confirm DLC state changes */ + llist_for_each_entry(dlc, &link->dlc_list, list) { + if (!dlc->state_send) + continue; + + if (dlc->add) { + dlc_set_active(dlc, link->state); + dlc->add = false; + } + + if (dlc->del) { + dlc->del = false; + } + + dlc->state_send = false; + } + } + + + /* The network responds to each STATUS ENQUIRY message with a + * STATUS message and resets the T392 timer */ + osmo_timer_schedule(&link->t392, osmo_tdef_get(link->net->T_defs, 392, OSMO_TDEF_S, 15), 0); + + return tx_lmi_q933_status(link, rep_type); +} + +/* check if the link become active. + * The link becomes active when enough times a STATUS/STATUS ENQUIRY arrives without any loss. + * Look at the last N393 STATUS/STATUS ENQUIRY PDUs. The link is valid if at least N392 + * got received. + * param[in] valid contains the status of the last packet */ +static void check_link_state(struct osmo_fr_link *link, bool valid) +{ + unsigned int last, i; + unsigned int carry = 0; + struct osmo_fr_dlc *dlc; + + link->succeed <<= 1; + if (valid) + link->succeed |= 1; + + /* count the bits */ + last = link->succeed & ((1 << link->net->n393) - 1); + for (i = 0; i < link->net->n393; i++) + if (last & (1 << i)) + carry++; + + if (link->net->n393 - carry >= link->net->n392) { + /* failing link */ + if (!link->state) + return; + + link_set_failed(link); + } else { + /* good link */ + if (link->state) + return; + + LOGPFRL(link, LOGL_NOTICE, "Link recovered\n"); + link->state = true; + if (link->status_cb) + link->status_cb(link, link->cb_data, link->state); + + if (link->role == FR_ROLE_USER_EQUIPMENT) { + /* make sure the next STATUS ENQUIRY is for a full + * status report to get the configred DLCs ASAP */ + link->polling_count = 0; + /* we must not proceed further below if we're in user role, + * as otherwise link recovery would set all DLCs as active */ + return; + } + + llist_for_each_entry(dlc, &link->dlc_list, list) { + if (!dlc->add && !dlc->del) + dlc_set_active(dlc, true); + } + } +} + +static int validate_pvc_status(struct tlv_parsed *tp, size_t tp_len) +{ + size_t i; + uint16_t len = 0; + + for (i = 0; i < tp_len; i++) { + if (!TLVP_PRESENT(&tp[i], Q933_IEI_PVC_STATUS)) + continue; + + /* PVC status can be 2 or 3 bytes. If the PVC is bigger + * ignore this to be compatible to future extensions. */ + len = TLVP_LEN(&tp[i], Q933_IEI_PVC_STATUS); + if (len <= 1) { + return -EINVAL; + } + /* FIXME: validate correct flags: are some flags invalid at the same time? */ + } + + return 0; +} + +static int parse_full_pvc_status(struct osmo_fr_link *link, struct tlv_parsed *tp, size_t tp_len) +{ + size_t i; + int err = 0; + struct osmo_fr_dlc *dlc, *tmp; + struct q933_a_pvc_sts *pvc; + uint16_t dlci = 0; + uint16_t *dlcis = talloc_zero_array(link, uint16_t, tp_len); + if (!dlcis) + return -ENOMEM; + + /* first run validate all PVCs */ + err = validate_pvc_status(tp, tp_len); + if (err < 0) + goto out; + + for (i = 0; i < tp_len; i++) { + if (!TLVP_PRESENT(&tp[i], Q933_IEI_PVC_STATUS)) + continue; + + /* parse only 3 byte PVCs */ + pvc = (struct q933_a_pvc_sts *) TLVP_VAL_MINLEN( + &tp[i], + Q933_IEI_PVC_STATUS, + sizeof(struct q933_a_pvc_sts)); + if (!pvc) + continue; + + dlci = ((pvc->dlci_msb & 0x3f) << 4) | (pvc->dlci_lsb & 0xf); + dlcis[i] = dlci; + dlc = osmo_fr_dlc_by_dlci(link, dlci); + if (!dlc) { + dlc = osmo_fr_dlc_alloc(link, dlci); + if (!dlc) { + LOGPFRL(link, LOGL_ERROR, "Could not create DLC %d\n", dlci); + continue; + } + } + + /* Figure A.3/Q.933: The delete bit is only applicable for timely notification + * using the optional single PVC asynchronous status report. + * Ignoring the delete. */ + dlc->add = pvc->new; + dlc_set_active(dlc, pvc->active); + dlc->del = 0; + } + + /* check if all dlc are present in PVC Status */ + llist_for_each_entry_safe(dlc, tmp, &link->dlc_list, list) { + bool found = false; + for (i = 0; i < tp_len; i++) { + if (dlcis[i] == dlc->dlci) { + found = true; + break; + } + } + + if (!found) { + dlc_set_active(dlc, false); + dlc->del = true; + } + } + + return 0; +out: + talloc_free(dlcis); + return err; +} + +static int parse_link_pvc_status(struct osmo_fr_link *link, struct tlv_parsed *tp, size_t tp_len) +{ + int err; + size_t i; + struct q933_a_pvc_sts *pvc; + struct osmo_fr_dlc *dlc; + uint16_t dlci = 0; + + err = validate_pvc_status(tp, tp_len); + if (err < 0) + return err; + + for (i = 0; i < tp_len; i++) { + if (!TLVP_PRESENT(&tp[i], Q933_IEI_PVC_STATUS)) + continue; + + /* parse only 3 byte PVCs */ + pvc = (struct q933_a_pvc_sts *) TLVP_VAL_MINLEN( + &tp[i], + Q933_IEI_PVC_STATUS, + sizeof(struct q933_a_pvc_sts)); + if (!pvc) + continue; + + dlci = ((pvc->dlci_msb & 0x3f) << 4) | (pvc->dlci_lsb & 0xf); + dlc = osmo_fr_dlc_by_dlci(link, dlci); + if (!dlc) { + /* don't create dlc's for the ones which are about to be deleted. */ + if (pvc->delete) + continue; + + dlc = osmo_fr_dlc_alloc(link, dlci); + if (!dlc) { + LOGPFRL(link, LOGL_ERROR, "Rx STATUS: Could not create DLC %d\n", dlci); + continue; + } + } + + if (pvc->delete) { + dlc->del = 1; + } else { + dlc->add = pvc->new; + dlc_set_active(dlc, pvc->active); + dlc->del = 0; + } + } + + return 0; +} + +static size_t count_pvc_status(struct tlv_parsed *tp, size_t tp_len) +{ + size_t i, count = 0; + for (i = 0; i < tp_len; i++) { + if (!TLVP_PRESENT(&tp[i], Q933_IEI_PVC_STATUS)) + continue; + count++; + } + + return count; +} + +static int rx_lmi_q933_status(struct msgb *msg, struct tlv_parsed *tp) +{ + struct osmo_fr_link *link = msg->dst; + const uint8_t *link_int_rx; + uint8_t rep_type; + + OSMO_ASSERT(link); + + if (link->role == FR_ROLE_NETWORK_EQUIPMENT) { + LOGPFRL(link, LOGL_ERROR, "Rx STATUS: STATUS aren't supported in role network\n"); + return -1; + } + + /* check for mandatory IEs */ + if (!TLVP_PRES_LEN(tp, Q933_IEI_REPORT_TYPE, 1)) { + LOGPFRL(link, LOGL_NOTICE, "Rx STATUS: Missing TLV Q933 Report Type\n"); + return -1; + } + + rep_type = *TLVP_VAL(tp, Q933_IEI_REPORT_TYPE); + + switch (rep_type) { + case Q933_REPT_FULL_STATUS: + case Q933_REPT_LINK_INTEGRITY_VERIF: + if (rep_type != link->expected_rep) { + LOGPFRL(link, LOGL_NOTICE, "Rx STATUS: Unexpected Q933 report type (got 0x%x != exp 0x%x)\n", + rep_type, link->expected_rep); + return -1; + } + + if (!TLVP_PRES_LEN(tp, Q933_IEI_LINK_INT_VERIF, 2)) { + LOGPFRL(link, LOGL_NOTICE, "Rx STATUS: Missing TLV Q933 Link Integrety Verification\n"); + return -1; + } + link_int_rx = TLVP_VAL(tp, Q933_IEI_LINK_INT_VERIF); + link->last_rx_seq = link_int_rx[0]; + /* The received receive sequence number is not valid if + * it is not equal to the last transmitted send sequence + * number. Ignore messages containing this error. As a + * result, timer T391 expires and the user then + * increments the error count. */ + if (link_int_rx[1] != link->last_tx_seq) + return 0; + break; + case Q933_REPT_SINGLE_PVC_ASYNC_STS: + default: + return -1; + } + + check_link_state(link, true); + if (count_pvc_status(tp, MAX_SUPPORTED_PVC + 1) > MAX_SUPPORTED_PVC) { + LOGPFRL(link, LOGL_ERROR, "Rx STATUS: Too many PVC! Only %d are supported!\n", MAX_SUPPORTED_PVC); + } + + switch (rep_type) { + case Q933_REPT_FULL_STATUS: + parse_full_pvc_status(link, tp, MAX_SUPPORTED_PVC); + break; + case Q933_REPT_LINK_INTEGRITY_VERIF: + parse_link_pvc_status(link, tp, MAX_SUPPORTED_PVC); + break; + default: + break; + } + + /* The network responds to each STATUS ENQUIRY message with a + * STATUS message and resets the T392 timer */ + osmo_timer_schedule(&link->t392, osmo_tdef_get(link->net->T_defs, 392, OSMO_TDEF_S, 15), 0); + + return 0; +} + +static int rx_lmi_q922(struct msgb *msg) +{ + struct osmo_fr_link *link = msg->dst; + struct q933_a_hdr *qh; + /* the + 1 is used to detect more than MAX_SUPPORTED_PVC */ + struct tlv_parsed tp[MAX_SUPPORTED_PVC + 1]; + uint8_t *lapf; + int rc; + + OSMO_ASSERT(link); + + if (msgb_l2len(msg) < 1) + return -1; + lapf = msgb_l2(msg); + + /* we only support LAPF UI frames */ + if (lapf[0] != LAPF_UI) + return -1; + + msg->l3h = msg->l2h + 1; + if (msgb_l3len(msg) < 3) + return -1; + + qh = (struct q933_a_hdr *) msgb_l3(msg); + if (qh->prot_disc != Q931_PDISC_CC) { + LOGPFRL(link, LOGL_NOTICE, + "Rx unsupported LMI protocol discriminator %u\n", qh->prot_disc); + return -1; + } + + rc = tlv_parse2(tp, MAX_SUPPORTED_PVC + 1, &q933_att_tlvdef, + msgb_l3(msg) + sizeof(*qh), + msgb_l3len(msg) - sizeof(*qh), 0, 0); + if (rc < 0) { + LOGPFRL(link, LOGL_NOTICE, + "Failed to parse TLVs in LMI message type %u\n", qh->msg_type); + return rc; + } + + switch (qh->msg_type) { + case Q931_MSGT_STATUS_ENQUIRY: + rc = rx_lmi_q933_status_enq(msg, tp); + break; + case Q931_MSGT_STATUS: + rc = rx_lmi_q933_status(msg, tp); + break; + default: + LOGPFRL(link, LOGL_NOTICE, + "Rx unsupported LMI message type %u\n", qh->msg_type); + rc = -1; + break; + } + msgb_free(msg); + + return rc; +} + +int osmo_fr_rx(struct msgb *msg) +{ + int rc = 0; + uint8_t *frh; + uint16_t dlci; + struct osmo_fr_dlc *dlc; + struct osmo_fr_link *link = msg->dst; + + OSMO_ASSERT(link); + + if (msgb_length(msg) < 2) { + LOGPFRL(link, LOGL_ERROR, "Rx short FR header: %u bytes\n", msgb_length(msg)); + rc = -1; + goto out; + } + + frh = msg->l1h = msgb_data(msg); + if (frh[0] & 0x01) { + LOGPFRL(link, LOGL_NOTICE, "Rx Unsupported single-byte FR address\n"); + rc = -1; + goto out; + } + if ((frh[1] & 0x0f) != 0x01) { + LOGPFRL(link, LOGL_NOTICE, "Rx Unknown second FR octet 0x%02x\n", frh[1]); + rc = -1; + goto out; + } + dlci = q922_to_dlci(frh); + msg->l2h = frh + 2; + + switch (dlci) { + case LMI_Q933A_DLCI: + return rx_lmi_q922(msg); + case LMI_CISCO_DLCI: + LOGPFRL(link, LOGL_ERROR, "Rx Unsupported FR DLCI %u\n", dlci); + goto out; + } + + if (!link->state) { + LOGPFRL(link, LOGL_NOTICE, "Link is not reliable. Discarding Rx PDU on DLCI %d\n", dlci); + goto out; + } + + dlc = osmo_fr_dlc_by_dlci(link, dlci); + if (dlc) { + if (dlc->active) { + /* dispatch to handler of respective DLC */ + msg->dst = dlc; + return dlc->rx_cb(dlc->cb_data, msg); + } else { + LOGPFRL(link, LOGL_NOTICE, "DLCI %u not yet active. Discarding Rx PDU\n", dlci); + } + } else { + if (link->unknown_dlc_rx_cb) + return link->unknown_dlc_rx_cb(link->unknown_dlc_rx_cb_data, msg); + else + LOGPFRL(link, LOGL_NOTICE, "DLCI %u doesn't exist. Discarding Rx PDU\n", dlci); + } + +out: + msgb_free(msg); + + return rc; +} + +int osmo_fr_tx_dlc(struct msgb *msg) +{ + uint8_t *frh; + struct osmo_fr_dlc *dlc = msg->dst; + struct osmo_fr_link *link = dlc->link; + + OSMO_ASSERT(dlc); + OSMO_ASSERT(link); + + if (!link->state) { + LOGPFRL(link, LOGL_NOTICE, "Link is not reliable (yet), discarding Tx\n"); + msgb_free(msg); + return -1; + } + if (!dlc->active) { + LOGPFRL(link, LOGL_NOTICE, "DLCI %u is not active (yet), discarding Tx\n", dlc->dlci); + msgb_free(msg); + return -1; + } + LOGPFRL(link, LOGL_DEBUG, "DLCI %u is active, sending message\n", dlc->dlci); + + if (msgb_headroom(msg) < 2) { + msgb_free(msg); + return -ENOSPC; + } + + frh = msgb_push(msg, 2); + dlci_to_q922(frh, dlc->dlci); + + msg->dst = link; + return link->tx_cb(link->cb_data, msg); +} + +/* Every T391 seconds, the user equipment sends a STATUS ENQUIRY + * message to the network and resets its polling timer (T391). */ +static void fr_t391_cb(void *data) +{ + struct osmo_fr_link *link = data; + + OSMO_ASSERT(link); + + if (link->polling_count % link->net->n391 == 0) + tx_lmi_q933_status_enq(link, Q933_REPT_FULL_STATUS); + else + tx_lmi_q933_status_enq(link, Q933_REPT_LINK_INTEGRITY_VERIF); + link->polling_count++; + osmo_timer_schedule(&link->t391, osmo_tdef_get(link->net->T_defs, 391, OSMO_TDEF_S, 10), 0); +} + +static void fr_t392_cb(void *data) +{ + struct osmo_fr_link *link = data; + + OSMO_ASSERT(link); + + /* A.5 The network increments the error count .. Non-receipt of + * a STATUS ENQUIRY within T392, which results in restarting + * T392 */ + link->err_count++; + check_link_state(link, false); + osmo_timer_schedule(&link->t392, osmo_tdef_get(link->net->T_defs, 392, OSMO_TDEF_S, 15), 0); +} + +/* allocate a frame relay network */ +struct osmo_fr_network *osmo_fr_network_alloc(void *ctx) +{ + struct osmo_fr_network *net = talloc_zero(ctx, struct osmo_fr_network); + if (!net) + return NULL; + + INIT_LLIST_HEAD(&net->links); + net->T_defs = fr_tdefs; + osmo_tdefs_reset(net->T_defs); + net->n391 = 6; + net->n392 = 3; + net->n393 = 4; + + return net; +} + +void osmo_fr_network_free(struct osmo_fr_network *net) +{ + struct osmo_fr_link *link, *tmp; + + if (!net) + return; + + llist_for_each_entry_safe(link, tmp, &net->links, list) { + osmo_fr_link_free(link); + } +} + +/* allocate a frame relay link in a given network */ +struct osmo_fr_link *osmo_fr_link_alloc(struct osmo_fr_network *net, enum osmo_fr_role role, const char *name) +{ + struct osmo_fr_link *link = talloc_zero(net, struct osmo_fr_link); + if (!link) + return NULL; + link->role = role; + link->net = net; + link->name = talloc_strdup(link, name); + INIT_LLIST_HEAD(&link->dlc_list); + llist_add_tail(&link->list, &net->links); + + osmo_timer_setup(&link->t391, fr_t391_cb, link); + osmo_timer_setup(&link->t392, fr_t392_cb, link); + + switch (role) { + case FR_ROLE_USER_EQUIPMENT: + osmo_timer_schedule(&link->t391, osmo_tdef_get(link->net->T_defs, 391, OSMO_TDEF_S, 15), 0); + break; + case FR_ROLE_NETWORK_EQUIPMENT: + osmo_timer_schedule(&link->t392, osmo_tdef_get(link->net->T_defs, 392, OSMO_TDEF_S, 15), 0); + break; + } + + LOGPFRL(link, LOGL_INFO, "Creating frame relay link with role %s\n", osmo_fr_role_str(role)); + + return link; +} + +void osmo_fr_link_free(struct osmo_fr_link *link) +{ + struct osmo_fr_dlc *dlc, *tmp; + + if (!link) + return; + + osmo_timer_del(&link->t391); + osmo_timer_del(&link->t392); + + llist_for_each_entry_safe(dlc, tmp, &link->dlc_list, list) { + osmo_fr_dlc_free(dlc); + } + + llist_del(&link->list); + talloc_free(link); +} + +/* allocate a data link connectoin on a given framerelay link */ +struct osmo_fr_dlc *osmo_fr_dlc_alloc(struct osmo_fr_link *link, uint16_t dlci) +{ + struct osmo_fr_dlc *dlc = talloc_zero(link, struct osmo_fr_dlc); + if (!dlc) + return NULL; + + dlc->link = link; + dlc->dlci = dlci; + dlc->active = false; + + llist_add_tail(&dlc->list, &link->dlc_list); + + dlc->add = true; + tx_lmi_q933_status(link, Q933_REPT_SINGLE_PVC_ASYNC_STS); + + return dlc; +} + +void osmo_fr_dlc_free(struct osmo_fr_dlc *dlc) +{ + llist_del(&dlc->list); + talloc_free(dlc); +} + +/* TODO: rework osmo_fr_dlc_alloc/free with handling it's own memory. + * For network role: The dlc have to created by the application (e.g. vty). + * The dlc shouldn't free'd directly. It should be communicated to the + * other side and wait until it's confirmed OR the link go off and free it afterwards. + * For user equpment role: The dlc can be created by the application or the dlc will be created + * by the frame relay because the network is configuring the dlc. + * The dlc shouldn't be free'd. Only the handler should be set to NULL. + */ + +struct osmo_fr_dlc *osmo_fr_dlc_by_dlci(struct osmo_fr_link *link, uint16_t dlci) +{ + struct osmo_fr_dlc *dlc; + + llist_for_each_entry(dlc, &link->dlc_list, list) { + if (dlc->dlci == dlci) + return dlc; + } + return NULL; +} + + +#include <osmocom/vty/vty.h> +#include <osmocom/vty/tdef_vty.h> + +static void fr_dlc_dump_vty(struct vty *vty, const struct osmo_fr_dlc *dlc) +{ + vty_out(vty, " FR DLC %05u: %s%s%s%s", dlc->dlci, + dlc->active ? "ACTIVE" : "INACTIVE", + dlc->add ? " ADDED" : "", dlc->del ? " DELETED" : "", VTY_NEWLINE); +} + +static void fr_link_dump_vty(struct vty *vty, const struct osmo_fr_link *link) +{ + const struct osmo_fr_dlc *dlc; + + vty_out(vty, "FR Link '%s': Role %s, LastRxSeq %u, LastTxSeq %u%s", + link->name, link->role == FR_ROLE_USER_EQUIPMENT ? "USER" : "NETWORK", + link->last_rx_seq, link->last_tx_seq, VTY_NEWLINE); + llist_for_each_entry(dlc, &link->dlc_list, list) { + fr_dlc_dump_vty(vty, dlc); + } +} + +void osmo_fr_network_dump_vty(struct vty *vty, const struct osmo_fr_network *net) +{ + struct osmo_fr_link *link; + + vty_out(vty, "FR Network: N391 %u, N392 %u, N393 %u%s", + net->n391, net->n392, net->n393, VTY_NEWLINE); + osmo_tdef_vty_out_all(vty, net->T_defs, " "); + llist_for_each_entry(link, &net->links, list) { + fr_link_dump_vty(vty, link); + } +} diff --git a/src/gb/gprs_bssgp.c b/src/gb/gprs_bssgp.c index 896f1c5a..7abef804 100644 --- a/src/gb/gprs_bssgp.c +++ b/src/gb/gprs_bssgp.c @@ -40,10 +40,16 @@ #include <osmocom/gprs/gprs_bssgp_bss.h> #include <osmocom/gprs/gprs_ns.h> -#include "common_vty.h" +#include "osmocom/gsm/gsm48.h" +#include "gprs_bssgp_internal.h" void *bssgp_tall_ctx = NULL; +static int _gprs_ns_sendmsg(void *ctx, struct msgb *msg); + +bssgp_bvc_send bssgp_ns_send = _gprs_ns_sendmsg; +void *bssgp_ns_send_data = NULL; + static const struct rate_ctr_desc bssgp_ctr_description[] = { { "packets:in", "Packets at BSSGP Level ( In)" }, { "packets:out","Packets at BSSGP Level (Out)" }, @@ -67,6 +73,14 @@ LLIST_HEAD(bssgp_bvc_ctxts); static int _bssgp_tx_dl_ud(struct bssgp_flow_control *fc, struct msgb *msg, uint32_t llc_pdu_len, void *priv); + +/* callback to be backward compatible with old users which do not set the bssgp_ns_send function */ +static int _gprs_ns_sendmsg(void *ctx, struct msgb *msg) +{ + OSMO_ASSERT(bssgp_nsi); + return gprs_ns_sendmsg(bssgp_nsi, msg); +} + /* Find a BTS Context based on parsed RA ID and Cell ID */ struct bssgp_bvc_ctx *btsctx_by_raid_cid(const struct gprs_ra_id *raid, uint16_t cid) { @@ -80,6 +94,75 @@ struct bssgp_bvc_ctx *btsctx_by_raid_cid(const struct gprs_ra_id *raid, uint16_t return NULL; } +/* Transmit a BVC-RESET or BVC-RESET-ACK with a given nsei and bvci (Chapter 10.4.12) + * \param[in] pdu Either BSSGP_PDUT_BVC_RESET or BSSGP_PDUT_BVC_RESET_ACK + * \param[in] nsei The NSEI to transmit over + * \param[in] bvci BVCI of the BVC to reset + * \param[in] cause The cause of the reset only valid for BSSGP_PDUT_BVC_RESET. + * \param[in] ra_id Pointer to the ra_id to include. If NULL no cell information will be included + * \param[in] cell_id The cell_id to include (if ra_id is not NULL) + * returns >= 0 on success, on error < 0. + */ +static int tx_bvc_reset_nsei_bvci(enum bssgp_pdu_type pdu, uint16_t nsei, uint16_t bvci, + enum gprs_bssgp_cause cause, const struct gprs_ra_id *ra_id, uint16_t cell_id) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph = + (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + uint16_t _bvci = osmo_htons(bvci); + + OSMO_ASSERT(pdu == BSSGP_PDUT_BVC_RESET || pdu == BSSGP_PDUT_BVC_RESET_ACK); + + msgb_nsei(msg) = nsei; + msgb_bvci(msg) = BVCI_SIGNALLING; + bgph->pdu_type = pdu; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + + if (pdu == BSSGP_PDUT_BVC_RESET) { + msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, (uint8_t *) &cause); + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-RESET " + "CAUSE=%s\n", bvci, bssgp_cause_str(cause)); + } else { + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-RESET-ACK\n", bvci); + } + + if (ra_id) { + uint8_t bssgp_cid[8]; + bssgp_create_cell_id(bssgp_cid, ra_id, cell_id); + msgb_tvlv_put(msg, BSSGP_IE_CELL_ID, sizeof(bssgp_cid), bssgp_cid); + } + + /* Optional: Feature Bitmap */ + + return bssgp_ns_send(bssgp_ns_send_data, msg); +} + +/*! Transmit a BVC-RESET message with a given nsei and bvci (Chapter 10.4.12) + * \param[in] nsei The NSEI to transmit over + * \param[in] bvci BVCI of the BVC to reset + * \param[in] cause The cause of the reset + * \param[in] ra_id Pointer to the ra_id to include. If NULL no cell information will be included + * \param[in] cell_id The cell_id to include (if ra_id is not NULL) + * returns >= 0 on success, on error < 0. + */ +int bssgp_tx_bvc_reset_nsei_bvci(uint16_t nsei, uint16_t bvci, enum gprs_bssgp_cause cause, const struct gprs_ra_id *ra_id, uint16_t cell_id) +{ + return tx_bvc_reset_nsei_bvci(BSSGP_PDUT_BVC_RESET, nsei, bvci, cause, ra_id, cell_id); +} + +/*! Transmit a BVC-RESET-ACK message with a given nsei and bvci (Chapter 10.4.12) + * \param[in] nsei The NSEI to transmit over + * \param[in] bvci BVCI of the BVC to reset + * \param[in] ra_id Pointer to the ra_id to include. If NULL no cell information will be included + * \param[in] cell_id The cell_id to include (if ra_id is not NULL) + * returns >= 0 on success, on error < 0. + */ +int bssgp_tx_bvc_reset_ack_nsei_bvci(uint16_t nsei, uint16_t bvci, const struct gprs_ra_id *ra_id, uint16_t cell_id) +{ + return tx_bvc_reset_nsei_bvci(BSSGP_PDUT_BVC_RESET_ACK, nsei, bvci, 0, ra_id, cell_id); +} + /*! Initiate reset procedure for all PTP BVC on a given NSEI. * * This function initiates reset procedure for all PTP BVC with a given cause. @@ -94,7 +177,7 @@ int bssgp_tx_bvc_ptp_reset(uint16_t nsei, enum gprs_bssgp_cause cause) llist_for_each_entry(bctx, &bssgp_bvc_ctxts, list) { if (bctx->nsei == nsei && bctx->bvci != BVCI_SIGNALLING) { - LOGP(DBSSGP, LOGL_DEBUG, "NSEI=%u/BVCI=%u RESET due to %s\n", + LOGP(DLBSSGP, LOGL_DEBUG, "NSEI=%u/BVCI=%u RESET due to %s\n", nsei, bctx->bvci, bssgp_cause_str(cause)); rc = bssgp_tx_bvc_reset(bctx, bctx->bvci, cause); if (rc < 0) @@ -117,6 +200,12 @@ struct bssgp_bvc_ctx *btsctx_by_bvci_nsei(uint16_t bvci, uint16_t nsei) return NULL; } +void bssgp_set_bssgp_callback(bssgp_bvc_send ns_send, void *data) +{ + bssgp_ns_send = ns_send; + bssgp_ns_send_data = data; +} + struct bssgp_bvc_ctx *btsctx_alloc(uint16_t bvci, uint16_t nsei) { struct bssgp_bvc_ctx *ctx; @@ -126,25 +215,36 @@ struct bssgp_bvc_ctx *btsctx_alloc(uint16_t bvci, uint16_t nsei) return NULL; ctx->bvci = bvci; ctx->nsei = nsei; + ctx->is_sgsn = true; /* FIXME: BVCI is not unique, only BVCI+NSEI ?!? */ ctx->ctrg = rate_ctr_group_alloc(ctx, &bssgp_ctrg_desc, bvci); - if (!ctx->ctrg) { - talloc_free(ctx); - return NULL; - } + if (!ctx->ctrg) + goto err_ctrg; + ctx->fc = talloc_zero(ctx, struct bssgp_flow_control); + if (!ctx->fc) + goto err_fc; + /* cofigure for 2Mbit, 30 packets in queue */ bssgp_fc_init(ctx->fc, 100000, 2*1024*1024/8, 30, &_bssgp_tx_dl_ud); llist_add(&ctx->list, &bssgp_bvc_ctxts); return ctx; + +err_fc: + rate_ctr_group_free(ctx->ctrg); +err_ctrg: + talloc_free(ctx); + return NULL; } void bssgp_bvc_ctx_free(struct bssgp_bvc_ctx *ctx) { if (!ctx) return; + + osmo_timer_del(&ctx->fc->timer); rate_ctr_group_free(ctx->ctrg); llist_del(&ctx->list); talloc_free(ctx); @@ -163,7 +263,7 @@ static int bssgp_tx_fc_bvc_ack(uint16_t nsei, uint8_t tag, uint16_t ns_bvci) bgph->pdu_type = BSSGP_PDUT_FLOW_CONTROL_BVC_ACK; msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &tag); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* 10.3.7 SUSPEND-ACK PDU */ @@ -182,7 +282,7 @@ int bssgp_tx_suspend_ack(uint16_t nsei, uint32_t tlli, bssgp_msgb_ra_put(msg, ra_id); msgb_tvlv_put(msg, BSSGP_IE_SUSPEND_REF_NR, 1, &suspend_ref); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* 10.3.8 SUSPEND-NACK PDU */ @@ -204,7 +304,7 @@ int bssgp_tx_suspend_nack(uint16_t nsei, uint32_t tlli, if (cause) msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, cause); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* 10.3.10 RESUME-ACK PDU */ @@ -222,7 +322,7 @@ int bssgp_tx_resume_ack(uint16_t nsei, uint32_t tlli, bssgp_msgb_tlli_put(msg, tlli); bssgp_msgb_ra_put(msg, ra_id); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* 10.3.11 RESUME-NACK PDU */ @@ -243,7 +343,7 @@ int bssgp_tx_resume_nack(uint16_t nsei, uint32_t tlli, if (cause) msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, cause); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } uint16_t bssgp_parse_cell_id(struct gprs_ra_id *raid, const uint8_t *buf) @@ -266,7 +366,7 @@ int bssgp_create_cell_id(uint8_t *buf, const struct gprs_ra_id *raid, } /* Chapter 8.4 BVC-Reset Procedure */ -static int bssgp_rx_bvc_reset(struct msgb *msg, struct tlv_parsed *tp, +static int bssgp_rx_bvc_reset(struct msgb *msg, struct tlv_parsed *tp, uint16_t ns_bvci) { struct osmo_bssgp_prim nmp; @@ -275,7 +375,7 @@ static int bssgp_rx_bvc_reset(struct msgb *msg, struct tlv_parsed *tp, uint16_t bvci; bvci = tlvp_val16be(tp, BSSGP_IE_BVCI); - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx RESET cause=%s\n", bvci, + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx RESET cause=%s\n", bvci, bssgp_cause_str(*TLVP_VAL(tp, BSSGP_IE_CAUSE))); /* look-up or create the BTS context for this BVC */ @@ -288,19 +388,26 @@ static int bssgp_rx_bvc_reset(struct msgb *msg, struct tlv_parsed *tp, /* When we receive a BVC-RESET PDU (at least of a PTP BVCI), the BSS * informs us about its RAC + Cell ID, so we can create a mapping */ - if (bvci != 0 && bvci != 1) { - if (!TLVP_PRESENT(tp, BSSGP_IE_CELL_ID)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx RESET " + if (bctx->is_sgsn && bvci != BVCI_SIGNALLING && bvci != BVCI_PTM) { + if (!TLVP_PRES_LEN(tp, BSSGP_IE_CELL_ID, 8)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx RESET " "missing mandatory IE\n", bvci); return -EINVAL; } /* actually extract RAC / CID */ bctx->cell_id = bssgp_parse_cell_id(&bctx->ra_id, TLVP_VAL(tp, BSSGP_IE_CELL_ID)); - LOGP(DBSSGP, LOGL_NOTICE, "Cell %s CI %u on BVCI %u\n", + LOGP(DLBSSGP, LOGL_NOTICE, "Cell %s CI %u on BVCI %u\n", osmo_rai_name(&bctx->ra_id), bctx->cell_id, bvci); } + /* Acknowledge the RESET to the BTS */ + if (bvci == BVCI_SIGNALLING || bvci == BVCI_PTM || bctx->is_sgsn) + bssgp_tx_simple_bvci(BSSGP_PDUT_BVC_RESET_ACK, + nsei, bvci, ns_bvci); + else + bssgp_tx_bvc_reset_ack_nsei_bvci(nsei, bvci, &bctx->ra_id, bctx->cell_id); + /* Send NM_BVC_RESET.ind to NM */ memset(&nmp, 0, sizeof(nmp)); nmp.nsei = nsei; @@ -310,10 +417,6 @@ static int bssgp_rx_bvc_reset(struct msgb *msg, struct tlv_parsed *tp, osmo_prim_init(&nmp.oph, SAP_BSSGP_NM, PRIM_NM_BVC_RESET, PRIM_OP_INDICATION, msg); bssgp_prim_cb(&nmp.oph, NULL); - - /* Acknowledge the RESET to the BTS */ - bssgp_tx_simple_bvci(BSSGP_PDUT_BVC_RESET_ACK, - nsei, bvci, ns_bvci); return 0; } @@ -326,20 +429,20 @@ static int bssgp_rx_bvc_block(struct msgb *msg, struct tlv_parsed *tp) bvci = tlvp_val16be(tp, BSSGP_IE_BVCI); if (bvci == BVCI_SIGNALLING) { /* 8.3.2: Signalling BVC shall never be blocked */ - LOGP(DBSSGP, LOGL_ERROR, "NSEI=%u/BVCI=%u " + LOGP(DLBSSGP, LOGL_ERROR, "NSEI=%u/BVCI=%u " "received block for signalling BVC!?!\n", nsei, msgb_bvci(msg)); return 0; } - LOGP(DBSSGP, LOGL_INFO, "BSSGP Rx BVCI=%u BVC-BLOCK\n", bvci); + LOGP(DLBSSGP, LOGL_INFO, "BSSGP Rx BVCI=%u BVC-BLOCK\n", bvci); ptp_ctx = btsctx_by_bvci_nsei(bvci, nsei); if (!ptp_ctx) return bssgp_tx_status(BSSGP_CAUSE_UNKNOWN_BVCI, &bvci, msg); ptp_ctx->state |= BVC_S_BLOCKED; - rate_ctr_inc(&ptp_ctx->ctrg->ctr[BSSGP_CTR_BLOCKED]); + rate_ctr_inc(rate_ctr_group_get_ctr(ptp_ctx->ctrg, BSSGP_CTR_BLOCKED)); /* Send NM_BVC_BLOCK.ind to NM */ memset(&nmp, 0, sizeof(nmp)); @@ -364,13 +467,13 @@ static int bssgp_rx_bvc_unblock(struct msgb *msg, struct tlv_parsed *tp) bvci = tlvp_val16be(tp, BSSGP_IE_BVCI); if (bvci == BVCI_SIGNALLING) { /* 8.3.2: Signalling BVC shall never be blocked */ - LOGP(DBSSGP, LOGL_ERROR, "NSEI=%u/BVCI=%u " + LOGP(DLBSSGP, LOGL_ERROR, "NSEI=%u/BVCI=%u " "received unblock for signalling BVC!?!\n", nsei, msgb_bvci(msg)); return 0; } - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx BVC-UNBLOCK\n", bvci); + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx BVC-UNBLOCK\n", bvci); ptp_ctx = btsctx_by_bvci_nsei(bvci, nsei); if (!ptp_ctx) @@ -402,12 +505,12 @@ static int bssgp_rx_ul_ud(struct msgb *msg, struct tlv_parsed *tp, /* extract TLLI and parse TLV IEs */ msgb_tlli(msg) = osmo_ntohl(budh->tlli); - DEBUGP(DBSSGP, "BSSGP TLLI=0x%08x Rx UPLINK-UNITDATA\n", msgb_tlli(msg)); + DEBUGP(DLBSSGP, "BSSGP TLLI=0x%08x Rx UPLINK-UNITDATA\n", msgb_tlli(msg)); /* Cell ID and LLC_PDU are the only mandatory IE */ - if (!TLVP_PRESENT(tp, BSSGP_IE_CELL_ID) || + if (!TLVP_PRES_LEN(tp, BSSGP_IE_CELL_ID, 8) || !TLVP_PRESENT(tp, BSSGP_IE_LLC_PDU)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP TLLI=0x%08x Rx UL-UD " + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP TLLI=0x%08x Rx UL-UD " "missing mandatory IE\n", msgb_tlli(msg)); return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); } @@ -435,16 +538,16 @@ static int bssgp_rx_suspend(struct msgb *msg, struct tlv_parsed *tp) uint16_t ns_bvci = msgb_bvci(msg), nsei = msgb_nsei(msg); int rc; - if (!TLVP_PRESENT(tp, BSSGP_IE_TLLI) || - !TLVP_PRESENT(tp, BSSGP_IE_ROUTEING_AREA)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx SUSPEND " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_TLLI, 4) || + !TLVP_PRES_LEN(tp, BSSGP_IE_ROUTEING_AREA, 6)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx SUSPEND " "missing mandatory IE\n", ns_bvci); return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); } tlli = tlvp_val32be(tp, BSSGP_IE_TLLI); - DEBUGP(DBSSGP, "BSSGP BVCI=%u TLLI=0x%08x Rx SUSPEND\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u TLLI=0x%08x Rx SUSPEND\n", ns_bvci, tlli); gsm48_parse_ra(&raid, TLVP_VAL(tp, BSSGP_IE_ROUTEING_AREA)); @@ -476,10 +579,10 @@ static int bssgp_rx_resume(struct msgb *msg, struct tlv_parsed *tp) uint16_t ns_bvci = msgb_bvci(msg), nsei = msgb_nsei(msg); int rc; - if (!TLVP_PRESENT(tp, BSSGP_IE_TLLI) || - !TLVP_PRESENT(tp, BSSGP_IE_ROUTEING_AREA) || - !TLVP_PRESENT(tp, BSSGP_IE_SUSPEND_REF_NR)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx RESUME " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_TLLI, 4 ) || + !TLVP_PRES_LEN(tp, BSSGP_IE_ROUTEING_AREA, 6) || + !TLVP_PRES_LEN(tp, BSSGP_IE_SUSPEND_REF_NR, 1)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx RESUME " "missing mandatory IE\n", ns_bvci); return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); } @@ -487,7 +590,7 @@ static int bssgp_rx_resume(struct msgb *msg, struct tlv_parsed *tp) tlli = tlvp_val32be(tp, BSSGP_IE_TLLI); suspend_ref = *TLVP_VAL(tp, BSSGP_IE_SUSPEND_REF_NR); - DEBUGP(DBSSGP, "BSSGP BVCI=%u TLLI=0x%08x Rx RESUME\n", ns_bvci, tlli); + DEBUGP(DLBSSGP, "BSSGP BVCI=%u TLLI=0x%08x Rx RESUME\n", ns_bvci, tlli); gsm48_parse_ra(&raid, TLVP_VAL(tp, BSSGP_IE_ROUTEING_AREA)); @@ -518,21 +621,21 @@ static int bssgp_rx_llc_disc(struct msgb *msg, struct tlv_parsed *tp, uint32_t tlli = 0; uint16_t nsei = msgb_nsei(msg); - if (!TLVP_PRESENT(tp, BSSGP_IE_TLLI) || - !TLVP_PRESENT(tp, BSSGP_IE_LLC_FRAMES_DISCARDED) || - !TLVP_PRESENT(tp, BSSGP_IE_BVCI) || - !TLVP_PRESENT(tp, BSSGP_IE_NUM_OCT_AFF)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx LLC DISCARDED " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_TLLI, 4) || + !TLVP_PRES_LEN(tp, BSSGP_IE_LLC_FRAMES_DISCARDED, 1) || + !TLVP_PRES_LEN(tp, BSSGP_IE_BVCI, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_NUM_OCT_AFF, 3)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx LLC DISCARDED " "missing mandatory IE\n", ctx->bvci); + return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); } - if (TLVP_PRESENT(tp, BSSGP_IE_TLLI)) - tlli = tlvp_val32be(tp, BSSGP_IE_TLLI); + tlli = tlvp_val32be(tp, BSSGP_IE_TLLI); - DEBUGP(DBSSGP, "BSSGP BVCI=%u TLLI=%08x Rx LLC DISCARDED\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u TLLI=%08x Rx LLC DISCARDED\n", ctx->bvci, tlli); - rate_ctr_inc(&ctx->ctrg->ctr[BSSGP_CTR_DISCARDED]); + rate_ctr_inc(rate_ctr_group_get_ctr(ctx->ctrg, BSSGP_CTR_DISCARDED)); /* send NM_LLC_DISCARDED to NM */ memset(&nmp, 0, sizeof(nmp)); @@ -553,27 +656,27 @@ int bssgp_rx_status(struct msgb *msg, struct tlv_parsed *tp, struct osmo_bssgp_prim nmp; enum gprs_bssgp_cause cause; - if (!TLVP_PRESENT(tp, BSSGP_IE_CAUSE)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx STATUS " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_CAUSE, 1)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx STATUS " "missing mandatory IE\n", bvci); cause = BSSGP_CAUSE_PROTO_ERR_UNSPEC; } else { cause = *TLVP_VAL(tp, BSSGP_IE_CAUSE); } - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP BVCI=%u Rx BVC STATUS, cause=%s\n", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP BVCI=%u Rx BVC STATUS, cause=%s\n", bvci, bssgp_cause_str(cause)); if (cause == BSSGP_CAUSE_BVCI_BLOCKED || cause == BSSGP_CAUSE_UNKNOWN_BVCI) { - if (!TLVP_PRESENT(tp, BSSGP_IE_BVCI)) - LOGP(DBSSGP, LOGL_ERROR, + if (!TLVP_PRES_LEN(tp, BSSGP_IE_BVCI, 2)) + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx STATUS cause=%s " "missing conditional BVCI IE\n", bvci, bssgp_cause_str(cause)); } if (bctx) - rate_ctr_inc(&bctx->ctrg->ctr[BSSGP_CTR_STATUS]); + rate_ctr_inc(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_STATUS)); /* send NM_STATUS to NM */ memset(&nmp, 0, sizeof(nmp)); @@ -586,7 +689,6 @@ int bssgp_rx_status(struct msgb *msg, struct tlv_parsed *tp, return bssgp_prim_cb(&nmp.oph, NULL); } - /* One element (msgb) in a BSSGP Flow Control queue */ struct bssgp_fc_queue_element { /* linked list of queue elements */ @@ -618,7 +720,7 @@ static void fc_timer_cb(void *data) list); if (bssgp_fc_needs_queueing(fc, fcqe->llc_pdu_len)) { - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP-FC: fc_timer_cb() but still " + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP-FC: fc_timer_cb() but still " "not able to send PDU of %u bytes\n", fcqe->llc_pdu_len); /* make sure we re-start the timer */ fc_queue_timer_cfg(fc); @@ -744,7 +846,7 @@ static int bssgp_fc_needs_queueing(struct bssgp_flow_control *fc, uint32_t pdu_l static int _bssgp_tx_dl_ud(struct bssgp_flow_control *fc, struct msgb *msg, uint32_t llc_pdu_len, void *priv) { - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* input function of the flow control implementation, called first @@ -756,7 +858,7 @@ int bssgp_fc_in(struct bssgp_flow_control *fc, struct msgb *msg, struct timeval time_now; if (llc_pdu_len > fc->bucket_size_max) { - LOGP(DBSSGP, LOGL_NOTICE, "Single PDU (size=%u) is larger " + LOGP(DLBSSGP, LOGL_NOTICE, "Single PDU (size=%u) is larger " "than maximum bucket size (%u)!\n", llc_pdu_len, fc->bucket_size_max); msgb_free(msg); @@ -817,15 +919,15 @@ static int bssgp_rx_fc_bvc(struct msgb *msg, struct tlv_parsed *tp, uint32_t old_leak_rate = bctx->fc->bucket_leak_rate; uint32_t old_r_def_ms = bctx->r_default_ms; - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx Flow Control BVC\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx Flow Control BVC\n", bctx->bvci); - if (!TLVP_PRESENT(tp, BSSGP_IE_TAG) || - !TLVP_PRESENT(tp, BSSGP_IE_BVC_BUCKET_SIZE) || - !TLVP_PRESENT(tp, BSSGP_IE_BUCKET_LEAK_RATE) || - !TLVP_PRESENT(tp, BSSGP_IE_BMAX_DEFAULT_MS) || - !TLVP_PRESENT(tp, BSSGP_IE_R_DEFAULT_MS)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx FC BVC " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_TAG, 1) || + !TLVP_PRES_LEN(tp, BSSGP_IE_BVC_BUCKET_SIZE, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_BUCKET_LEAK_RATE, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_BMAX_DEFAULT_MS, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_R_DEFAULT_MS,2)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx FC BVC " "missing mandatory IE\n", bctx->bvci); return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); } @@ -840,17 +942,17 @@ static int bssgp_rx_fc_bvc(struct msgb *msg, struct tlv_parsed *tp, bctx->r_default_ms = 100 * tlvp_val16be(tp, BSSGP_IE_R_DEFAULT_MS) / 8; if (old_leak_rate != 0 && bctx->fc->bucket_leak_rate == 0) - LOGP(DBSSGP, LOGL_NOTICE, "BSS instructs us to bucket leak " + LOGP(DLBSSGP, LOGL_NOTICE, "BSS instructs us to bucket leak " "rate of 0, stopping all DL GPRS!\n"); else if (old_leak_rate == 0 && bctx->fc->bucket_leak_rate != 0) - LOGP(DBSSGP, LOGL_NOTICE, "BSS instructs us to bucket leak " + LOGP(DLBSSGP, LOGL_NOTICE, "BSS instructs us to bucket leak " "rate of != 0, restarting all DL GPRS!\n"); if (old_r_def_ms != 0 && bctx->r_default_ms == 0) - LOGP(DBSSGP, LOGL_NOTICE, "BSS instructs us to MS default " + LOGP(DLBSSGP, LOGL_NOTICE, "BSS instructs us to MS default " "bucket leak rate of 0, stopping DL GPRS!\n"); else if (old_r_def_ms == 0 && bctx->r_default_ms != 0) - LOGP(DBSSGP, LOGL_NOTICE, "BSS instructs us to MS default " + LOGP(DLBSSGP, LOGL_NOTICE, "BSS instructs us to MS default " "bucket leak rate != 0, restarting DL GPRS!\n"); /* reconfigure the timer for flow control based on new values */ @@ -887,13 +989,13 @@ static int bssgp_rx_ptp(struct msgb *msg, struct tlv_parsed *tp, break; case BSSGP_PDUT_RA_CAPABILITY: /* BSS requests RA capability or IMSI */ - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx RA CAPABILITY UPDATE\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx RA CAPABILITY UPDATE\n", bctx->bvci); /* FIXME: send GMM_RA_CAPABILITY_UPDATE.ind to GMM */ /* FIXME: send RA_CAPA_UPDATE_ACK */ break; case BSSGP_PDUT_RADIO_STATUS: - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx RADIO STATUS\n", bctx->bvci); + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx RADIO STATUS\n", bctx->bvci); /* BSS informs us of some exception */ /* FIXME: send GMM_RADIO_STATUS.ind to GMM */ break; @@ -903,7 +1005,7 @@ static int bssgp_rx_ptp(struct msgb *msg, struct tlv_parsed *tp, break; case BSSGP_PDUT_FLOW_CONTROL_MS: /* BSS informs us of available bandwidth to one MS */ - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx Flow Control MS\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx Flow Control MS\n", bctx->bvci); /* FIXME: actually implement flow control */ /* FIXME: Send FLOW_CONTROL_MS_ACK */ @@ -911,12 +1013,15 @@ static int bssgp_rx_ptp(struct msgb *msg, struct tlv_parsed *tp, case BSSGP_PDUT_STATUS: /* This is already handled in bssgp_rcvmsg() */ break; + case BSSGP_PDUT_BVC_RESET: + rc = bssgp_rx_bvc_reset(msg, tp, bctx->bvci); + break; case BSSGP_PDUT_DOWNLOAD_BSS_PFC: case BSSGP_PDUT_CREATE_BSS_PFC_ACK: case BSSGP_PDUT_CREATE_BSS_PFC_NACK: case BSSGP_PDUT_MODIFY_BSS_PFC: case BSSGP_PDUT_DELETE_BSS_PFC_ACK: - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx PDU type %s not [yet] " + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx PDU type %s not [yet] " "implemented\n", bctx->bvci, bssgp_pdu_str(pdu_type)); rc = bssgp_tx_status(BSSGP_CAUSE_PDU_INCOMP_FEAT, NULL, msg); break; @@ -927,13 +1032,13 @@ static int bssgp_rx_ptp(struct msgb *msg, struct tlv_parsed *tp, case BSSGP_PDUT_RA_CAPA_UPDATE_ACK: case BSSGP_PDUT_FLOW_CONTROL_BVC_ACK: case BSSGP_PDUT_FLOW_CONTROL_MS_ACK: - DEBUGP(DBSSGP, "BSSGP BVCI=%u PDU type %s only exists in DL\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u PDU type %s only exists in DL\n", bctx->bvci, bssgp_pdu_str(pdu_type)); bssgp_tx_status(BSSGP_CAUSE_PROTO_ERR_UNSPEC, NULL, msg); rc = -EINVAL; break; default: - DEBUGP(DBSSGP, "BSSGP BVCI=%u PDU type %s unknown\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u PDU type %s unknown\n", bctx->bvci, bssgp_pdu_str(pdu_type)); rc = bssgp_tx_status(BSSGP_CAUSE_PROTO_ERR_UNSPEC, NULL, msg); break; @@ -964,13 +1069,13 @@ static int bssgp_rx_sign(struct msgb *msg, struct tlv_parsed *tp, break; case BSSGP_PDUT_FLUSH_LL_ACK: /* BSS informs us it has performed LL FLUSH */ - DEBUGP(DBSSGP, "BSSGP Rx BVCI=%u FLUSH LL ACK\n", bvci); + DEBUGP(DLBSSGP, "BSSGP Rx BVCI=%u FLUSH LL ACK\n", bvci); /* FIXME: send NM_FLUSH_LL.res to NM */ break; case BSSGP_PDUT_LLC_DISCARD: /* BSS informs that some LLC PDU's have been discarded */ if (!bctx) { - LOGP(DBSSGP, LOGL_ERROR, + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Rx LLC-DISCARD missing mandatory BVCI\n"); goto err_mand_ie; } @@ -978,9 +1083,9 @@ static int bssgp_rx_sign(struct msgb *msg, struct tlv_parsed *tp, break; case BSSGP_PDUT_BVC_BLOCK: /* BSS tells us that BVC shall be blocked */ - if (!TLVP_PRESENT(tp, BSSGP_IE_BVCI) || - !TLVP_PRESENT(tp, BSSGP_IE_CAUSE)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP Rx BVC-BLOCK " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_BVCI, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_CAUSE, 1)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Rx BVC-BLOCK " "missing mandatory IE\n"); goto err_mand_ie; } @@ -988,21 +1093,21 @@ static int bssgp_rx_sign(struct msgb *msg, struct tlv_parsed *tp, break; case BSSGP_PDUT_BVC_UNBLOCK: /* BSS tells us that BVC shall be unblocked */ - if (!TLVP_PRESENT(tp, BSSGP_IE_BVCI)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP Rx BVC-UNBLOCK " + if (!TLVP_PRES_LEN(tp, BSSGP_IE_BVCI, 2)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Rx BVC-UNBLOCK " "missing mandatory IE\n"); goto err_mand_ie; } rc = bssgp_rx_bvc_unblock(msg, tp); break; case BSSGP_PDUT_BVC_RESET_ACK: - LOGP(DBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx BVC-RESET-ACK\n", bvci); + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP BVCI=%u Rx BVC-RESET-ACK\n", bvci); break; case BSSGP_PDUT_BVC_RESET: - /* BSS tells us that BVC init is required */ - if (!TLVP_PRESENT(tp, BSSGP_IE_BVCI) || - !TLVP_PRESENT(tp, BSSGP_IE_CAUSE)) { - LOGP(DBSSGP, LOGL_ERROR, "BSSGP Rx BVC-RESET " + /* SGSN or BSS tells us that BVC init is required */ + if (!TLVP_PRES_LEN(tp, BSSGP_IE_BVCI, 2) || + !TLVP_PRES_LEN(tp, BSSGP_IE_CAUSE, 1)) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Rx BVC-RESET " "missing mandatory IE\n"); goto err_mand_ie; } @@ -1011,6 +1116,15 @@ static int bssgp_rx_sign(struct msgb *msg, struct tlv_parsed *tp, case BSSGP_PDUT_STATUS: /* This is already handled in bssgp_rcvmsg() */ break; + + case BSSGP_PDUT_RAN_INFO: + case BSSGP_PDUT_RAN_INFO_REQ: + case BSSGP_PDUT_RAN_INFO_ACK: + case BSSGP_PDUT_RAN_INFO_ERROR: + case BSSGP_PDUT_RAN_INFO_APP_ERROR: + rc = bssgp_rx_rim(msg, tp, bvci); + break; + /* those only exist in the SGSN -> BSS direction */ case BSSGP_PDUT_PAGING_PS: case BSSGP_PDUT_PAGING_CS: @@ -1022,13 +1136,13 @@ static int bssgp_rx_sign(struct msgb *msg, struct tlv_parsed *tp, case BSSGP_PDUT_BVC_BLOCK_ACK: case BSSGP_PDUT_BVC_UNBLOCK_ACK: case BSSGP_PDUT_SGSN_INVOKE_TRACE: - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx PDU type %s only exists in DL\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx PDU type %s only exists in DL\n", bvci, bssgp_pdu_str(pdu_type)); bssgp_tx_status(BSSGP_CAUSE_PROTO_ERR_UNSPEC, NULL, msg); rc = -EINVAL; break; default: - DEBUGP(DBSSGP, "BSSGP BVCI=%u Rx PDU type %s unknown\n", + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx PDU type %s unknown\n", bvci, bssgp_pdu_str(pdu_type)); rc = bssgp_tx_status(BSSGP_CAUSE_PROTO_ERR_UNSPEC, NULL, msg); break; @@ -1066,14 +1180,14 @@ int bssgp_rcvmsg(struct msgb *msg) rc = bssgp_tlv_parse(&tp, budh->data, data_len); } if (rc < 0) { - LOGP(DBSSGP, LOGL_ERROR, "Failed to parse BSSGP %s message. Invalid message was: %s\n", + LOGP(DLBSSGP, LOGL_ERROR, "Failed to parse BSSGP %s message. Invalid message was: %s\n", bssgp_pdu_str(pdu_type), msgb_hexdump(msg)); if (pdu_type != BSSGP_PDUT_STATUS) return bssgp_tx_status(BSSGP_CAUSE_INV_MAND_INF, NULL, msg); return rc; } - if (bvci == BVCI_SIGNALLING && TLVP_PRESENT(&tp, BSSGP_IE_BVCI)) + if (bvci == BVCI_SIGNALLING && TLVP_PRES_LEN(&tp, BSSGP_IE_BVCI, 2)) bvci = tlvp_val16be(&tp, BSSGP_IE_BVCI); /* look-up or create the BTS context for this BVC */ @@ -1081,8 +1195,8 @@ int bssgp_rcvmsg(struct msgb *msg) if (bctx) { log_set_context(LOG_CTX_GB_BVC, bctx); - rate_ctr_inc(&bctx->ctrg->ctr[BSSGP_CTR_PKTS_IN]); - rate_ctr_add(&bctx->ctrg->ctr[BSSGP_CTR_BYTES_IN], + rate_ctr_inc(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_PKTS_IN)); + rate_ctr_add(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_BYTES_IN), msgb_bssgp_len(msg)); } @@ -1096,7 +1210,7 @@ int bssgp_rcvmsg(struct msgb *msg) * registered if a BVCI is given. */ if (!bctx && bvci != BVCI_SIGNALLING && pdu_type != BSSGP_PDUT_BVC_RESET) { - LOGP(DBSSGP, LOGL_NOTICE, "NSEI=%u/BVCI=%u Rejecting PDU type %s for unknown BVCI\n", nsei, bvci, + LOGP(DLBSSGP, LOGL_NOTICE, "NSEI=%u/BVCI=%u Rejecting PDU type %s for unknown BVCI\n", nsei, bvci, bssgp_pdu_str(pdu_type)); return bssgp_tx_status(BSSGP_CAUSE_UNKNOWN_BVCI, &bvci, msg); } @@ -1108,7 +1222,7 @@ int bssgp_rcvmsg(struct msgb *msg) else if (bctx) rc = bssgp_rx_ptp(msg, &tp, bctx); else - LOGP(DBSSGP, LOGL_NOTICE, + LOGP(DLBSSGP, LOGL_NOTICE, "NSEI=%u/BVCI=%u Cannot handle PDU type %s for unknown BVCI, NS BVCI %u\n", nsei, bvci, bssgp_pdu_str(pdu_type), ns_bvci); @@ -1132,7 +1246,7 @@ int bssgp_tx_dl_ud(struct msgb *msg, uint16_t pdu_lifetime, /* Identifiers from UP: TLLI, BVCI, NSEI (all in msgb->cb) */ if (bvci <= BVCI_PTM ) { - LOGP(DBSSGP, LOGL_ERROR, "Cannot send DL-UD to BVCI %u\n", + LOGP(DLBSSGP, LOGL_ERROR, "Cannot send DL-UD to BVCI %u\n", bvci); msgb_free(msg); return -EINVAL; @@ -1140,7 +1254,7 @@ int bssgp_tx_dl_ud(struct msgb *msg, uint16_t pdu_lifetime, bctx = btsctx_by_bvci_nsei(bvci, nsei); if (!bctx) { - LOGP(DBSSGP, LOGL_ERROR, "Cannot send DL-UD to unknown BVCI %u\n", + LOGP(DLBSSGP, LOGL_ERROR, "Cannot send DL-UD to unknown BVCI %u\n", bvci); msgb_free(msg); return -ENODEV; @@ -1178,6 +1292,7 @@ int bssgp_tx_dl_ud(struct msgb *msg, uint16_t pdu_lifetime, #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" int imsi_len = gsm48_generate_mid_from_imsi(mi, dup->imsi); + OSMO_ASSERT(imsi_len <= GSM48_MID_MAX_SIZE); if (imsi_len > 2) msgb_tvlv_push(msg, BSSGP_IE_IMSI, imsi_len-2, mi+2); @@ -1205,8 +1320,8 @@ int bssgp_tx_dl_ud(struct msgb *msg, uint16_t pdu_lifetime, budh->tlli = osmo_htonl(msgb_tlli(msg)); budh->pdu_type = BSSGP_PDUT_DL_UNITDATA; - rate_ctr_inc(&bctx->ctrg->ctr[BSSGP_CTR_PKTS_OUT]); - rate_ctr_add(&bctx->ctrg->ctr[BSSGP_CTR_BYTES_OUT], msg->len); + rate_ctr_inc(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_PKTS_OUT)); + rate_ctr_add(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_BYTES_OUT), msg->len); /* Identifiers down: BVCI, NSEI (in msgb->cb) */ @@ -1247,6 +1362,7 @@ int bssgp_tx_paging(uint16_t nsei, uint16_t ns_bvci, * mi[131], which is wrong */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" + OSMO_ASSERT(imsi_len <= GSM48_MID_MAX_SIZE); msgb_tvlv_put(msg, BSSGP_IE_IMSI, imsi_len-2, mi+2); #pragma GCC diagnostic pop /* DRX Parameters */ @@ -1284,12 +1400,14 @@ int bssgp_tx_paging(uint16_t nsei, uint16_t ns_bvci, msgb_tvlv_put(msg, BSSGP_IE_TMSI, 4, (uint8_t *) &ptmsi); } - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } void bssgp_set_log_ss(int ss) { - DBSSGP = ss; + /* BSSGP has moved from DGPRS to DLGPRS, please update your code if it's + * still calling this function + */ } /*! @@ -1310,7 +1428,7 @@ void bssgp_fc_flush_queue(struct bssgp_flow_control *fc) /*! * \brief Flush the queues of all BSSGP contexts. */ -void bssgp_flush_all_queues() +void bssgp_flush_all_queues(void) { struct bssgp_bvc_ctx *bctx; diff --git a/src/gb/gprs_bssgp2.c b/src/gb/gprs_bssgp2.c new file mode 100644 index 00000000..104fe08e --- /dev/null +++ b/src/gb/gprs_bssgp2.c @@ -0,0 +1,486 @@ +/* BSSGP2 - second generation of BSSGP library */ + +/* (C) 2020 Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <osmocom/core/utils.h> +#include <osmocom/core/byteswap.h> +#include <osmocom/core/msgb.h> + +#include <osmocom/gsm/gsm48.h> +#include <osmocom/gsm/tlv.h> + +#include <osmocom/gprs/gprs_ns2.h> +#include <osmocom/gprs/gprs_bssgp.h> +#include <osmocom/gprs/gprs_bssgp2.h> + + +/*! transmit BSSGP PDU over NS (PTP BVC) + * \param[in] nsi NS Instance through which to transmit + * \param[in] nsei NSEI of NSE through which to transmit + * \param[in] bvci BVCI through which to transmit + * \param[in] msg BSSGP PDU to transmit + * \returns 0 on success; negative on error */ +int bssgp2_nsi_tx_ptp(struct gprs_ns2_inst *nsi, uint16_t nsei, uint16_t bvci, + struct msgb *msg, uint32_t lsp) +{ + struct osmo_gprs_ns2_prim nsp = {}; + int rc; + + if (!msg) + return 0; + + nsp.bvci = bvci; + nsp.nsei = nsei; + nsp.u.unitdata.link_selector = lsp; + + osmo_prim_init(&nsp.oph, SAP_NS, GPRS_NS2_PRIM_UNIT_DATA, PRIM_OP_REQUEST, msg); + rc = gprs_ns2_recv_prim(nsi, &nsp.oph); + + return rc; +} + +/*! transmit BSSGP PDU over NS (SIGNALING BVC) + * \param[in] nsi NS Instance through which to transmit + * \param[in] nsei NSEI of NSE through which to transmit + * \param[in] msg BSSGP PDU to transmit + * \returns 0 on success; negative on error */ +int bssgp2_nsi_tx_sig(struct gprs_ns2_inst *nsi, uint16_t nsei, struct msgb *msg, uint32_t lsp) +{ + return bssgp2_nsi_tx_ptp(nsi, nsei, 0, msg, lsp); +} + +/*! Encode BSSGP BVC-BLOCK PDU as per TS 48.018 Section 10.4.8. */ +struct msgb *bssgp2_enc_bvc_block(uint16_t bvci, enum gprs_bssgp_cause cause) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_BLOCK; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, (uint8_t *) &cause); + + return msg; +} + +/*! Encode BSSGP BVC-BLOCK-ACK PDU as per TS 48.018 Section 10.4.9. */ +struct msgb *bssgp2_enc_bvc_block_ack(uint16_t bvci) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_BLOCK_ACK; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + + return msg; +} + +/*! Encode BSSGP BVC-UNBLOCK PDU as per TS 48.018 Section 10.4.10. */ +struct msgb *bssgp2_enc_bvc_unblock(uint16_t bvci) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_UNBLOCK; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + + return msg; +} + +/*! Encode BSSGP BVC-UNBLOCK-ACK PDU as per TS 48.018 Section 10.4.11. */ +struct msgb *bssgp2_enc_bvc_unblock_ack(uint16_t bvci) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_UNBLOCK_ACK; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + + return msg; +} + +/*! Encode BSSGP BVC-RESET PDU as per TS 48.018 Section 10.4.12. + * \param[in] bvci PTP BVCI to encode into the BVCI IE + * \param[in] cause BSSGP Cause value (reason for reset) + * \param[in] ra_id Routing Area ID to be encoded to CELL_ID IE (optional) + * \param[in] cell_id Cell ID to be encoded to CELL_ID IE (only if ra_id is non-NULL) + * \param[in] feat_bm Feature Bitmap (optional) + * \param[in] ext_feat_bm Extended Feature Bitmap (optional) */ +struct msgb *bssgp2_enc_bvc_reset(uint16_t bvci, enum gprs_bssgp_cause cause, + const struct gprs_ra_id *ra_id, uint16_t cell_id, + const uint8_t *feat_bm, const uint8_t *ext_feat_bm) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_RESET; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, (uint8_t *) &cause); + if (ra_id) { + uint8_t bssgp_cid[8]; + bssgp_create_cell_id(bssgp_cid, ra_id, cell_id); + msgb_tvlv_put(msg, BSSGP_IE_CELL_ID, sizeof(bssgp_cid), bssgp_cid); + } + + if (feat_bm) + msgb_tvlv_put(msg, BSSGP_IE_FEATURE_BITMAP, 1, feat_bm); + + if (ext_feat_bm) + msgb_tvlv_put(msg, BSSGP_IE_EXT_FEATURE_BITMAP, 1, feat_bm); + + return msg; +} + +/*! Encode BSSGP BVC-RESET-ACK PDU as per TS 48.018 Section 10.4.13. + * \param[in] bvci PTP BVCI to encode into the BVCI IE + * \param[in] ra_id Routing Area ID to be encoded to CELL_ID IE (optional) + * \param[in] cell_id Cell ID to be encoded to CELL_ID IE (only if ra_id is non-NULL) + * \param[in] feat_bm Feature Bitmap (optional) + * \param[in] ext_feat_bm Extended Feature Bitmap (optional) */ +struct msgb *bssgp2_enc_bvc_reset_ack(uint16_t bvci, const struct gprs_ra_id *ra_id, uint16_t cell_id, + const uint8_t *feat_bm, const uint8_t *ext_feat_bm) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint16_t _bvci = osmo_htons(bvci); + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_BVC_RESET_ACK; + + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + if (ra_id) { + uint8_t bssgp_cid[8]; + bssgp_create_cell_id(bssgp_cid, ra_id, cell_id); + msgb_tvlv_put(msg, BSSGP_IE_CELL_ID, sizeof(bssgp_cid), bssgp_cid); + } + + if (feat_bm) + msgb_tvlv_put(msg, BSSGP_IE_FEATURE_BITMAP, 1, feat_bm); + + if (ext_feat_bm) + msgb_tvlv_put(msg, BSSGP_IE_EXT_FEATURE_BITMAP, 1, feat_bm); + + return msg; +} + +/*! Encode BSSGP STATUS PDU as per TS 48.018 Section 10.4.14. + * \param[in] cause BSSGP Cause value + * \param[in] bvci optional BVCI - only encoded if non-NULL + * \param[in] msg optional message buffer containing PDU in error - only encoded if non-NULL + * \param[in] max_pdu_len Maximum BSSGP PDU size the NS layer accepts */ +struct msgb *bssgp2_enc_status(uint8_t cause, const uint16_t *bvci, const struct msgb *orig_msg, uint16_t max_pdu_len) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_STATUS; + msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, &cause); + /* FIXME: Require/encode BVCI only if cause is BVCI unknown/blocked + * See 3GPP TS 48.018 Ch. 10.4.14 */ + if (bvci) { + uint16_t _bvci = osmo_htons(*bvci); + msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); + } + if (orig_msg) { + uint32_t orig_len, max_orig_len; + /* Calculate how big the reply would be: the BSSGP msg so far + size of the PDU IN ERROR including tvl */ + orig_len = msgb_bssgp_len(orig_msg); + max_orig_len = msgb_length(msg) + TVLV_GROSS_LEN(orig_len); + /* Truncate the difference between max_orig_len and mtu */ + if (max_orig_len > max_pdu_len) + orig_len -= max_orig_len - max_pdu_len; + msgb_tvlv_put(msg, BSSGP_IE_PDU_IN_ERROR, orig_len, msgb_bssgph(orig_msg)); + } + + return msg; +} + +static const unsigned int bssgp_fc_gran_tbl[] = { + [BSSGP_FC_GRAN_100] = 100, + [BSSGP_FC_GRAN_1000] = 1000, + [BSSGP_FC_GRAN_10000] = 10000, + [BSSGP_FC_GRAN_100000] = 100000, +}; + +/*! Decode a FLOW-CONTROL-BVC PDU as per TS 48.018 Section 10.4.4. + * \param[out] fc caller-allocated memory for parsed output + * \param[in] tp pre-parsed TLVs; caller must ensure mandatory IE presence/length + * \returns 0 on success; negative in case of error */ +int bssgp2_dec_fc_bvc(struct bssgp2_flow_ctrl *fc, const struct tlv_parsed *tp) +{ + unsigned int granularity = 100; + + /* optional "Flow Control Granularity IE" (11.3.102); applies to + * bucket_size_max, bucket_leak_rate and PFC FC params IE */ + if (TLVP_PRESENT(tp, BSSGP_IE_FLOW_CTRL_GRANULARITY)) { + uint8_t gran = *TLVP_VAL(tp, BSSGP_IE_FLOW_CTRL_GRANULARITY); + granularity = bssgp_fc_gran_tbl[gran & 3]; + } + + /* mandatory IEs */ + fc->tag = *TLVP_VAL(tp, BSSGP_IE_TAG); + fc->bucket_size_max = granularity * tlvp_val16be(tp, BSSGP_IE_BVC_BUCKET_SIZE); + fc->bucket_leak_rate = (granularity * tlvp_val16be(tp, BSSGP_IE_BUCKET_LEAK_RATE)) / 8; + fc->u.bvc.bmax_default_ms = granularity * tlvp_val16be(tp, BSSGP_IE_BMAX_DEFAULT_MS); + fc->u.bvc.r_default_ms = (granularity * tlvp_val16be(tp, BSSGP_IE_R_DEFAULT_MS)) / 8; + + /* optional / conditional */ + if (TLVP_PRESENT(tp, BSSGP_IE_BUCKET_FULL_RATIO)) { + fc->bucket_full_ratio_present = true; + fc->bucket_full_ratio = *TLVP_VAL(tp, BSSGP_IE_BUCKET_FULL_RATIO); + } else { + fc->bucket_full_ratio_present = false; + } + + if (TLVP_PRESENT(tp, BSSGP_IE_BVC_MEASUREMENT)) { + uint16_t val = tlvp_val16be(tp, BSSGP_IE_BVC_MEASUREMENT); + fc->u.bvc.measurement_present = true; + /* convert from centi-seconds to milli-seconds */ + if (val == 0xffff) + fc->u.bvc.measurement = 0xffffffff; + else + fc->u.bvc.measurement = val * 10; + } else { + fc->u.bvc.measurement_present = false; + } + + return 0; + +} + +/*! Encode a FLOW-CONTROL-BVC PDU as per TS 48.018 Section 10.4.4. + * \param[in] fc structure describing to-be-encoded FC parameters + * \param[in] gran if non-NULL: Encode using specified unit granularity + * \returns encoded PDU or NULL in case of error */ +struct msgb *bssgp2_enc_fc_bvc(const struct bssgp2_flow_ctrl *fc, enum bssgp_fc_granularity *gran) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + unsigned int granularity = 100; + + if (gran) + granularity = bssgp_fc_gran_tbl[*gran & 3]; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_FLOW_CONTROL_BVC; + + msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &fc->tag); + msgb_tvlv_put_16be(msg, BSSGP_IE_BVC_BUCKET_SIZE, fc->bucket_size_max / granularity); + msgb_tvlv_put_16be(msg, BSSGP_IE_BUCKET_LEAK_RATE, fc->bucket_leak_rate * 8 / granularity); + msgb_tvlv_put_16be(msg, BSSGP_IE_BMAX_DEFAULT_MS, fc->u.bvc.bmax_default_ms / granularity); + msgb_tvlv_put_16be(msg, BSSGP_IE_R_DEFAULT_MS, fc->u.bvc.r_default_ms * 8 / granularity); + + if (fc->bucket_full_ratio_present) + msgb_tvlv_put(msg, BSSGP_IE_BUCKET_FULL_RATIO, 1, &fc->bucket_full_ratio); + + if (fc->u.bvc.measurement_present) { + uint16_t val; + /* convert from ms to cs */ + if (fc->u.bvc.measurement == 0xffffffff) + val = 0xffff; + else + val = fc->u.bvc.measurement / 10; + msgb_tvlv_put_16be(msg, BSSGP_IE_BVC_MEASUREMENT, val); + } + + if (gran) { + uint8_t val = *gran & 3; + msgb_tvlv_put(msg, BSSGP_IE_FLOW_CTRL_GRANULARITY, 1, &val); + } + + return msg; +} + +/*! Encode BSSGP FLUSH-LL PDU as per TS 48.018 Section 10.4.1. + * \param[in] tlli - the TLLI of the MS + * \param[in] old_bvci BVCI + * \param[in] new_bvci2 optional BVCI - only encoded if non-NULL + * \param[in] nsei optional - only encoded if non-NULL + * \returns encoded PDU or NULL in case of error */ +struct msgb *bssgp2_enc_flush_ll(uint32_t tlli, uint16_t old_bvci, + const uint16_t *new_bvci, const uint16_t *nsei) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_FLUSH_LL; + + msgb_tvlv_put_32be(msg, BSSGP_IE_TLLI, tlli); + msgb_tvlv_put_16be(msg, BSSGP_IE_BVCI, old_bvci); + if (new_bvci) + msgb_tvlv_put_16be(msg, BSSGP_IE_BVCI, *new_bvci); + + if (nsei) + msgb_tvlv_put_16be(msg, BSSGP_IE_BVCI, *nsei); + + return msg; +} + +/*! Encode a FLOW-CONTROL-BVC-ACK PDU as per TS 48.018 Section 10.4.4. + * \param[in] tag the tag IE value to encode + * \returns encoded PDU or NULL in case of error */ +struct msgb *bssgp2_enc_fc_bvc_ack(uint8_t tag) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_FLOW_CONTROL_BVC_ACK; + + msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &tag); + + return msg; +} + +/*! Decode a FLOW-CONTROL-MS PDU as per TS 48.018 Section 10.4.6. + * \param[out] fc caller-allocated memory for parsed output + * \param[in] tp pre-parsed TLVs; caller must ensure mandatory IE presence/length + * \returns 0 on success; negative in case of error */ +int bssgp2_dec_fc_ms(struct bssgp2_flow_ctrl *fc, struct tlv_parsed *tp) +{ + unsigned int granularity = 100; + + /* optional "Flow Control Granularity IE" (11.3.102); applies to + * bucket_size_max, bucket_leak_rate and PFC FC params IE */ + if (TLVP_PRESENT(tp, BSSGP_IE_FLOW_CTRL_GRANULARITY)) { + uint8_t gran = *TLVP_VAL(tp, BSSGP_IE_FLOW_CTRL_GRANULARITY); + granularity = bssgp_fc_gran_tbl[gran & 3]; + } + + /* mandatory IEs */ + fc->u.ms.tlli = tlvp_val32be(tp, BSSGP_IE_TLLI); + fc->tag = *TLVP_VAL(tp, BSSGP_IE_TAG); + fc->bucket_size_max = granularity * tlvp_val16be(tp, BSSGP_IE_MS_BUCKET_SIZE); + fc->bucket_leak_rate = (granularity * tlvp_val16be(tp, BSSGP_IE_BUCKET_LEAK_RATE)) / 8; + + /* optional / conditional */ + if (TLVP_PRESENT(tp, BSSGP_IE_BUCKET_FULL_RATIO)) { + fc->bucket_full_ratio_present = true; + fc->bucket_full_ratio = *TLVP_VAL(tp, BSSGP_IE_BUCKET_FULL_RATIO); + } else { + fc->bucket_full_ratio_present = false; + } + + return 0; +} + +/*! Encode a FLOW-CONTROL-MS PDU as per TS 48.018 Section 10.4.6. + * \param[in] fc structure describing to-be-encoded FC parameters + * \param[in] gran if non-NULL: Encode using specified unit granularity + * \returns encoded PDU or NULL in case of error */ +struct msgb *bssgp2_enc_fc_ms(const struct bssgp2_flow_ctrl *fc, enum bssgp_fc_granularity *gran) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + unsigned int granularity = 100; + + if (gran) + granularity = bssgp_fc_gran_tbl[*gran & 3]; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_FLOW_CONTROL_MS; + + msgb_tvlv_put_32be(msg, BSSGP_IE_TLLI, fc->u.ms.tlli); + msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &fc->tag); + msgb_tvlv_put_16be(msg, BSSGP_IE_MS_BUCKET_SIZE, fc->bucket_size_max / granularity); + msgb_tvlv_put_16be(msg, BSSGP_IE_BUCKET_LEAK_RATE, fc->bucket_leak_rate * 8 / granularity); + + if (fc->bucket_full_ratio_present) + msgb_tvlv_put(msg, BSSGP_IE_BUCKET_FULL_RATIO, 1, &fc->bucket_full_ratio); + + if (gran) { + uint8_t val = *gran & 3; + msgb_tvlv_put(msg, BSSGP_IE_FLOW_CTRL_GRANULARITY, 1, &val); + } + + return msg; +} + +/*! Encode a FLOW-CONTROL-BVC-ACK PDU as per TS 48.018 Section 10.4.7. + * \param[in] tlli the TLLI IE value to encode + * \param[in] tag the tag IE value to encode + * \returns encoded PDU or NULL in case of error */ +struct msgb *bssgp2_enc_fc_ms_ack(uint32_t tlli, uint8_t tag) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + + if (!msg) + return NULL; + + bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); + bgph->pdu_type = BSSGP_PDUT_FLOW_CONTROL_MS_ACK; + + msgb_tvlv_put_32be(msg, BSSGP_IE_TLLI, tlli); + msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &tag); + + return msg; +} diff --git a/src/gb/gprs_bssgp_bss.c b/src/gb/gprs_bssgp_bss.c index f06c403f..8230d871 100644 --- a/src/gb/gprs_bssgp_bss.c +++ b/src/gb/gprs_bssgp_bss.c @@ -34,7 +34,7 @@ #include <osmocom/gprs/gprs_bssgp_bss.h> #include <osmocom/gprs/gprs_ns.h> -#include "common_vty.h" +#include "gprs_bssgp_internal.h" #define GSM_IMSI_LENGTH 17 @@ -60,7 +60,7 @@ int bssgp_tx_suspend(uint16_t nsei, uint32_t tlli, struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=0) Tx SUSPEND (TLLI=0x%04x)\n", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=0) Tx SUSPEND (TLLI=0x%04x)\n", tlli); msgb_nsei(msg) = nsei; msgb_bvci(msg) = 0; /* Signalling */ @@ -69,7 +69,7 @@ int bssgp_tx_suspend(uint16_t nsei, uint32_t tlli, bssgp_msgb_tlli_put(msg, tlli); bssgp_msgb_ra_put(msg, ra_id); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! GMM-RESUME.req (Chapter 10.3.9) */ @@ -80,7 +80,7 @@ int bssgp_tx_resume(uint16_t nsei, uint32_t tlli, struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=0) Tx RESUME (TLLI=0x%04x)\n", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=0) Tx RESUME (TLLI=0x%04x)\n", tlli); msgb_nsei(msg) = nsei; msgb_bvci(msg) = 0; /* Signalling */ @@ -91,7 +91,7 @@ int bssgp_tx_resume(uint16_t nsei, uint32_t tlli, msgb_tvlv_put(msg, BSSGP_IE_SUSPEND_REF_NR, 1, &suspend_ref); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit RA-CAPABILITY-UPDATE (10.3.3) */ @@ -101,7 +101,7 @@ int bssgp_tx_ra_capa_upd(struct bssgp_bvc_ctx *bctx, uint32_t tlli, uint8_t tag) struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx RA-CAPA-UPD (TLLI=0x%04x)\n", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx RA-CAPA-UPD (TLLI=0x%04x)\n", bctx->bvci, tlli); /* set NSEI and BVCI in msgb cb */ @@ -113,7 +113,7 @@ int bssgp_tx_ra_capa_upd(struct bssgp_bvc_ctx *bctx, uint32_t tlli, uint8_t tag) msgb_tvlv_put(msg, BSSGP_IE_TAG, 1, &tag); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* first common part of RADIO-STATUS */ @@ -123,7 +123,7 @@ static struct msgb *common_tx_radio_status(struct bssgp_bvc_ctx *bctx) struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx RADIO-STATUS ", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx RADIO-STATUS ", bctx->bvci); /* set NSEI and BVCI in msgb cb */ @@ -139,9 +139,9 @@ static struct msgb *common_tx_radio_status(struct bssgp_bvc_ctx *bctx) static int common_tx_radio_status2(struct msgb *msg, uint8_t cause) { msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, &cause); - LOGPC(DBSSGP, LOGL_NOTICE, "CAUSE=%s\n", bssgp_cause_str(cause)); + LOGPC(DLBSSGP, LOGL_NOTICE, "CAUSE=%s\n", bssgp_cause_str(cause)); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit RADIO-STATUS for TLLI (10.3.5) */ @@ -153,7 +153,7 @@ int bssgp_tx_radio_status_tlli(struct bssgp_bvc_ctx *bctx, uint8_t cause, if (!msg) return -ENOMEM; bssgp_msgb_tlli_put(msg, tlli); - LOGPC(DBSSGP, LOGL_NOTICE, "TLLI=0x%08x ", tlli); + LOGPC(DLBSSGP, LOGL_NOTICE, "TLLI=0x%08x ", tlli); return common_tx_radio_status2(msg, cause); } @@ -168,7 +168,7 @@ int bssgp_tx_radio_status_tmsi(struct bssgp_bvc_ctx *bctx, uint8_t cause, if (!msg) return -ENOMEM; msgb_tvlv_put(msg, BSSGP_IE_TMSI, 4, (uint8_t *)&_tmsi); - LOGPC(DBSSGP, LOGL_NOTICE, "TMSI=0x%08x ", tmsi); + LOGPC(DLBSSGP, LOGL_NOTICE, "TMSI=0x%08x ", tmsi); return common_tx_radio_status2(msg, cause); } @@ -189,11 +189,12 @@ int bssgp_tx_radio_status_imsi(struct bssgp_bvc_ctx *bctx, uint8_t cause, * mi[131], which is wrong */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" + OSMO_ASSERT(imsi_len <= GSM48_MID_MAX_SIZE); /* strip the MI type and length values (2 bytes) */ if (imsi_len > 2) msgb_tvlv_put(msg, BSSGP_IE_IMSI, imsi_len-2, mi+2); #pragma GCC diagnostic pop - LOGPC(DBSSGP, LOGL_NOTICE, "IMSI=%s ", imsi); + LOGPC(DLBSSGP, LOGL_NOTICE, "IMSI=%s ", imsi); return common_tx_radio_status2(msg, cause); } @@ -219,7 +220,7 @@ int bssgp_tx_flush_ll_ack(struct bssgp_bvc_ctx *bctx, uint32_t tlli, msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci_new); msgb_tvlv_put(msg, BSSGP_IE_NUM_OCT_AFF, 3, (uint8_t *) &_oct_aff); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit LLC-DISCARDED (Chapter 10.4.3) */ @@ -232,7 +233,7 @@ int bssgp_tx_llc_discarded(struct bssgp_bvc_ctx *bctx, uint32_t tlli, uint16_t _bvci = osmo_htons(bctx->bvci); uint32_t _oct_aff = osmo_htonl(num_octets & 0xFFFFFF); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx LLC-DISCARDED " + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx LLC-DISCARDED " "TLLI=0x%04x, FRAMES=%u, OCTETS=%u\n", bctx->bvci, tlli, num_frames, num_octets); msgb_nsei(msg) = bctx->nsei; @@ -245,7 +246,7 @@ int bssgp_tx_llc_discarded(struct bssgp_bvc_ctx *bctx, uint32_t tlli, msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); msgb_tvlv_put(msg, BSSGP_IE_NUM_OCT_AFF, 3, ((uint8_t *) &_oct_aff) + 1); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit a BVC-BLOCK message (Chapter 10.4.8) */ @@ -256,7 +257,7 @@ int bssgp_tx_bvc_block(struct bssgp_bvc_ctx *bctx, uint8_t cause) (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); uint16_t _bvci = osmo_htons(bctx->bvci); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-BLOCK " + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-BLOCK " "CAUSE=%s\n", bctx->bvci, bssgp_cause_str(cause)); msgb_nsei(msg) = bctx->nsei; @@ -266,7 +267,7 @@ int bssgp_tx_bvc_block(struct bssgp_bvc_ctx *bctx, uint8_t cause) msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, &cause); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit a BVC-UNBLOCK message (Chapter 10.4.10) */ @@ -277,7 +278,7 @@ int bssgp_tx_bvc_unblock(struct bssgp_bvc_ctx *bctx) (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); uint16_t _bvci = osmo_htons(bctx->bvci); - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-UNBLOCK\n", bctx->bvci); + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-UNBLOCK\n", bctx->bvci); msgb_nsei(msg) = bctx->nsei; msgb_bvci(msg) = 0; /* Signalling */ @@ -285,34 +286,29 @@ int bssgp_tx_bvc_unblock(struct bssgp_bvc_ctx *bctx) msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit a BVC-RESET message (Chapter 10.4.12) */ +int bssgp_tx_bvc_reset2(struct bssgp_bvc_ctx *bctx, uint16_t bvci, uint8_t cause, bool add_cell_id) +{ + if (add_cell_id) + return bssgp_tx_bvc_reset_nsei_bvci(bctx->nsei, bvci, cause, &bctx->ra_id, bctx->cell_id); + else + return bssgp_tx_bvc_reset_nsei_bvci(bctx->nsei, bvci, cause, NULL, 0); +} int bssgp_tx_bvc_reset(struct bssgp_bvc_ctx *bctx, uint16_t bvci, uint8_t cause) { - struct msgb *msg = bssgp_msgb_alloc(); - struct bssgp_normal_hdr *bgph = - (struct bssgp_normal_hdr *) msgb_put(msg, sizeof(*bgph)); - uint16_t _bvci = osmo_htons(bvci); - - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP (BVCI=%u) Tx BVC-RESET " - "CAUSE=%s\n", bvci, bssgp_cause_str(cause)); - - msgb_nsei(msg) = bctx->nsei; - msgb_bvci(msg) = 0; /* Signalling */ - bgph->pdu_type = BSSGP_PDUT_BVC_RESET; - - msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); - msgb_tvlv_put(msg, BSSGP_IE_CAUSE, 1, &cause); - if (bvci != BVCI_PTM) { - uint8_t bssgp_cid[8]; - bssgp_create_cell_id(bssgp_cid, &bctx->ra_id, bctx->cell_id); - msgb_tvlv_put(msg, BSSGP_IE_CELL_ID, sizeof(bssgp_cid), bssgp_cid); + /* The Cell Identifier IE is mandatory in the BVC-RESET PDU sent from BSS to SGSN in order to reset a + * BVC corresponding to a PTP functional entity. The Cell Identifier IE shall not be used in any other + * BVC-RESET PDU. */ + switch (bvci) { + case BVCI_SIGNALLING: + case BVCI_PTM: + return bssgp_tx_bvc_reset2(bctx, bvci, cause, false); + default: + return bssgp_tx_bvc_reset2(bctx, bvci, cause, true); } - /* Optional: Feature Bitmap */ - - return gprs_ns_sendmsg(bssgp_nsi, msg); } /*! Transmit a FLOW_CONTROL-BVC (Chapter 10.4.4) @@ -384,7 +380,7 @@ int bssgp_tx_fc_bvc(struct bssgp_bvc_ctx *bctx, uint8_t tag, sizeof(e_queue_delay), (uint8_t *) &e_queue_delay); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! Transmit a FLOW_CONTROL-MS (Chapter 10.4.6) @@ -427,7 +423,7 @@ int bssgp_tx_fc_ms(struct bssgp_bvc_ctx *bctx, uint32_t tlli, uint8_t tag, msgb_tvlv_put(msg, BSSGP_IE_BUCKET_FULL_RATIO, 1, bucket_full_ratio); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /*! RL-UL-UNITDATA.req (Chapter 10.2.2) @@ -470,10 +466,10 @@ int bssgp_tx_ul_ud(struct bssgp_bvc_ctx *bctx, uint32_t tlli, msgb_nsei(msg) = bctx->nsei; msgb_bvci(msg) = bctx->bvci; - rate_ctr_inc(&bctx->ctrg->ctr[BSSGP_CTR_PKTS_OUT]); - rate_ctr_add(&bctx->ctrg->ctr[BSSGP_CTR_BYTES_OUT], msg->len); + rate_ctr_inc(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_PKTS_OUT)); + rate_ctr_add(rate_ctr_group_get_ctr(bctx->ctrg, BSSGP_CTR_BYTES_OUT), msg->len); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* Parse a single GMM-PAGING.req to a given NSEI/NS-BVCI */ @@ -514,24 +510,24 @@ int bssgp_rx_paging(struct bssgp_paging_info *pinfo, TLVP_LEN(&tp, BSSGP_IE_IMSI)); /* DRX Parameters */ - if (!TLVP_PRESENT(&tp, BSSGP_IE_DRX_PARAMS)) + if (!TLVP_PRES_LEN(&tp, BSSGP_IE_DRX_PARAMS, 2)) goto err_mand_ie; pinfo->drx_params = tlvp_val16be(&tp, BSSGP_IE_DRX_PARAMS); /* Scope */ - if (TLVP_PRESENT(&tp, BSSGP_IE_BSS_AREA_ID)) { + if (TLVP_PRES_LEN(&tp, BSSGP_IE_BSS_AREA_ID, 1)) { pinfo->scope = BSSGP_PAGING_BSS_AREA; - } else if (TLVP_PRESENT(&tp, BSSGP_IE_LOCATION_AREA)) { + } else if (TLVP_PRES_LEN(&tp, BSSGP_IE_LOCATION_AREA, 5)) { pinfo->scope = BSSGP_PAGING_LOCATION_AREA; memcpy(ra, TLVP_VAL(&tp, BSSGP_IE_LOCATION_AREA), TLVP_LEN(&tp, BSSGP_IE_LOCATION_AREA)); gsm48_parse_ra(&pinfo->raid, ra); - } else if (TLVP_PRESENT(&tp, BSSGP_IE_ROUTEING_AREA)) { + } else if (TLVP_PRES_LEN(&tp, BSSGP_IE_ROUTEING_AREA, 6)) { pinfo->scope = BSSGP_PAGING_ROUTEING_AREA; memcpy(ra, TLVP_VAL(&tp, BSSGP_IE_ROUTEING_AREA), TLVP_LEN(&tp, BSSGP_IE_ROUTEING_AREA)); gsm48_parse_ra(&pinfo->raid, ra); - } else if (TLVP_PRESENT(&tp, BSSGP_IE_BVCI)) { + } else if (TLVP_PRES_LEN(&tp, BSSGP_IE_BVCI, 2)) { pinfo->scope = BSSGP_PAGING_BVCI; pinfo->bvci = tlvp_val16be(&tp, BSSGP_IE_BVCI); } else @@ -549,8 +545,7 @@ int bssgp_rx_paging(struct bssgp_paging_info *pinfo, } /* Optional (P-)TMSI */ - if (TLVP_PRESENT(&tp, BSSGP_IE_TMSI) && - TLVP_LEN(&tp, BSSGP_IE_TMSI) >= 4) { + if (TLVP_PRES_LEN(&tp, BSSGP_IE_TMSI, 4)) { if (!pinfo->ptmsi) pinfo->ptmsi = talloc_zero_size(pinfo, sizeof(uint32_t)); *(pinfo->ptmsi) = osmo_load32be(TLVP_VAL(&tp, BSSGP_IE_TMSI)); diff --git a/src/gb/gprs_bssgp_internal.h b/src/gb/gprs_bssgp_internal.h new file mode 100644 index 00000000..5022d32d --- /dev/null +++ b/src/gb/gprs_bssgp_internal.h @@ -0,0 +1,9 @@ + +#pragma once + +#include <osmocom/gprs/gprs_bssgp.h> + +extern bssgp_bvc_send bssgp_ns_send; +extern void *bssgp_ns_send_data; + +int bssgp_rx_rim(struct msgb *msg, struct tlv_parsed *tp, uint16_t bvci); diff --git a/src/gb/gprs_bssgp_rim.c b/src/gb/gprs_bssgp_rim.c new file mode 100644 index 00000000..9c09e1ef --- /dev/null +++ b/src/gb/gprs_bssgp_rim.c @@ -0,0 +1,1261 @@ +/*! \file gprs_bssgp.c + * GPRS BSSGP RIM protocol implementation as per 3GPP TS 48.018. */ +/* + * (C) 2020-2021 by sysmocom - s.f.m.c. GmbH + * Author: Philipp Maier <pmaier@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> +#include <osmocom/gprs/gprs_bssgp.h> +#include <osmocom/gprs/gprs_bssgp_rim.h> +#include <osmocom/gsm/gsm0808_utils.h> +#include "gprs_bssgp_internal.h" + +/* TVLV IEs use a variable length field. To be sure we will do all buffer + * length checks with the maximum possible header length, which is + * 1 octet tag + 2 octets length = 3 */ +#define TVLV_HDR_MAXLEN 3 + +/* Usually RIM application containers and their surrounding RIM containers + * are not likely to exceed 128 octets, so the usual header length will be 2 */ +#define TVLV_HDR_LEN 2 + +/* The reporting cell identifier is encoded as a cell identifier IE + * (3GPP TS 48.018, sub-clause 11.3.9) but without IE and length octets. */ +#define REP_CELL_ID_LEN 8 + +const struct value_string bssgp_rim_routing_info_discr_strs[] = { + { BSSGP_RIM_ROUTING_INFO_GERAN, "GERAN-cell" }, + { BSSGP_RIM_ROUTING_INFO_UTRAN, "UTRAN-RNC" }, + { BSSGP_RIM_ROUTING_INFO_EUTRAN, "E-UTRAN-eNodeB/HeNB" }, + { 0, NULL } +}; + +/*! Parse a RIM Routing address IE (3GPP TS 29.060, chapter 7.7.57 and 7.7.77). + * \param[out] ri user provided memory to store the parsed results. + * \param[in] buf input buffer of the value part of the RIM Routing address IE. + * \discr[in] discr value part (one byte) of the RIM Routing Address Discriminator IE. + * \returns length of parsed octets, -EINVAL on error. */ +int bssgp_parse_rim_ra(struct bssgp_rim_routing_info *ri, const uint8_t *buf, + unsigned int len, uint8_t discr) +{ + struct gprs_ra_id raid_temp; + + memset(ri, 0, sizeof(*ri)); + if (len < 2) + return -EINVAL; + + ri->discr = discr; + + switch (ri->discr) { + case BSSGP_RIM_ROUTING_INFO_GERAN: + if (len < 8) + return -EINVAL; + ri->geran.cid = bssgp_parse_cell_id(&ri->geran.raid, buf); + return 8; + case BSSGP_RIM_ROUTING_INFO_UTRAN: + if (len < 8) + return -EINVAL; + gsm48_parse_ra(&ri->utran.raid, buf); + ri->utran.rncid = osmo_load16be(buf + 6); + return 8; + case BSSGP_RIM_ROUTING_INFO_EUTRAN: + if (len < 6 || len > 13) + return -EINVAL; + /* Note: 3GPP TS 24.301 Figure 9.9.3.32.1 and 3GPP TS 24.008 + * Figure 10.5.130 specify MCC/MNC encoding in the same way, + * so we can re-use gsm48_parse_ra() for that. */ + gsm48_parse_ra(&raid_temp, buf); + ri->eutran.tai.mcc = raid_temp.mcc; + ri->eutran.tai.mnc = raid_temp.mnc; + ri->eutran.tai.mnc_3_digits = raid_temp.mnc_3_digits; + ri->eutran.tai.tac = osmo_load16be(buf + 3); + memcpy(ri->eutran.global_enb_id, buf + 5, len - 5); + ri->eutran.global_enb_id_len = len - 5; + return len; + default: + return -EINVAL; + } +} + +/*! Parse a RIM Routing information IE (3GPP TS 48.018, chapter 11.3.70). + * \param[out] ri user provided memory to store the parsed results. + * \param[in] buf input buffer of the value part of the IE. + * \returns length of parsed octets, -EINVAL on error. */ +int bssgp_parse_rim_ri(struct bssgp_rim_routing_info *ri, const uint8_t *buf, + unsigned int len) +{ + uint8_t discr; + int rc; + + if (len < 1) + return -EINVAL; + + discr = buf[0] & 0x0f; + + rc = bssgp_parse_rim_ra(ri, buf + 1, len - 1, discr); + if (rc < 0) + return rc; + return rc + 1; +} + +/*! Encode a RIM Routing information IE (3GPP TS 48.018, chapter 11.3.70). + * \param[out] buf user provided memory (at least 14 byte) for the generated value part of the IE. + * \param[in] ri user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_create_rim_ri(uint8_t *buf, const struct bssgp_rim_routing_info *ri) +{ + int rc; + struct gprs_ra_id raid_temp; + int len; + + buf[0] = ri->discr & 0x0f; + buf++; + + switch (ri->discr) { + case BSSGP_RIM_ROUTING_INFO_GERAN: + rc = bssgp_create_cell_id(buf, &ri->geran.raid, ri->geran.cid); + if (rc < 0) + return -EINVAL; + len = rc + 1; + break; + case BSSGP_RIM_ROUTING_INFO_UTRAN: + gsm48_encode_ra((struct gsm48_ra_id *)buf, &ri->utran.raid); + osmo_store16be(ri->utran.rncid, buf + 6); + len = 9; + break; + case BSSGP_RIM_ROUTING_INFO_EUTRAN: + /* Note: 3GPP TS 24.301 Figure 9.9.3.32.1 and 3GPP TS 24.008 + * Figure 10.5.130 specify MCC/MNC encoding in the same way, + * so we can re-use gsm48_encode_ra() for that. */ + raid_temp = (struct gprs_ra_id) { + .mcc = ri->eutran.tai.mcc, + .mnc = ri->eutran.tai.mnc, + .mnc_3_digits = ri->eutran.tai.mnc_3_digits, + }; + + gsm48_encode_ra((struct gsm48_ra_id *)buf, &raid_temp); + osmo_store16be(ri->eutran.tai.tac, buf + 3); + OSMO_ASSERT(ri->eutran.global_enb_id_len <= + sizeof(ri->eutran.global_enb_id)); + memcpy(buf + 5, ri->eutran.global_enb_id, + ri->eutran.global_enb_id_len); + len = ri->eutran.global_enb_id_len + 6; + break; + default: + return -EINVAL; + } + + OSMO_ASSERT(len <= BSSGP_RIM_ROUTING_INFO_MAXLEN); + return len; +} + +/*! Encode a RIM Routing information into a human readable string. + * \param[buf] user provided string buffer to store the resulting string. + * \param[buf_len] maximum length of string buffer. + * \param[in] ri user provided input data struct. + * \returns pointer to the beginning of the resulting string stored in string buffer. */ +char *bssgp_rim_ri_name_buf(char *buf, size_t buf_len, const struct bssgp_rim_routing_info *ri) +{ + char plmn_str[16]; + char enb_id_str[16]; + char g_id_ps_str[32]; + struct osmo_plmn_id plmn; + struct osmo_cell_global_id_ps g_id_ps; + + if (!ri) + return NULL; + + switch (ri->discr) { + case BSSGP_RIM_ROUTING_INFO_GERAN: + g_id_ps.rai.rac = ri->geran.raid.rac; + g_id_ps.rai.lac.lac = ri->geran.raid.lac; + g_id_ps.rai.lac.plmn.mcc = ri->geran.raid.mcc; + g_id_ps.rai.lac.plmn.mnc_3_digits = ri->geran.raid.mnc_3_digits; + g_id_ps.rai.lac.plmn.mnc = ri->geran.raid.mnc; + g_id_ps.cell_identity = ri->geran.cid; + snprintf(buf, buf_len, "%s-%s", bssgp_rim_routing_info_discr_str(ri->discr), + osmo_cgi_ps_name_buf(g_id_ps_str, sizeof(g_id_ps_str), &g_id_ps)); + break; + case BSSGP_RIM_ROUTING_INFO_UTRAN: + g_id_ps.rai.rac = ri->utran.raid.rac; + g_id_ps.rai.lac.lac = ri->utran.raid.lac; + g_id_ps.rai.lac.plmn.mcc = ri->utran.raid.mcc; + g_id_ps.rai.lac.plmn.mnc_3_digits = ri->utran.raid.mnc_3_digits; + g_id_ps.rai.lac.plmn.mnc = ri->utran.raid.mnc; + g_id_ps.cell_identity = ri->utran.rncid; + snprintf(buf, buf_len, "%s-%s", bssgp_rim_routing_info_discr_str(ri->discr), + osmo_cgi_ps_name_buf(g_id_ps_str, sizeof(g_id_ps_str), &g_id_ps)); + break; + case BSSGP_RIM_ROUTING_INFO_EUTRAN: + plmn.mcc = ri->eutran.tai.mcc; + plmn.mnc = ri->eutran.tai.mnc; + plmn.mnc_3_digits = ri->eutran.tai.mnc_3_digits; + snprintf(buf, buf_len, "%s-%s-%u-%s", bssgp_rim_routing_info_discr_str(ri->discr), + osmo_plmn_name_buf(plmn_str, sizeof(plmn_str), &plmn), ri->eutran.tai.tac, + osmo_hexdump_buf(enb_id_str, sizeof(enb_id_str), ri->eutran.global_enb_id, + ri->eutran.global_enb_id_len, "", false)); + break; + default: + snprintf(buf, buf_len, "invalid"); + } + + return buf; +} + +/*! Encode a RIM Routing information into a human readable string. + * \param[in] ri user provided input data struct. + * \returns pointer to the resulting string. */ +const char *bssgp_rim_ri_name(const struct bssgp_rim_routing_info *ri) +{ + static __thread char rim_ri_buf[64]; + return bssgp_rim_ri_name_buf(rim_ri_buf, sizeof(rim_ri_buf), ri); +} + +/*! Decode a RAN Information Request Application Container for NACC (3GPP TS 48.018, section 11.3.63.1.1). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_req_app_cont_nacc(struct bssgp_ran_inf_req_app_cont_nacc *cont, const uint8_t *buf, size_t len) +{ + int rc; + + if (len < REP_CELL_ID_LEN) + return -EINVAL; + + rc = gsm0808_decode_cell_id_u((union gsm0808_cell_id_u*)&cont->reprt_cell, + CELL_IDENT_WHOLE_GLOBAL_PS, buf, len); + if (rc < 0) + return -EINVAL; + + return 0; +} + +/*! Encode a RAN Information Request Application Container for NACC (3GPP TS 48.018, section 11.3.63.1.1). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_req_app_cont_nacc(uint8_t *buf, size_t len, const struct bssgp_ran_inf_req_app_cont_nacc *cont) +{ + int rc; + struct gprs_ra_id *raid; + + if (len < REP_CELL_ID_LEN) + return -EINVAL; + + raid = (struct gprs_ra_id *)&cont->reprt_cell.rai; + rc = bssgp_create_cell_id(buf, raid, cont->reprt_cell.cell_identity); + if (rc < 0) + return -EINVAL; + return rc; +} + +/*! Decode a RAN Information Application Container (3GPP TS 48.018, section 11.3.63.2.1). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_app_cont_nacc(struct bssgp_ran_inf_app_cont_nacc *cont, const uint8_t *buf, size_t len) +{ + unsigned int i; + int remaining_buf_len; + int rc; + + /* The given buffer must at least contain a reporting cell identifer + * plus one octet that defines number/type of attached sysinfo messages. */ + if (len < REP_CELL_ID_LEN + 1) + return -EINVAL; + + rc = gsm0808_decode_cell_id_u((union gsm0808_cell_id_u*)&cont->reprt_cell, + CELL_IDENT_WHOLE_GLOBAL_PS, buf, len); + if (rc < 0) + return -EINVAL; + + buf += REP_CELL_ID_LEN; + + cont->type_psi = buf[0] & 1; + cont->num_si = buf[0] >> 1; + buf++; + + /* The number of sysinfo messages may be zero */ + if (cont->num_si == 0) + return 0; + + /* Check if the prospected system information messages fit in the + * remaining buffer space */ + remaining_buf_len = len - REP_CELL_ID_LEN - 1; + if (remaining_buf_len <= 0) + return -EINVAL; + if (cont->type_psi && remaining_buf_len / BSSGP_RIM_PSI_LEN < cont->num_si) + return -EINVAL; + else if (remaining_buf_len / BSSGP_RIM_SI_LEN < cont->num_si) + return -EINVAL; + + for (i = 0; i < cont->num_si; i++) { + cont->si[i] = buf; + if (cont->type_psi) + buf += BSSGP_RIM_PSI_LEN; + else + buf += BSSGP_RIM_SI_LEN; + } + + return 0; +} + +/*! Encode a RAN Information Application Container (3GPP TS 48.018, section 11.3.63.2.1). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_app_cont_nacc(uint8_t *buf, size_t len, const struct bssgp_ran_inf_app_cont_nacc *cont) +{ + uint8_t *buf_ptr = buf; + int rc; + unsigned int silen; + unsigned int i; + struct gprs_ra_id *raid; + + if (cont->type_psi) + silen = BSSGP_RIM_PSI_LEN; + else + silen = BSSGP_RIM_SI_LEN; + + /* The buffer must accept the reporting cell id, plus 1 byte to define + * the type and number of sysinfo messages. */ + if (len < REP_CELL_ID_LEN + 1 + silen * cont->num_si) + return -EINVAL; + + raid = (struct gprs_ra_id *)&cont->reprt_cell.rai; + rc = bssgp_create_cell_id(buf_ptr, raid, cont->reprt_cell.cell_identity); + if (rc < 0) + return -EINVAL; + buf_ptr += rc; + + buf_ptr[0] = 0x00; + if (cont->type_psi) + buf_ptr[0] |= 0x01; + buf_ptr[0] |= (cont->num_si << 1); + buf_ptr++; + + for (i = 0; i < cont->num_si; i++) { + memcpy(buf_ptr, cont->si[i], silen); + buf_ptr += silen; + } + + return (int)(buf_ptr - buf); +} + +/* 3GPP TS 48.018, table 11.3.64.1.b, NACC Cause coding */ +const struct value_string bssgp_nacc_cause_strs[] = { + { BSSGP_NACC_CAUSE_UNSPEC, "unspecified error" }, + { BSSGP_NACC_CAUSE_SYNTAX_ERR, "syntax error in app container" }, + { BSSGP_NACC_CAUSE_RPRT_CELL_MISSMTCH, "reporting cell id mismatch" }, + { BSSGP_NACC_CAUSE_SIPSI_TYPE_ERR, "SI/PSI type error" }, + { BSSGP_NACC_CAUSE_SIPSI_LEN_ERR, "SI/PSI inconsistent length" }, + { BSSGP_NACC_CAUSE_SIPSI_SET_ERR, "inconsistent set of msg" }, + { 0, NULL } +}; + +/*! Decode a Application Error Container for NACC (3GPP TS 48.018, section 11.3.64.1). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_app_err_cont_nacc(struct bssgp_app_err_cont_nacc *cont, const uint8_t *buf, size_t len) +{ + /* The buffer must at least contain the NACC cause code, it should also + * contain the application container, but we won't error if it is missing. */ + if (len < 1) + return -EINVAL; + + cont->nacc_cause = buf[0]; + + if (len > 1) { + cont->err_app_cont = buf + 1; + cont->err_app_cont_len = len - 1; + } else { + cont->err_app_cont = NULL; + cont->err_app_cont_len = 0; + } + + return 0; +} + +/*! Encode Application Error Container for NACC (3GPP TS 48.018, section 11.3.64.1). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_app_err_cont_nacc(uint8_t *buf, size_t len, const struct bssgp_app_err_cont_nacc *cont) +{ + uint8_t *buf_ptr = buf; + + /* The buffer must accept the length of the application container and the NACC + * cause code, which is one octet in length. */ + if (len < cont->err_app_cont_len + 1) + return -EINVAL; + + buf_ptr[0] = cont->nacc_cause; + buf_ptr++; + + memcpy(buf_ptr, cont->err_app_cont, cont->err_app_cont_len); + buf_ptr += cont->err_app_cont_len; + + return (int)(buf_ptr - buf); +} + +/* The structs bssgp_ran_inf_req_rim_cont, bssgp_ran_inf_rim_cont and bssgp_ran_inf_app_err_rim_cont *cont + * share four common fields at the beginning, we use the following struct as parameter type for the common + * encoder/decoder functions. (See also 3GPP TS 48.018 table 11.3.62a.1.b, table 11.3.62a.2.b, and + * table 11.3.62a.5.b) */ +struct bssgp_ran_inf_x_cont { + enum bssgp_ran_inf_app_id app_id; + uint32_t seq_num; + struct bssgp_rim_pdu_ind pdu_ind; + uint8_t prot_ver; +}; + +static int dec_rim_cont_common(struct bssgp_ran_inf_x_cont *cont, struct tlv_parsed *tp) +{ + if (TLVP_PRES_LEN(tp, BSSGP_IE_RIM_APP_IDENTITY, sizeof(uint8_t))) + cont->app_id = TLVP_VAL(tp, BSSGP_IE_RIM_APP_IDENTITY)[0]; + else + return -EINVAL; + + if (TLVP_PRES_LEN(tp, BSSGP_IE_RIM_SEQ_NR, sizeof(cont->seq_num))) + cont->seq_num = tlvp_val32be(tp, BSSGP_IE_RIM_SEQ_NR); + else + return -EINVAL; + + if (TLVP_PRES_LEN(tp, BSSGP_IE_RIM_PDU_INDICATIONS, sizeof(cont->pdu_ind))) + memcpy(&cont->pdu_ind, TLVP_VAL(tp, BSSGP_IE_RIM_PDU_INDICATIONS), sizeof(cont->pdu_ind)); + else + return -EINVAL; + + if (TLVP_PRES_LEN(tp, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver))) + cont->prot_ver = TLVP_VAL(tp, BSSGP_IE_RIM_PROTOCOL_VERSION)[0]; + else + cont->prot_ver = 1; + + return 0; +} + +static uint8_t *enc_rim_cont_common(uint8_t *buf, size_t len, const struct bssgp_ran_inf_x_cont *cont) +{ + + uint32_t seq_num = osmo_htonl(cont->seq_num); + uint8_t app_id_temp; + uint8_t *buf_ptr = buf; + + if (len < + TVLV_HDR_MAXLEN * 4 + sizeof(app_id_temp) + sizeof(seq_num) + sizeof(cont->pdu_ind) + + sizeof(cont->prot_ver)) + return NULL; + + app_id_temp = cont->app_id; + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_APP_IDENTITY, sizeof(app_id_temp), &app_id_temp); + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_SEQ_NR, sizeof(seq_num), (uint8_t *) & seq_num); + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_PDU_INDICATIONS, sizeof(cont->pdu_ind), (uint8_t *) & cont->pdu_ind); + if (cont->prot_ver > 0) + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver), &cont->prot_ver); + + return buf_ptr; +} + +/* 3GPP TS 48.018, table 11.3.61.b: RIM Application Identity coding */ +const struct value_string bssgp_ran_inf_app_id_strs[] = { + { BSSGP_RAN_INF_APP_ID_NACC, "Network Assisted Cell Change (NACC)" }, + { BSSGP_RAN_INF_APP_ID_SI3, "System Information 3 (SI3)" }, + { BSSGP_RAN_INF_APP_ID_MBMS, "MBMS data channel" }, + { BSSGP_RAN_INF_APP_ID_SON, "SON Transfer" }, + { BSSGP_RAN_INF_APP_ID_UTRA_SI, "UTRA System Information (UTRA SI)" }, + { 0, NULL } +}; + +/*! Decode a RAN Information Request RIM Container (3GPP TS 48.018, table 11.3.62a.1.b). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_req_rim_cont(struct bssgp_ran_inf_req_rim_cont *cont, const uint8_t *buf, size_t len) +{ + int rc; + struct tlv_parsed tp; + + memset(cont, 0, sizeof(*cont)); + + rc = tlv_parse(&tp, &tvlv_att_def, buf, len, 0, 0); + if (rc < 0) + return -EINVAL; + + rc = dec_rim_cont_common((struct bssgp_ran_inf_x_cont *)cont, &tp); + if (rc < 0) + return -EINVAL; + + if (TLVP_PRESENT(&tp, BSSGP_IE_RIM_REQ_APP_CONTAINER)) { + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + rc = bssgp_dec_ran_inf_req_app_cont_nacc(&cont->u.app_cont_nacc, + TLVP_VAL(&tp, BSSGP_IE_RIM_REQ_APP_CONTAINER), + TLVP_LEN(&tp, BSSGP_IE_RIM_REQ_APP_CONTAINER)); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add parsers for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + + if (rc < 0) + return rc; + } + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID, 1)) { + cont->son_trans_app_id = TLVP_VAL(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + cont->son_trans_app_id_len = TLVP_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + } + + return 0; +} + +/* Dub a TLVP header into a given buffer. The value part of the IE must start + * at the 2nd octet. Should the length field make a 3 octet TLVP header + * necessary (unlikely, but possible) the value part is moved ahead by one + * octet. The function returns a pointer to the end of value part. */ +static uint8_t *dub_tlvp_header(uint8_t *buf, uint8_t iei, uint16_t len) +{ + uint8_t *buf_ptr = buf; + + buf_ptr[0] = iei; + if (len <= TVLV_MAX_ONEBYTE) { + buf_ptr[1] = (uint8_t) len; + buf_ptr[1] |= 0x80; + buf_ptr += TVLV_HDR_LEN; + } else { + memmove(buf_ptr + 1, buf_ptr, len); + buf_ptr[1] = len >> 8; + buf_ptr[2] = len & 0xff; + buf_ptr += TVLV_HDR_MAXLEN; + } + buf_ptr += len; + + return buf_ptr; +} + +/*! Encode a RAN Information Request RIM Container (3GPP TS 48.018, table 11.3.62a.1.b). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_req_rim_cont(uint8_t *buf, size_t len, const struct bssgp_ran_inf_req_rim_cont *cont) +{ + uint8_t *buf_ptr = buf; + int app_cont_len = 0; + int remaining_buf_len; + + buf_ptr = enc_rim_cont_common(buf_ptr, len, (struct bssgp_ran_inf_x_cont *)cont); + if (!buf_ptr) + return -EINVAL; + + remaining_buf_len = len - (int)(buf_ptr - buf); + if (remaining_buf_len <= 0) + return -EINVAL; + + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + app_cont_len = + bssgp_enc_ran_inf_req_app_cont_nacc(buf_ptr + TVLV_HDR_LEN, remaining_buf_len - TVLV_HDR_MAXLEN, + &cont->u.app_cont_nacc); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add encoders for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + + if (app_cont_len < 0) + return -EINVAL; + buf_ptr = dub_tlvp_header(buf_ptr, BSSGP_IE_RIM_REQ_APP_CONTAINER, app_cont_len); + + remaining_buf_len = len - (int)(buf_ptr - buf); + if (remaining_buf_len < 0) + return -EINVAL; + + if (cont->son_trans_app_id && cont->son_trans_app_id_len > 0) { + if (remaining_buf_len < cont->son_trans_app_id_len + TVLV_HDR_MAXLEN) + return -EINVAL; + buf_ptr = + tvlv_put(buf_ptr, BSSGP_IE_SON_TRANSFER_APP_ID, cont->son_trans_app_id_len, cont->son_trans_app_id); + } + return (int)(buf_ptr - buf); +} + +/*! Decode a RAN Information RIM Container (3GPP TS 48.018, table 11.3.62a.2.b). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_rim_cont(struct bssgp_ran_inf_rim_cont *cont, const uint8_t *buf, size_t len) +{ + int rc; + struct tlv_parsed tp; + + memset(cont, 0, sizeof(*cont)); + + rc = tlv_parse(&tp, &tvlv_att_def, buf, len, 0, 0); + if (rc < 0) + return -EINVAL; + + rc = dec_rim_cont_common((struct bssgp_ran_inf_x_cont *)cont, &tp); + if (rc < 0) + return -EINVAL; + + if (TLVP_PRESENT(&tp, BSSGP_IE_RAN_INFO_APP_CONTAINER)) { + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + rc = bssgp_dec_ran_inf_app_cont_nacc(&cont->u.app_cont_nacc, + TLVP_VAL(&tp, BSSGP_IE_RAN_INFO_APP_CONTAINER), + TLVP_LEN(&tp, BSSGP_IE_RAN_INFO_APP_CONTAINER)); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add parsers for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + + if (rc < 0) + return rc; + } else if (TLVP_PRESENT(&tp, BSSGP_IE_APP_ERROR_CONTAINER)) { + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + rc = bssgp_dec_app_err_cont_nacc(&cont->u.app_err_cont_nacc, + TLVP_VAL(&tp, BSSGP_IE_APP_ERROR_CONTAINER), TLVP_LEN(&tp, + BSSGP_IE_APP_ERROR_CONTAINER)); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add parsers for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + if (rc < 0) + return rc; + cont->app_err = true; + } + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID, 1)) { + cont->son_trans_app_id = TLVP_VAL(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + cont->son_trans_app_id_len = TLVP_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + } + + return 0; +} + +/*! Encode a RAN Information RIM Container (3GPP TS 48.018, table 11.3.62a.2.b). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_rim_cont(uint8_t *buf, size_t len, const struct bssgp_ran_inf_rim_cont *cont) +{ + uint8_t *buf_ptr = buf; + int app_cont_len = 0; + int remaining_buf_len; + + buf_ptr = enc_rim_cont_common(buf_ptr, len, (struct bssgp_ran_inf_x_cont *)cont); + if (!buf_ptr) + return -EINVAL; + + remaining_buf_len = len - (int)(buf_ptr - buf); + if (remaining_buf_len <= 0) + return -EINVAL; + + if (cont->app_err) { + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + app_cont_len = + bssgp_enc_app_err_cont_nacc(buf_ptr + TVLV_HDR_LEN, remaining_buf_len - TVLV_HDR_MAXLEN, + &cont->u.app_err_cont_nacc); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add encoders for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + if (app_cont_len < 0) + return -EINVAL; + buf_ptr = dub_tlvp_header(buf_ptr, BSSGP_IE_APP_ERROR_CONTAINER, app_cont_len); + } else { + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + app_cont_len = + bssgp_enc_ran_inf_app_cont_nacc(buf_ptr + TVLV_HDR_LEN, remaining_buf_len - TVLV_HDR_MAXLEN, + &cont->u.app_cont_nacc); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add encoders for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + if (app_cont_len < 0) + return -EINVAL; + buf_ptr = dub_tlvp_header(buf_ptr, BSSGP_IE_RAN_INFO_APP_CONTAINER, app_cont_len); + } + + remaining_buf_len = len - (int)(buf_ptr - buf); + if (remaining_buf_len < 0) + return -EINVAL; + + if (cont->son_trans_app_id && cont->son_trans_app_id_len > 0) { + if (remaining_buf_len < cont->son_trans_app_id_len + TVLV_HDR_MAXLEN) + return -EINVAL; + buf_ptr = + tvlv_put(buf_ptr, BSSGP_IE_SON_TRANSFER_APP_ID, cont->son_trans_app_id_len, cont->son_trans_app_id); + } + return (int)(buf_ptr - buf); +} + +/*! Decode a RAN Information ACK RIM Container (3GPP TS 48.018, table 11.3.62a.3.b). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_ack_rim_cont(struct bssgp_ran_inf_ack_rim_cont *cont, const uint8_t *buf, size_t len) +{ + int rc; + struct tlv_parsed tp; + + memset(cont, 0, sizeof(*cont)); + + rc = tlv_parse(&tp, &tvlv_att_def, buf, len, 0, 0); + if (rc < 0) + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_RIM_APP_IDENTITY, sizeof(uint8_t))) + cont->app_id = TLVP_VAL(&tp, BSSGP_IE_RIM_APP_IDENTITY)[0]; + else + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_RIM_SEQ_NR, sizeof(cont->seq_num))) + cont->seq_num = tlvp_val32be(&tp, BSSGP_IE_RIM_SEQ_NR); + else + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver))) + cont->prot_ver = TLVP_VAL(&tp, BSSGP_IE_RIM_PROTOCOL_VERSION)[0]; + else + cont->prot_ver = 1; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID, 1)) { + cont->son_trans_app_id = TLVP_VAL(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + cont->son_trans_app_id_len = TLVP_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + } + + return 0; +} + +/*! Encode a RAN Information ACK RIM Container (3GPP TS 48.018, table 11.3.62a.3.b). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_ack_rim_cont(uint8_t *buf, size_t len, const struct bssgp_ran_inf_ack_rim_cont *cont) +{ + uint8_t *buf_ptr = buf; + uint32_t seq_num = osmo_htonl(cont->seq_num); + uint8_t app_id_temp; + + if (len < + 4 * TVLV_HDR_MAXLEN + sizeof(app_id_temp) + sizeof(seq_num) + sizeof(cont->prot_ver) + + cont->son_trans_app_id_len) + return -EINVAL; + + app_id_temp = cont->app_id; + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_APP_IDENTITY, sizeof(app_id_temp), &app_id_temp); + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_SEQ_NR, sizeof(seq_num), (uint8_t *) & seq_num); + + if (cont->prot_ver > 0) + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver), &cont->prot_ver); + + if (cont->son_trans_app_id && cont->son_trans_app_id_len > 0) + buf_ptr = + tvlv_put(buf_ptr, BSSGP_IE_SON_TRANSFER_APP_ID, cont->son_trans_app_id_len, cont->son_trans_app_id); + + return (int)(buf_ptr - buf); +} + +/*! Decode a RAN Information Error RIM Container (3GPP TS 48.018, table 11.3.62a.4.b). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_err_rim_cont(struct bssgp_ran_inf_err_rim_cont *cont, const uint8_t *buf, size_t len) +{ + int rc; + struct tlv_parsed tp; + + memset(cont, 0, sizeof(*cont)); + + rc = tlv_parse(&tp, &tvlv_att_def, buf, len, 0, 0); + if (rc < 0) + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_RIM_APP_IDENTITY, sizeof(uint8_t))) + cont->app_id = TLVP_VAL(&tp, BSSGP_IE_RIM_APP_IDENTITY)[0]; + else + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_CAUSE, sizeof(cont->cause))) + cont->cause = TLVP_VAL(&tp, BSSGP_IE_CAUSE)[0]; + else + return -EINVAL; + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver))) + cont->prot_ver = TLVP_VAL(&tp, BSSGP_IE_RIM_PROTOCOL_VERSION)[0]; + else + cont->prot_ver = 1; + + if (TLVP_PRESENT(&tp, BSSGP_IE_PDU_IN_ERROR)) { + cont->err_pdu = TLVP_VAL(&tp, BSSGP_IE_PDU_IN_ERROR); + cont->err_pdu_len = TLVP_LEN(&tp, BSSGP_IE_PDU_IN_ERROR); + } else { + return -EINVAL; + } + + if (TLVP_PRES_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID, 1)) { + cont->son_trans_app_id = TLVP_VAL(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + cont->son_trans_app_id_len = TLVP_LEN(&tp, BSSGP_IE_SON_TRANSFER_APP_ID); + } + + return 0; +} + +/*! Encode a RAN Information Error RIM Container (3GPP TS 48.018, table 11.3.62a.4.b). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_err_rim_cont(uint8_t *buf, size_t len, const struct bssgp_ran_inf_err_rim_cont *cont) +{ + uint8_t *buf_ptr = buf; + uint8_t app_id_temp; + + if (len < + TVLV_HDR_MAXLEN * 5 + sizeof(app_id_temp) + sizeof(cont->cause) + sizeof(cont->prot_ver) + + cont->err_pdu_len + cont->son_trans_app_id_len) + return -EINVAL; + + app_id_temp = cont->app_id; + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_APP_IDENTITY, sizeof(app_id_temp), &app_id_temp); + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_CAUSE, sizeof(cont->cause), &cont->cause); + + if (cont->prot_ver > 0) + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_RIM_PROTOCOL_VERSION, sizeof(cont->prot_ver), &cont->prot_ver); + + if (cont->err_pdu && cont->err_pdu_len > 0) + buf_ptr = tvlv_put(buf_ptr, BSSGP_IE_PDU_IN_ERROR, cont->err_pdu_len, cont->err_pdu); + else + return -EINVAL; + + if (cont->son_trans_app_id && cont->son_trans_app_id_len > 0) + buf_ptr = + tvlv_put(buf_ptr, BSSGP_IE_SON_TRANSFER_APP_ID, cont->son_trans_app_id_len, cont->son_trans_app_id); + + return (int)(buf_ptr - buf); +} + +/*! Decode a RAN Information Application Error RIM Container (3GPP TS 48.018, table 11.3.62a.5.b). + * \param[out] user provided memory for decoded data struct. + * \param[in] buf user provided memory with the encoded value data of the IE. + * \returns 0 on success, -EINVAL on error. */ +int bssgp_dec_ran_inf_app_err_rim_cont(struct bssgp_ran_inf_app_err_rim_cont *cont, const uint8_t *buf, size_t len) +{ + int rc; + struct tlv_parsed tp; + + memset(cont, 0, sizeof(*cont)); + + rc = tlv_parse(&tp, &tvlv_att_def, buf, len, 0, 0); + if (rc < 0) + return -EINVAL; + + rc = dec_rim_cont_common((struct bssgp_ran_inf_x_cont *)cont, &tp); + if (rc < 0) + return -EINVAL; + + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + rc = bssgp_dec_app_err_cont_nacc(&cont->u.app_err_cont_nacc, + TLVP_VAL(&tp, BSSGP_IE_APP_ERROR_CONTAINER), TLVP_LEN(&tp, + BSSGP_IE_APP_ERROR_CONTAINER)); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add parsers for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + if (rc < 0) + return rc; + + return 0; +} + +/*! Encode a RAN Information Application Error RIM Container (3GPP TS 48.018, table 11.3.62a.5.b). + * \param[out] buf user provided memory for the generated value part of the IE. + * \param[in] cont user provided input data struct. + * \returns length of encoded octets, -EINVAL on error. */ +int bssgp_enc_ran_inf_app_err_rim_cont(uint8_t *buf, size_t len, const struct bssgp_ran_inf_app_err_rim_cont *cont) +{ + uint8_t *buf_ptr = buf; + int app_cont_len = 0; + int remaining_buf_len; + + buf_ptr = enc_rim_cont_common(buf_ptr, len, (struct bssgp_ran_inf_x_cont *)cont); + if (!buf_ptr) + return -EINVAL; + + remaining_buf_len = len - (int)(buf_ptr - buf); + if (remaining_buf_len <= 0) + return -EINVAL; + + switch (cont->app_id) { + case BSSGP_RAN_INF_APP_ID_NACC: + app_cont_len = + bssgp_enc_app_err_cont_nacc(buf_ptr + TVLV_HDR_LEN, remaining_buf_len - TVLV_HDR_MAXLEN, + &cont->u.app_err_cont_nacc); + break; + case BSSGP_RAN_INF_APP_ID_SI3: + case BSSGP_RAN_INF_APP_ID_MBMS: + case BSSGP_RAN_INF_APP_ID_SON: + case BSSGP_RAN_INF_APP_ID_UTRA_SI: + /* TODO: add encoders for Si3, MBMS, SON, UTRA-SI app containers */ + return -EOPNOTSUPP; + default: + return -EINVAL; + } + if (app_cont_len < 0) + return -EINVAL; + buf_ptr = dub_tlvp_header(buf_ptr, BSSGP_IE_APP_ERROR_CONTAINER, app_cont_len); + + return (int)(buf_ptr - buf); +} + +/*! Parse a given message buffer into a rim-pdu struct. + * \param[out] pdu user provided memory for the resulting RAN INFORMATION PDU. + * \param[in] msg BSSGP message buffer that contains the encoded RAN INFORMATION PDU. + * \returns 0 on sccess, -EINVAL on error. */ +int bssgp_parse_rim_pdu(struct bssgp_ran_information_pdu *pdu, const struct msgb *msg) +{ + struct tlv_parsed tp[2]; + struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *)msgb_bssgph(msg); + int data_len; + int rc; + uint16_t nsei = msgb_nsei(msg); + + memset(pdu, 0, sizeof(*pdu)); + + data_len = msgb_bssgp_len(msg) - sizeof(*bgph); + if (data_len < 0) + return -EINVAL; + + rc = osmo_tlv_prot_parse(&osmo_pdef_bssgp, tp, ARRAY_SIZE(tp), bgph->pdu_type, bgph->data, data_len, 0, 0, + DLBSSGP, __func__); + if (rc < 0) + return -EINVAL; + + if (TLVP_PRESENT(&tp[0], BSSGP_IE_RIM_ROUTING_INFO)) { + rc = bssgp_parse_rim_ri(&pdu->routing_info_dest, TLVP_VAL(&tp[0], BSSGP_IE_RIM_ROUTING_INFO), + TLVP_LEN(&tp[0], BSSGP_IE_RIM_ROUTING_INFO)); + if (rc < 0) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP RIM (NSEI=%u) invalid Destination Cell Identifier IE\n", nsei); + return -EINVAL; + } + } else { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP RIM (NSEI=%u) missing Destination Cell Identifier IE\n", nsei); + return -EINVAL; + } + + if (TLVP_PRESENT(&tp[1], BSSGP_IE_RIM_ROUTING_INFO)) { + rc = bssgp_parse_rim_ri(&pdu->routing_info_src, TLVP_VAL(&tp[1], BSSGP_IE_RIM_ROUTING_INFO), + TLVP_LEN(&tp[1], BSSGP_IE_RIM_ROUTING_INFO)); + if (rc < 0) { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP RIM (NSEI=%u) invalid Destination Cell Identifier IE\n", nsei); + return -EINVAL; + } + } else { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP RIM (NSEI=%u) missing Source Cell Identifier IE\n", nsei); + return -EINVAL; + } + + if (TLVP_PRESENT(&tp[0], BSSGP_IE_RI_REQ_RIM_CONTAINER)) + pdu->rim_cont_iei = BSSGP_IE_RI_REQ_RIM_CONTAINER; + else if (TLVP_PRESENT(&tp[0], BSSGP_IE_RI_RIM_CONTAINER)) + pdu->rim_cont_iei = BSSGP_IE_RI_RIM_CONTAINER; + else if (TLVP_PRESENT(&tp[0], BSSGP_IE_RI_APP_ERROR_RIM_CONT)) + pdu->rim_cont_iei = BSSGP_IE_RI_APP_ERROR_RIM_CONT; + else if (TLVP_PRESENT(&tp[0], BSSGP_IE_RI_ACK_RIM_CONTAINER)) + pdu->rim_cont_iei = BSSGP_IE_RI_ACK_RIM_CONTAINER; + else if (TLVP_PRESENT(&tp[0], BSSGP_IE_RI_ERROR_RIM_COINTAINER)) + pdu->rim_cont_iei = BSSGP_IE_RI_ERROR_RIM_COINTAINER; + else { + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP RIM (NSEI=%u) missing or wrong RIM Container IE\n", nsei); + return -EINVAL; + } + + pdu->rim_cont = TLVP_VAL(&tp[0], pdu->rim_cont_iei); + pdu->rim_cont_len = TLVP_LEN(&tp[0], pdu->rim_cont_iei); + + /* Make sure the rim container field is not empty */ + if (pdu->rim_cont_len < 1) + return -EINVAL; + if (!pdu->rim_cont) + return -EINVAL; + + /* Note: It is not an error if we fail to parse the RIM container, + * since there are applications where parsing the RIM container + * is not necessary (routing). It is up to the API user to check + * the results. */ + switch (pdu->rim_cont_iei) { + case BSSGP_IE_RI_REQ_RIM_CONTAINER: + rc = bssgp_dec_ran_inf_req_rim_cont(&pdu->decoded.req_rim_cont, pdu->rim_cont, pdu->rim_cont_len); + break; + case BSSGP_IE_RI_RIM_CONTAINER: + rc = bssgp_dec_ran_inf_rim_cont(&pdu->decoded.rim_cont, pdu->rim_cont, pdu->rim_cont_len); + break; + case BSSGP_IE_RI_APP_ERROR_RIM_CONT: + rc = bssgp_dec_ran_inf_app_err_rim_cont(&pdu->decoded.app_err_rim_cont, pdu->rim_cont, + pdu->rim_cont_len); + break; + case BSSGP_IE_RI_ACK_RIM_CONTAINER: + rc = bssgp_dec_ran_inf_ack_rim_cont(&pdu->decoded.ack_rim_cont, pdu->rim_cont, pdu->rim_cont_len); + break; + case BSSGP_IE_RI_ERROR_RIM_COINTAINER: + rc = bssgp_dec_ran_inf_err_rim_cont(&pdu->decoded.err_rim_cont, pdu->rim_cont, pdu->rim_cont_len); + break; + default: + LOGP(DLBSSGP, LOGL_DEBUG, "BSSGP RIM (NSEI=%u) cannot parse unknown RIM container.\n", nsei); + return 0; + } + if (rc < 0) { + LOGP(DLBSSGP, LOGL_DEBUG, "BSSGP RIM (NSEI=%u) unable to parse RIM container.\n", nsei); + return 0; + } + pdu->decoded_present = true; + + return 0; +} + +/*! Encode a given rim-pdu struct into a message buffer. + * \param[out] pdu user provided memory that contains the RAN INFORMATION PDU to encode. + * \returns BSSGP message buffer on sccess, NULL on error. */ +struct msgb *bssgp_encode_rim_pdu(const struct bssgp_ran_information_pdu *pdu) +{ + struct msgb *msg = bssgp_msgb_alloc(); + struct bssgp_normal_hdr *bgph; + uint8_t rim_ri_buf[BSSGP_RIM_ROUTING_INFO_MAXLEN]; + int rc; + + if (!msg) + return NULL; + bgph = (struct bssgp_normal_hdr *)msgb_put(msg, sizeof(*bgph)); + + /* Set PDU type based on RIM container type */ + switch (pdu->rim_cont_iei) { + case BSSGP_IE_RI_REQ_RIM_CONTAINER: + bgph->pdu_type = BSSGP_PDUT_RAN_INFO_REQ; + break; + case BSSGP_IE_RI_RIM_CONTAINER: + bgph->pdu_type = BSSGP_PDUT_RAN_INFO; + break; + case BSSGP_IE_RI_APP_ERROR_RIM_CONT: + bgph->pdu_type = BSSGP_PDUT_RAN_INFO_APP_ERROR; + break; + case BSSGP_IE_RI_ACK_RIM_CONTAINER: + bgph->pdu_type = BSSGP_PDUT_RAN_INFO_ACK; + break; + case BSSGP_IE_RI_ERROR_RIM_COINTAINER: + bgph->pdu_type = BSSGP_PDUT_RAN_INFO_ERROR; + break; + default: + /* The caller must correctly specify the container type! */ + OSMO_ASSERT(false); + } + + /* Put RIM routing information */ + rc = bssgp_create_rim_ri(rim_ri_buf, &pdu->routing_info_dest); + if (rc < 0 || rc > BSSGP_RIM_ROUTING_INFO_MAXLEN) + goto error; + msgb_tvlv_put(msg, BSSGP_IE_RIM_ROUTING_INFO, rc, rim_ri_buf); + rc = bssgp_create_rim_ri(rim_ri_buf, &pdu->routing_info_src); + if (rc < 0 || rc > BSSGP_RIM_ROUTING_INFO_MAXLEN) + goto error; + msgb_tvlv_put(msg, BSSGP_IE_RIM_ROUTING_INFO, rc, rim_ri_buf); + + /* Put RIM container */ + if (pdu->decoded_present) { + uint8_t *rim_cont_buf = talloc_zero_size(msg, msg->data_len); + if (!rim_cont_buf) + goto error; + + switch (pdu->rim_cont_iei) { + case BSSGP_IE_RI_REQ_RIM_CONTAINER: + rc = bssgp_enc_ran_inf_req_rim_cont(rim_cont_buf, msg->data_len, &pdu->decoded.req_rim_cont); + break; + case BSSGP_IE_RI_RIM_CONTAINER: + rc = bssgp_enc_ran_inf_rim_cont(rim_cont_buf, msg->data_len, &pdu->decoded.rim_cont); + break; + case BSSGP_IE_RI_APP_ERROR_RIM_CONT: + rc = bssgp_enc_ran_inf_app_err_rim_cont(rim_cont_buf, msg->data_len, + &pdu->decoded.app_err_rim_cont); + break; + case BSSGP_IE_RI_ACK_RIM_CONTAINER: + rc = bssgp_enc_ran_inf_ack_rim_cont(rim_cont_buf, msg->data_len, &pdu->decoded.ack_rim_cont); + break; + case BSSGP_IE_RI_ERROR_RIM_COINTAINER: + rc = bssgp_enc_ran_inf_err_rim_cont(rim_cont_buf, msg->data_len, &pdu->decoded.err_rim_cont); + break; + default: + /* The API user must set the iei properly! */ + OSMO_ASSERT(false); + } + if (rc < 0) { + talloc_free(rim_cont_buf); + goto error; + } + + msgb_tvlv_put(msg, pdu->rim_cont_iei, rc, rim_cont_buf); + talloc_free(rim_cont_buf); + } else { + /* Make sure the RIM container is actually present. */ + OSMO_ASSERT(pdu->rim_cont_iei != 0 && pdu->rim_cont_len > 0 && pdu->rim_cont); + msgb_tvlv_put(msg, pdu->rim_cont_iei, pdu->rim_cont_len, pdu->rim_cont); + } + + return msg; +error: + msgb_free(msg); + return 0; +} + +/*! Send RIM RAN INFORMATION REQUEST via BSSGP (3GPP TS 48.018, section 10.6.1). + * \param[in] pdu user provided memory for the RAN INFORMATION PDU to be sent. + * \param[in] nsei BSSGP network service entity identifier (NSEI). + * \returns 0 on sccess, -EINVAL on error. */ +int bssgp_tx_rim(const struct bssgp_ran_information_pdu *pdu, uint16_t nsei) +{ + struct msgb *msg; + struct bssgp_normal_hdr *bgph; + char ri_src_str[64]; + char ri_dest_str[64]; + + /* Encode RIM PDU into mesage buffer */ + msg = bssgp_encode_rim_pdu(pdu); + if (!msg) { + LOGP(DLBSSGP, LOGL_ERROR, + "BSSGP RIM (NSEI=%u) unable to encode BSSGP RIM PDU\n", nsei); + return -EINVAL; + } + + msgb_nsei(msg) = nsei; + msgb_bvci(msg) = 0; /* Signalling */ + + bgph = (struct bssgp_normal_hdr *)msgb_bssgph(msg); + DEBUGP(DLBSSGP, "BSSGP BVCI=0 NSEI=%u Tx RIM-PDU:%s, src=%s, dest=%s\n", + nsei, bssgp_pdu_str(bgph->pdu_type), + bssgp_rim_ri_name_buf(ri_src_str, sizeof(ri_src_str), &pdu->routing_info_src), + bssgp_rim_ri_name_buf(ri_dest_str, sizeof(ri_dest_str), &pdu->routing_info_dest)); + + return bssgp_ns_send(bssgp_ns_send_data, msg); +} + +/*! Send encoded RAN TRANSPARENT CONTAINER via BSSGP (3GPP TS 29.060, section 7.7.43). + * \param[in] msg user provided memory for the encoded RAN TRANSPARENT CONTAINER to be sent. + * (this function will take ownership of msg). + * \param[in] nsei BSSGP network service entity identifier (NSEI). + * \returns 0 on sccess, -EINVAL on error. */ +int bssgp_tx_rim_encoded(struct msgb *msg, uint16_t nsei) +{ + struct bssgp_normal_hdr *bgph; + + msgb_nsei(msg) = nsei; + msgb_bvci(msg) = 0; /* Signalling */ + + bgph = (struct bssgp_normal_hdr *)msgb_bssgph(msg); + DEBUGP(DLBSSGP, "BSSGP BVCI=0 NSEI=%u Tx RIM-PDU:%s\n", + nsei, bssgp_pdu_str(bgph->pdu_type)); + + return bssgp_ns_send(bssgp_ns_send_data, msg); +} + +/* For internal use only (called from gprs_bssgp.c) */ +int bssgp_rx_rim(struct msgb *msg, struct tlv_parsed *tp, uint16_t bvci) +{ + struct osmo_bssgp_prim nmp; + uint16_t nsei = msgb_nsei(msg); + struct bssgp_normal_hdr *bgph = (struct bssgp_normal_hdr *)msgb_bssgph(msg); + enum bssgp_prim prim; + char ri_src_str[64]; + char ri_dest_str[64]; + + /* Specify PRIM type based on the RIM PDU */ + switch (bgph->pdu_type) { + case BSSGP_PDUT_RAN_INFO: + case BSSGP_PDUT_RAN_INFO_REQ: + case BSSGP_PDUT_RAN_INFO_ACK: + case BSSGP_PDUT_RAN_INFO_ERROR: + case BSSGP_PDUT_RAN_INFO_APP_ERROR: + prim = PRIM_BSSGP_RIM_PDU_TRANSFER; + break; + default: + /* Caller already makes sure that this can't happen. */ + OSMO_ASSERT(false); + } + + /* Send BSSGP RIM indication to NM */ + memset(&nmp, 0, sizeof(nmp)); + nmp.nsei = nsei; + nmp.bvci = bvci; + nmp.tp = tp; + if (bssgp_parse_rim_pdu(&nmp.u.rim_pdu, msg) < 0) + return bssgp_tx_status(BSSGP_CAUSE_MISSING_MAND_IE, NULL, msg); + DEBUGP(DLBSSGP, "BSSGP BVCI=%u Rx RIM-PDU:%s, src=%s, dest=%s\n", + bvci, bssgp_pdu_str(bgph->pdu_type), + bssgp_rim_ri_name_buf(ri_src_str, sizeof(ri_src_str), &nmp.u.rim_pdu.routing_info_src), + bssgp_rim_ri_name_buf(ri_dest_str, sizeof(ri_dest_str), &nmp.u.rim_pdu.routing_info_dest)); + osmo_prim_init(&nmp.oph, SAP_BSSGP_RIM, prim, PRIM_OP_INDICATION, msg); + bssgp_prim_cb(&nmp.oph, NULL); + + return 0; +} diff --git a/src/gb/gprs_bssgp_util.c b/src/gb/gprs_bssgp_util.c index 669dfb86..92896c1f 100644 --- a/src/gb/gprs_bssgp_util.c +++ b/src/gb/gprs_bssgp_util.c @@ -32,7 +32,7 @@ #include <osmocom/gprs/gprs_bssgp.h> #include <osmocom/gprs/gprs_ns.h> -#include "common_vty.h" +#include "gprs_bssgp_internal.h" struct gprs_ns_inst *bssgp_nsi; @@ -43,7 +43,7 @@ struct gprs_ns_inst *bssgp_nsi; static const struct value_string bssgp_cause_strings[] = { { BSSGP_CAUSE_PROC_OVERLOAD, "Processor overload" }, { BSSGP_CAUSE_EQUIP_FAIL, "Equipment Failure" }, - { BSSGP_CAUSE_TRASIT_NET_FAIL, "Transit netowkr service failure" }, + { BSSGP_CAUSE_TRASIT_NET_FAIL, "Transit network service failure" }, { BSSGP_CAUSE_CAPA_GREATER_0KPBS, "Transmission capacity modified" }, { BSSGP_CAUSE_UNKNOWN_MS, "Unknown MS" }, { BSSGP_CAUSE_UNKNOWN_BVCI, "Unknown BVCI" }, @@ -106,6 +106,8 @@ static const struct value_string bssgp_pdu_strings[] = { { BSSGP_PDUT_UL_UNITDATA, "UL-UNITDATA" }, { BSSGP_PDUT_RA_CAPABILITY, "RA-CAPABILITY" }, { BSSGP_PDUT_PTM_UNITDATA, "PTM-UNITDATA" }, + { BSSGP_PDUT_DL_MMBS_UNITDATA, "DL-MBMS-UNITDATA" }, + { BSSGP_PDUT_UL_MMBS_UNITDATA, "UL-MBMS-UNITDATA" }, { BSSGP_PDUT_PAGING_PS, "PAGING-PS" }, { BSSGP_PDUT_PAGING_CS, "PAGING-CS" }, { BSSGP_PDUT_RA_CAPA_UDPATE, "RA-CAPABILITY-UPDATE" }, @@ -117,6 +119,10 @@ static const struct value_string bssgp_pdu_strings[] = { { BSSGP_PDUT_RESUME, "RESUME" }, { BSSGP_PDUT_RESUME_ACK, "RESUME-ACK" }, { BSSGP_PDUT_RESUME_NACK, "RESUME-NACK" }, + { BSSGP_PDUT_DUMMY_PAGING_PS, "DUMMY-PAGING-PS" }, + { BSSGP_PDUT_DUMMY_PAGING_PS_RESP, "DUMMY-PAGING-PS-RESP" }, + { BSSGP_PDUT_MS_REGISTR_ENQ, "MS-REGISTRATION-ENQ" }, + { BSSGP_PDUT_MS_REGISTR_ENQ_RESP, "MS-REGISTRATION-ENQ-RESP" }, { BSSGP_PDUT_BVC_BLOCK, "BVC-BLOCK" }, { BSSGP_PDUT_BVC_BLOCK_ACK, "BVC-BLOCK-ACK" }, { BSSGP_PDUT_BVC_RESET, "BVC-RESET" }, @@ -130,8 +136,11 @@ static const struct value_string bssgp_pdu_strings[] = { { BSSGP_PDUT_FLUSH_LL, "FLUSH-LL" }, { BSSGP_PDUT_FLUSH_LL_ACK, "FLUSH-LL-ACK" }, { BSSGP_PDUT_LLC_DISCARD, "LLC DISCARDED" }, + { BSSGP_PDUT_FLOW_CONTROL_PFC, "FLOW-CONTROL-PFC" }, + { BSSGP_PDUT_FLOW_CONTROL_PFC_ACK, "FLOW-CONTROL-PFC-ACK" }, { BSSGP_PDUT_SGSN_INVOKE_TRACE, "SGSN-INVOKE-TRACE" }, { BSSGP_PDUT_STATUS, "STATUS" }, + { BSSGP_PDUT_OVERLOAD, "OVERLOAD" }, { BSSGP_PDUT_DOWNLOAD_BSS_PFC, "DOWNLOAD-BSS-PFC" }, { BSSGP_PDUT_CREATE_BSS_PFC, "CREATE-BSS-PFC" }, { BSSGP_PDUT_CREATE_BSS_PFC_ACK, "CREATE-BSS-PFC-ACK" }, @@ -140,9 +149,338 @@ static const struct value_string bssgp_pdu_strings[] = { { BSSGP_PDUT_MODIFY_BSS_PFC_ACK, "MODIFY-BSS-PFC-ACK" }, { BSSGP_PDUT_DELETE_BSS_PFC, "DELETE-BSS-PFC" }, { BSSGP_PDUT_DELETE_BSS_PFC_ACK, "DELETE-BSS-PFC-ACK" }, + { BSSGP_PDUT_DELETE_BSS_PFC_REQ, "DELETE-BSS-PFC-REQ" }, + { BSSGP_PDUT_PS_HO_REQUIRED, "PS-HO-REQUIRED" }, + { BSSGP_PDUT_PS_HO_REQUIRED_ACK, "PS-HO-REQUIRED-ACK" }, + { BSSGP_PDUT_PS_HO_REQUIRED_NACK, "PS-HO-REQUIRED-NACK" }, + { BSSGP_PDUT_PS_HO_REQUEST, "PS-HO-REQUEST" }, + { BSSGP_PDUT_PS_HO_REQUEST_ACK, "PS-HO-REQUEST-ACK" }, + { BSSGP_PDUT_PS_HO_REQUEST_NACK, "PS-HO-REQUEST-NACK" }, + { BSSGP_PDUT_PS_HO_COMPLETE, "PS-HO-COMPLETE" }, + { BSSGP_PDUT_PS_HO_CANCEL, "PS-HO-CANCEL" }, + { BSSGP_PDUT_PS_HO_COMPLETE_ACK, "PS-HO-COMPLETE-ACK" }, + { BSSGP_PDUT_PERFORM_LOC_REQ, "PERFORM-LOC-REQ" }, + { BSSGP_PDUT_PERFORM_LOC_RESP, "PERFORM-LOC-RESP" }, + { BSSGP_PDUT_PERFORM_LOC_ABORT, "PERFORM-LOC-ABORT" }, + { BSSGP_PDUT_POSITION_COMMAND, "POSITION-COMMAND" }, + { BSSGP_PDUT_POSITION_RESPONSE, "POSITION-RESPONSE" }, + { BSSGP_PDUT_RAN_INFO, "RAN-INFO" }, + { BSSGP_PDUT_RAN_INFO_REQ, "RAN-INFO-REQ" }, + { BSSGP_PDUT_RAN_INFO_ACK, "RAN-INFO-ACK" }, + { BSSGP_PDUT_RAN_INFO_ERROR, "RAN-INFO-ERROR" }, + { BSSGP_PDUT_RAN_INFO_APP_ERROR, "RAN-INFO-APP-ERROR" }, + { BSSGP_PDUT_MBMS_START_REQ, "MBMS-START-REQ" }, + { BSSGP_PDUT_MBMS_START_RESP, "MBMS-START-RESP" }, + { BSSGP_PDUT_MBMS_STOP_REQ, "MBMS-STOP-REQ" }, + { BSSGP_PDUT_MBMS_STOP_RESP, "MBMS-STOP-RESP" }, + { BSSGP_PDUT_MBMS_UPDATE_REQ, "MBMS-UPDATE-REQ" }, + { BSSGP_PDUT_MBMS_UPDATE_RESP, "MBMS-UPDATE-RESP" }, { 0, NULL }, }; +static const uint8_t dl_ud_ies[] = { BSSGP_IE_PDU_LIFETIME }; +static const uint8_t ul_ud_ies[] = { BSSGP_IE_CELL_ID }; +static const uint8_t ra_cap_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_MS_RADIO_ACCESS_CAP }; +static const uint8_t dl_mb_ud_ies[] = { BSSGP_IE_PDU_LIFETIME, BSSGP_IE_TMGI, BSSGP_IE_LLC_PDU }; +static const uint8_t ul_mb_ud_ies[] = { BSSGP_IE_PDU_LIFETIME, BSSGP_IE_TMGI, BSSGP_IE_LLC_PDU }; +static const uint8_t pag_ps_ies[] = { BSSGP_IE_IMSI, BSSGP_IE_QOS_PROFILE }; +static const uint8_t pag_cs_ies[] = { BSSGP_IE_IMSI, BSSGP_IE_DRX_PARAMS }; +static const uint8_t ra_cap_upd_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG }; +static const uint8_t ra_cap_upd_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG, BSSGP_IE_RA_CAP_UPD_CAUSE }; +static const uint8_t rad_sts_ies[] = { BSSGP_IE_RADIO_CAUSE }; +static const uint8_t suspend_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA }; +static const uint8_t suspend_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA, BSSGP_IE_SUSPEND_REF_NR }; +static const uint8_t suspend_nack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA }; +static const uint8_t resume_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA, BSSGP_IE_SUSPEND_REF_NR }; +static const uint8_t resume_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA }; +static const uint8_t resume_nack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_ROUTEING_AREA }; +static const uint8_t d_pag_ps_ies[] = { BSSGP_IE_IMSI }; +static const uint8_t d_pag_ps_resp_ies[] = { BSSGP_IE_IMSI, BSSGP_IE_T_UNTIL_NEXT_PAGING }; +static const uint8_t d_pag_ps_rej_ies[] = { BSSGP_IE_IMSI, BSSGP_IE_T_UNTIL_NEXT_PAGING }; +static const uint8_t ms_reg_enq_ies[] = { BSSGP_IE_IMSI }; +static const uint8_t ms_reg_enq_res_ies[] = { BSSGP_IE_IMSI }; +static const uint8_t flush_ll_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_BVCI }; +static const uint8_t flush_ll_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_FLUSH_ACTION }; +static const uint8_t llc_disc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_LLC_FRAMES_DISCARDED, BSSGP_IE_BVCI, + BSSGP_IE_NUM_OCT_AFF }; +static const uint8_t fc_bvc_ies[] = { BSSGP_IE_TAG, BSSGP_IE_BVC_BUCKET_SIZE, BSSGP_IE_BUCKET_LEAK_RATE, + BSSGP_IE_BMAX_DEFAULT_MS, BSSGP_IE_R_DEFAULT_MS }; +static const uint8_t fc_bvc_ack_ies[] = { BSSGP_IE_TAG }; +static const uint8_t fc_ms_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG, BSSGP_IE_MS_BUCKET_SIZE, + BSSGP_IE_BUCKET_LEAK_RATE }; +static const uint8_t fc_ms_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG }; +static const uint8_t block_ies[] = { BSSGP_IE_BVCI, BSSGP_IE_CAUSE }; +static const uint8_t block_ack_ies[] = { BSSGP_IE_BVCI }; +static const uint8_t unblock_ies[] = { BSSGP_IE_BVCI }; +static const uint8_t unblock_ack_ies[] = { BSSGP_IE_BVCI }; +static const uint8_t reset_ies[] = { BSSGP_IE_BVCI, BSSGP_IE_CAUSE }; +static const uint8_t reset_ack_ies[] = { BSSGP_IE_BVCI }; +static const uint8_t status_ies[] = { BSSGP_IE_CAUSE }; +static const uint8_t inv_trc_ies[] = { BSSGP_IE_TRACE_TYPE, BSSGP_IE_TRACE_REFERENC }; +static const uint8_t dl_bss_pfc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID }; +static const uint8_t crt_bss_pfc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, + BSSGP_IE_PACKET_FLOW_TIMER, BSSGP_IE_AGG_BSS_QOS_PROFILE }; +static const uint8_t crt_bss_pfc_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, + BSSGP_IE_AGG_BSS_QOS_PROFILE }; +static const uint8_t crt_bss_pfc_nack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, BSSGP_IE_CAUSE }; +static const uint8_t mod_bss_pfc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, + BSSGP_IE_AGG_BSS_QOS_PROFILE }; +static const uint8_t mod_bss_pfc_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, + BSSGP_IE_PACKET_FLOW_TIMER, BSSGP_IE_AGG_BSS_QOS_PROFILE }; +static const uint8_t del_bss_pfc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID }; +static const uint8_t del_bss_pfc_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID }; +static const uint8_t fc_pfc_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG, BSSGP_IE_PFC_FLOW_CTRL_PARAMS }; +static const uint8_t fc_pfc_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_TAG }; +static const uint8_t del_bss_pfc_req_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_PACKET_FLOW_ID, BSSGP_IE_CAUSE }; +static const uint8_t ps_ho_required_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_CAUSE, BSSGP_IE_CELL_ID, + BSSGP_IE_ACTIVE_PFC_LIST }; +static const uint8_t ps_ho_required_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_LIST_OF_SETUP_PFC }; +static const uint8_t ps_ho_required_nack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_CAUSE }; +static const uint8_t ps_ho_request_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_IMSI, BSSGP_IE_CAUSE, + BSSGP_IE_CELL_ID, BSSGP_IE_SBSS_TO_TBSS_TR_CONT, + BSSGP_IE_PFC_TO_BE_SETUP_LIST }; +static const uint8_t ps_ho_request_ack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_LIST_OF_SETUP_PFC, + BSSGP_IE_TBSS_TO_SBSS_TR_CONT }; +static const uint8_t ps_ho_request_nack_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_CAUSE }; +static const uint8_t ps_ho_compl_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_IMSI }; +static const uint8_t ps_ho_cancel_ies[] = { BSSGP_IE_TLLI, BSSGP_IE_CAUSE, BSSGP_IE_CELL_ID }; +static const uint8_t ps_ho_compl_ack_ies[] = { BSSGP_IE_TLLI }; +static const uint8_t overload_ies[] = { BSSGP_IE_PRIO_CLASS_IND }; +static const uint8_t rinfo_ies[] = { BSSGP_IE_RIM_ROUTING_INFO, BSSGP_IE_RI_RIM_CONTAINER }; +static const uint8_t rinfo_req_ies[] = { BSSGP_IE_RIM_ROUTING_INFO, BSSGP_IE_RI_REQ_RIM_CONTAINER }; +static const uint8_t rinfo_ack_ies[] = { BSSGP_IE_RIM_ROUTING_INFO, BSSGP_IE_RI_ACK_RIM_CONTAINER }; +static const uint8_t rinfo_err_ies[] = { BSSGP_IE_RIM_ROUTING_INFO, BSSGP_IE_RI_ERROR_RIM_COINTAINER }; +static const uint8_t rinfo_aerr_ies[] = { BSSGP_IE_RIM_ROUTING_INFO, BSSGP_IE_RI_APP_ERROR_RIM_CONT }; + +#define DL BSSGP_PDUF_DL +#define UL BSSGP_PDUF_UL +#define SIG BSSGP_PDUF_SIG +#define PTP BSSGP_PDUF_PTP +#define PTM BSSGP_PDUF_PTM + +const struct osmo_tlv_prot_def osmo_pdef_bssgp = { + .name = "BSSGP", + .tlv_def = &tvlv_att_def, + .msg_def = { + [BSSGP_PDUT_DL_UNITDATA] = MSG_DEF("DL-UNITDATA", dl_ud_ies, DL|PTP), + [BSSGP_PDUT_UL_UNITDATA] = MSG_DEF("UL-UNITDATA", ul_ud_ies, UL|PTP), + [BSSGP_PDUT_RA_CAPABILITY] = MSG_DEF("RA-CAPABILITY", ra_cap_ies, DL|PTP), + [BSSGP_PDUT_DL_MMBS_UNITDATA] = MSG_DEF("DL-MBMS-UNITDATA", dl_mb_ud_ies, DL|PTM), + [BSSGP_PDUT_UL_MMBS_UNITDATA] = MSG_DEF("UL-MBMS-UNITDATA", ul_mb_ud_ies, UL|PTM), + [BSSGP_PDUT_PAGING_PS] = MSG_DEF("PAGING-PS", pag_ps_ies, DL|PTP|SIG), + [BSSGP_PDUT_PAGING_CS] = MSG_DEF("PAGING-CS", pag_cs_ies, DL|PTP|SIG), + [BSSGP_PDUT_RA_CAPA_UDPATE] = MSG_DEF("RA-CAPABILITY-UPDATE", ra_cap_upd_ies, UL|PTP), + [BSSGP_PDUT_RA_CAPA_UPDATE_ACK] = MSG_DEF("RA-CAPABILITY-UPDATE-ACK", ra_cap_upd_ack_ies, DL|PTP), + [BSSGP_PDUT_RADIO_STATUS] = MSG_DEF("RADIO-STATUS", rad_sts_ies, UL|PTP), + [BSSGP_PDUT_SUSPEND] = MSG_DEF("SUSPEND", suspend_ies, UL|SIG), + [BSSGP_PDUT_SUSPEND_ACK] = MSG_DEF("SUSPEND-ACK", suspend_ack_ies, DL|SIG), + [BSSGP_PDUT_SUSPEND_NACK] = MSG_DEF("SUSPEND-NACK", suspend_nack_ies, DL|SIG), + [BSSGP_PDUT_RESUME] = MSG_DEF("RESUME", resume_ies, UL|SIG), + [BSSGP_PDUT_RESUME_ACK] = MSG_DEF("RESUME-ACK", resume_ack_ies, DL|SIG), + [BSSGP_PDUT_RESUME_NACK] = MSG_DEF("RESUME-NACK", resume_nack_ies, DL|SIG), + [BSSGP_PDUT_DUMMY_PAGING_PS] = MSG_DEF("DUMMY-PAGING-PS", d_pag_ps_ies, DL|SIG|PTP), + [BSSGP_PDUT_DUMMY_PAGING_PS_RESP] = MSG_DEF("DUMMY-PAGING-PS-RESP", d_pag_ps_resp_ies, UL|SIG|PTP), + [BSSGP_PDUT_PAGING_PS_REJECT] = MSG_DEF("PAGING-PS-REJ", d_pag_ps_rej_ies, UL|SIG|PTP), + [BSSGP_PDUT_MS_REGISTR_ENQ] = MSG_DEF("MS-REGISRATION-ENQ", ms_reg_enq_ies, UL|SIG), + [BSSGP_PDUT_MS_REGISTR_ENQ_RESP] = MSG_DEF("MS-REGISRATION-ENQ-RESP", ms_reg_enq_res_ies, DL|SIG), + [BSSGP_PDUT_FLUSH_LL] = MSG_DEF("FLUSH-LL", flush_ll_ies, DL|SIG), + [BSSGP_PDUT_FLUSH_LL_ACK] = MSG_DEF("FLUSH-LL-ACK", flush_ll_ack_ies, UL|SIG), + [BSSGP_PDUT_LLC_DISCARD] = MSG_DEF("LLC-DISCARDED", llc_disc_ies, UL|SIG), + [BSSGP_PDUT_FLOW_CONTROL_BVC] = MSG_DEF("FC-BVC", fc_bvc_ies, UL|PTP), + [BSSGP_PDUT_FLOW_CONTROL_BVC_ACK] = MSG_DEF("FC-BVC-ACK", fc_bvc_ack_ies, DL|PTP), + [BSSGP_PDUT_FLOW_CONTROL_MS] = MSG_DEF("FC-MS", fc_ms_ies, UL|PTP), + [BSSGP_PDUT_FLOW_CONTROL_MS_ACK] = MSG_DEF("FC-MS-ACK", fc_ms_ack_ies, DL|PTP), + [BSSGP_PDUT_BVC_BLOCK] = MSG_DEF("BVC-BLOCK", block_ies, UL|SIG), + [BSSGP_PDUT_BVC_BLOCK_ACK] = MSG_DEF("BVC-BLOCK-ACK", block_ack_ies, DL|SIG), + [BSSGP_PDUT_BVC_UNBLOCK] = MSG_DEF("BVC-UNBLOCK", unblock_ies, UL|SIG), + [BSSGP_PDUT_BVC_UNBLOCK_ACK] = MSG_DEF("BVC-UNBLOCK-ACK", unblock_ack_ies, DL|SIG), + [BSSGP_PDUT_BVC_RESET] = MSG_DEF("BVC-RESET", reset_ies, UL|DL|SIG|PTP), + [BSSGP_PDUT_BVC_RESET_ACK] = MSG_DEF("BVC-RESET-ACK", reset_ack_ies, UL|DL|SIG|PTP), + [BSSGP_PDUT_STATUS] = MSG_DEF("STATUS", status_ies, UL|DL|PTP|SIG|PTM), + [BSSGP_PDUT_SGSN_INVOKE_TRACE] = MSG_DEF("SGSN-INVOKE-TRACE", inv_trc_ies, DL|SIG), + [BSSGP_PDUT_DOWNLOAD_BSS_PFC] = MSG_DEF("DOWNLOAD-BSS-PFC", dl_bss_pfc_ies, UL|PTP), + [BSSGP_PDUT_CREATE_BSS_PFC] = MSG_DEF("CREATE-BSS-PFC", crt_bss_pfc_ies, DL|PTP), + [BSSGP_PDUT_CREATE_BSS_PFC_ACK] = MSG_DEF("CREATE-BSS-PFC-ACK", crt_bss_pfc_ack_ies, UL|PTP), + [BSSGP_PDUT_CREATE_BSS_PFC_NACK] = MSG_DEF("CREATE-BSS-PFC-NACK", crt_bss_pfc_nack_ies, UL|PTP), + [BSSGP_PDUT_MODIFY_BSS_PFC] = MSG_DEF("MODIFY-BSS-PFC", mod_bss_pfc_ies, DL|PTP), + [BSSGP_PDUT_MODIFY_BSS_PFC_ACK] = MSG_DEF("MODIFY-BSS-PFC-ACK", mod_bss_pfc_ack_ies, UL|PTP), + [BSSGP_PDUT_DELETE_BSS_PFC] = MSG_DEF("DELETE-BSS-PFC", del_bss_pfc_ies, DL|PTP), + [BSSGP_PDUT_DELETE_BSS_PFC_ACK] = MSG_DEF("DELETE-BSS-PFC-ACK", del_bss_pfc_ack_ies, UL|PTP), + [BSSGP_PDUT_FLOW_CONTROL_PFC] = MSG_DEF("FC-PFC", fc_pfc_ies, UL|PTP), + [BSSGP_PDUT_FLOW_CONTROL_PFC_ACK] = MSG_DEF("FC-PFC-ACK", fc_pfc_ack_ies, DL|PTP), + [BSSGP_PDUT_DELETE_BSS_PFC_REQ] = MSG_DEF("DELETE-BSS-PFC-REQ", del_bss_pfc_req_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_REQUIRED] = MSG_DEF("PS-HO-REQUIRED", ps_ho_required_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_REQUIRED_ACK] = MSG_DEF("PS-HO-REQUIRED-ACK", ps_ho_required_ack_ies, DL|PTP), + [BSSGP_PDUT_PS_HO_REQUIRED_NACK] = MSG_DEF("PS-HO-REQUIRED-NACK", ps_ho_required_nack_ies, DL|PTP), + [BSSGP_PDUT_PS_HO_REQUEST] = MSG_DEF("PS-HO-REQUEST", ps_ho_request_ies, DL|PTP), + [BSSGP_PDUT_PS_HO_REQUEST_ACK] = MSG_DEF("PS-HO-REQUEST-ACK", ps_ho_request_ack_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_REQUEST_NACK] = MSG_DEF("PS-HO-REQUEST-NACK", ps_ho_request_nack_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_COMPLETE] = MSG_DEF("PS-HO-COMPLETE", ps_ho_compl_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_CANCEL] = MSG_DEF("PS-HO-CANCEL", ps_ho_cancel_ies, UL|PTP), + [BSSGP_PDUT_PS_HO_COMPLETE_ACK] = MSG_DEF("PS-HO-COMPLETE-ACK", ps_ho_compl_ack_ies, DL|PTP), + [BSSGP_PDUT_OVERLOAD] = MSG_DEF("OVERLOAD", overload_ies, DL|SIG), + /* TODO: Messages on LCS SAP */ + /* Messages on RIM SAP */ + [BSSGP_PDUT_RAN_INFO] = MSG_DEF("RAN-INFORMATION", rinfo_ies, DL|UL|SIG), + [BSSGP_PDUT_RAN_INFO_REQ] = MSG_DEF("RAN-INFORMATION-REQUEST", rinfo_req_ies, DL|UL|SIG), + [BSSGP_PDUT_RAN_INFO_ACK] = MSG_DEF("RAN-INFORMATION-ACK", rinfo_ack_ies, DL|UL|SIG), + [BSSGP_PDUT_RAN_INFO_ERROR] = MSG_DEF("RAN-INFORMATION-ERROR", rinfo_err_ies, DL|UL|SIG), + [BSSGP_PDUT_RAN_INFO_APP_ERROR] = MSG_DEF("RAN-INFORMATION-APP-ERROR", rinfo_aerr_ies, DL|UL|SIG), + /* TODO: Messages on MBMS SAP */ + }, + .ie_def = { + [BSSGP_IE_ALIGNMENT] = { 0, "Alignment Octets" }, + [BSSGP_IE_BMAX_DEFAULT_MS] = { 2, "Bmax default MS" }, + [BSSGP_IE_BSS_AREA_ID] = { 1, "BSS Area Indication" }, + [BSSGP_IE_BUCKET_LEAK_RATE] = { 2, "Bucket Leak Rate (R)" }, + [BSSGP_IE_BVC_BUCKET_SIZE] = { 2, "BVC Bucket Size" }, + [BSSGP_IE_BVCI] = { 2, "BVCI" }, + [BSSGP_IE_BVC_MEASUREMENT] = {2, "BVC Measurement" }, + [BSSGP_IE_CAUSE] = { 1, "Cause" }, + [BSSGP_IE_CELL_ID] = { 8, "Cell Identifier" }, + [BSSGP_IE_CHAN_NEEDED] = { 1, "Channel Needed" }, + [BSSGP_IE_DRX_PARAMS] = { 2, "DRX Parameters" }, + [BSSGP_IE_EMLPP_PRIO] = { 3, "eMLPP Priority" }, + [BSSGP_IE_FLUSH_ACTION] = { 1, "Flush Action" }, + [BSSGP_IE_IMSI] = { 1, "Mobile Identity" }, + [BSSGP_IE_LLC_PDU] = { 0, "LLC-PDU" }, + [BSSGP_IE_LLC_FRAMES_DISCARDED] = { 1, "LLC Frames Discarded" }, + [BSSGP_IE_LOCATION_AREA] = { 5, "Location Area" }, + [BSSGP_IE_LSA_ID_LIST] = { 3, "LSA Identifier List" }, + [BSSGP_IE_LSA_INFORMATION] = { 5, "LSA Information" }, + [BSSGP_IE_MOBILE_ID] = { 1, "Mobile Identity" }, + [BSSGP_IE_MS_BUCKET_SIZE] = { 2, "MS Bucket Size" }, + [BSSGP_IE_MS_RADIO_ACCESS_CAP] = { 1, "MS Radio Access Capability" }, + [BSSGP_IE_OMC_ID] = { 1, "OMC Id" }, + [BSSGP_IE_PDU_IN_ERROR] = { 0, "PDU In Error" }, + [BSSGP_IE_PDU_LIFETIME] = { 2, "PDU Lifetime" }, + [BSSGP_IE_PRIORITY] = { 1, "Priority" }, + [BSSGP_IE_QOS_PROFILE] = { 3, "QoS Profile" }, + [BSSGP_IE_RADIO_CAUSE] = { 1, "Radio Cause" }, + [BSSGP_IE_RA_CAP_UPD_CAUSE] = { 1, "RA-Cap-UPD-Cause" }, + [BSSGP_IE_ROUTEING_AREA] = { 6, "Routeing Area" }, + [BSSGP_IE_R_DEFAULT_MS] = { 2, "R_default_MS" }, + [BSSGP_IE_SUSPEND_REF_NR] = { 1, "Suspend Reference Number" }, + [BSSGP_IE_TAG] = { 1, "Tag" }, + [BSSGP_IE_TLLI] = { 4, "TLLI" }, + [BSSGP_IE_TMSI] = { 4, "TMSI" }, + [BSSGP_IE_TRACE_REFERENC] = { 2, "Trace Reference" }, + [BSSGP_IE_TRACE_TYPE] = { 1, "Trace Type" }, + [BSSGP_IE_TRANSACTION_ID] = { 2, "Transaction Id" }, + [BSSGP_IE_TRIGGER_ID] = { 1, "Trigger Id" }, + [BSSGP_IE_NUM_OCT_AFF] = { 3, "Number of octets affected" }, + [BSSGP_IE_PACKET_FLOW_ID] = { 1, "Packet Flow Identifier (PFI)" }, + [BSSGP_IE_AGG_BSS_QOS_PROFILE] = { 14, "Aggregate BSS QoS Profile" }, + [BSSGP_IE_PACKET_FLOW_TIMER] = { 1, "GPRS Timer" }, + [BSSGP_IE_FEATURE_BITMAP] = { 1, "Feature Bitmap" }, + [BSSGP_IE_BUCKET_FULL_RATIO] = { 1, "Bucket Full Ratio" }, + [BSSGP_IE_SERVICE_UTRAN_CCO] = { 1, "Service UTRAN COO" }, + [BSSGP_IE_NSEI] = { 2, "NSEI" }, + [BSSGP_IE_RRLP_APDU] = { 1, "RLLP APDU" }, + [BSSGP_IE_LCS_QOS] = { 4, "LCS QoS" }, + [BSSGP_IE_LCS_CLIENT_TYPE] = { 1, "LCS Client Type" }, + [BSSGP_IE_REQUESTED_GPS_AST_DATA] = { 4, "Requested GPS Assistance Data" }, + [BSSGP_IE_LOCATION_TYPE] = { 2, "Location Type" }, + [BSSGP_IE_LOCATION_ESTIMATE] = { 1, "Location Estimate" }, + [BSSGP_IE_POSITIONING_DATA] = { 1, "Positioning Data" }, + [BSSGP_IE_DECIPHERING_KEYS] = { 15, "Deciphering Keys" }, + [BSSGP_IE_LCS_PRIORITY] = { 1, "LCS Priority" }, + [BSSGP_IE_LCS_CAUSE] = { 1, "LCS Cause" }, + [BSSGP_IE_LCS_CAPABILITY] = { 1, "LCS Capability" }, + [BSSGP_IE_RRLP_FLAGS] = { 1, "RRLP Flags" }, + [BSSGP_IE_RIM_APP_IDENTITY] = { 1, "RIM Application Identity" }, + [BSSGP_IE_RIM_SEQ_NR] = { 4, "RIM Sequence Number" }, + [BSSGP_IE_RIM_REQ_APP_CONTAINER] = { 12, "RIM-REQUEST RIM Container" }, + [BSSGP_IE_RAN_INFO_APP_CONTAINER] = { 12, "RAN-INFORMATION RIM Container" }, + [BSSGP_IE_RI_ACK_RIM_CONTAINER] = { 9, "RAN-INFORMATION-ACK RIM Container" }, + [BSSGP_IE_RI_ERROR_RIM_COINTAINER] = { 9, "RAN-INFOIRMATION-ERROR RIM Container" }, + [BSSGP_IE_RI_APP_ERROR_RIM_CONT] = { 14, "RAN-INFORMATION-APP-ERROR RIM Container" }, + [BSSGP_IE_RIM_PDU_INDICATIONS] = { 1, "RIM PDU Indications" }, + [BSSGP_IE_RIM_PROTOCOL_VERSION] = { 1, "RIM Protocol Version Number" }, + [BSSGP_IE_PFC_FLOW_CTRL_PARAMS] = { 7, "PFC FLow Control Parameters" }, + [BSSGP_IE_GLOBAL_CN_ID] = { 5, "Global CN-Id" }, + [BSSGP_IE_RIM_ROUTING_INFO] = { 1, "RIM Routing Information" }, + [BSSGP_IE_MBMS_SESSION_ID] = { 0, "MBMS Session Identity" }, + [BSSGP_IE_MBMS_SESSION_DURATION] = { 0, "MBMS Session Duration" }, + [BSSGP_IE_MBMS_SA_ID_LIST] = { 3, "MBMS Service Area Identity List" }, + [BSSGP_IE_MBMS_RESPONSE] = { 1, "MBMS Response" }, + [BSSGP_IE_MBMS_RA_LIST] = { 9, "MBMS Routing Area List" }, + [BSSGP_IE_MBMS_SESSION_INFO] = { 1, "MBMS Session Information" }, + [BSSGP_IE_TMGI] = { 6, "TMGI" }, + [BSSGP_IE_MBMS_STOP_CAUSE] = { 1, "MBM Stop Cause" }, + [BSSGP_IE_SBSS_TO_TBSS_TR_CONT] = { 7, "Source BSS to Target BSS Transparent Container" }, + [BSSGP_IE_TBSS_TO_SBSS_TR_CONT] = { 0, "Target BSS to Source BSS Transparent Container" }, + [BSSGP_IE_NAS_CONT_FOR_PS_HO] = { 0, "NAS container for PS Handover" }, + [BSSGP_IE_PFC_TO_BE_SETUP_LIST] = { 9, "PFCs to be set-up list" }, + [BSSGP_IE_LIST_OF_SETUP_PFC] = { 1, "List of set-up PFCs" }, + [BSSGP_IE_EXT_FEATURE_BITMAP] = { 1, "Extended Feature Bitmap" }, + [BSSGP_IE_SRC_TO_TGT_TR_CONT] = { 0, "Source to Target Transparent Container" }, + [BSSGP_IE_TGT_TO_SRC_TR_CONT] = { 0, "Target to Source Transparent Container" }, + [BSSGP_IE_NC_ID] = { 8, "RNC Identifier" }, + [BSSGP_IE_PAGE_MODE] = { 1, "Page Mode" }, + [BSSGP_IE_CONTAINER_ID] = { 1, "Container ID" }, + [BSSGP_IE_GLOBAL_TFI] = { 1, "Global TFI" }, + [BSSGP_IE_IMEI] = { 1, "IMEI" }, + [BSSGP_IE_TIME_TO_MBMS_DATA_XFR] = { 1, "Time to MBMS Data Transfer" }, + [BSSGP_IE_MBMS_SESSION_REP_NR] = { 1, "MBMS Session Repetition Number" }, + [BSSGP_IE_INTER_RAT_HO_INFO] = { 0, "Inter RAT Handover Info" }, + [BSSGP_IE_PS_HO_COMMAND] = { 0, "PS Handover Command" }, + [BSSGP_IE_PS_HO_INDICATIONS] = { 1, "PS Handover Indications" }, + [BSSGP_IE_SI_PSI_CONTAINER] = { 1, "SI/PSI Container" }, + [BSSGP_IE_ACTIVE_PFC_LIST] = { 2, "Active PFCs List" }, + [BSSGP_IE_VELOCITY_DATA] = { 0, "Velocity Data" }, + [BSSGP_IE_DTM_HO_COMMAND] = { 0, "DTM Handover Command" }, + [BSSGP_IE_CS_INDICATION] = { 1, "CS Indication" }, + [BSSGP_IE_RQD_GANNS_AST_DATA] = { 0, "Requested GANSS Assistance Data" }, + [BSSGP_IE_GANSS_LOCATION_TYPE] = { 1, "GANSS Location Type" }, + [BSSGP_IE_GANSS_POSITIONING_DATA] = { 0, "GANSS Positioning Data" }, + [BSSGP_IE_FLOW_CTRL_GRANULARITY] = { 1, "Flow Control Granularity" }, + [BSSGP_IE_ENB_ID] = { 6, "eNB Identifier" }, + [BSSGP_IE_EUTRAN_IRAT_HO_INFO] = { 0, "E-UTRAN Inter RAT Handover Info" }, + [BSSGP_IE_SUB_PID4RAT_FREQ_PRIO] = { 1, "Subscriber Profile ID for RAT/Frequency priority" }, + [BSSGP_IE_REQ4IRAT_HO_INFO] = { 1, "Request for Inter-RAT Handover Info" }, + [BSSGP_IE_RELIABLE_IRAT_HO_INFO] = { 1, "Reliable Inter-RAT Handover Info" }, + [BSSGP_IE_SON_TRANSFER_APP_ID] = { 0, "SON Transfer Application Identity" }, + [BSSGP_IE_CSG_ID] = { 5, "CSG Identifier" }, + [BSSGP_IE_TAC] = { 3, "Tracking Area Code" }, + [BSSGP_IE_REDIRECT_ATTEMPT_FLAG] = { 1, "Redirect Attempt Flag" }, + [BSSGP_IE_REDIRECTION_INDICATION] = { 1, "Redirection Indication" }, + [BSSGP_IE_REDIRECTION_COMPLETED] = { 1, "Redirection Completed" }, + [BSSGP_IE_UNCONF_SEND_STATE_VAR] = { 2, "Unconfirmed send state variable" }, + [BSSGP_IE_IRAT_MEASUREMENT_CONF] = { 10, "IRAT Measurement Configuration" }, + [BSSGP_IE_SCI] = { 1, "SCI" }, + [BSSGP_IE_GGSN_PGW_LOCATION] = { 1, "GGSN/P-GW Location" }, + [BSSGP_IE_SELECTED_PLMN_ID] = { 3, "Selected PLMN ID" }, + [BSSGP_IE_PRIO_CLASS_IND] = { 1, "Priority Class Indication" }, + [BSSGP_IE_SOURCE_CELL_ID] = { 6, "Source Cell ID" }, + [BSSGP_IE_IRAT_MEAS_CFG_E_EARFCN] = { 10, "IRAT Measurement Configuration (extended E-ARFCNs)" }, + [BSSGP_IE_EDRX_PARAMETERS] = { 1, "eDRX Parameters" }, + [BSSGP_IE_T_UNTIL_NEXT_PAGING] = { 2, "Time Until Next Paging Occasion" }, + [BSSGP_IE_COVERAGE_CLASS] = { 1, "Coverage Class" }, + [BSSGP_IE_PAGING_ATTEMPT_INFO] = { 1, "Paging Attempt Information" }, + [BSSGP_IE_EXCEPTION_REPORT_FLAG] = { 1, "Exception Report Flag" }, + [BSSGP_IE_OLD_RA_ID] = { 6, "Old Routing Area Identification" }, + [BSSGP_IE_ATTACH_IND] = { 1, "Attach Indicator" }, + [BSSGP_IE_PLMN_ID] = { 3, "PLMN Identity" }, + [BSSGP_IE_MME_QUERY] = { 1, "MME Query" }, + [BSSGP_IE_SGSN_GROUP_ID] = { 3, "SGSN Group Identity" }, + [BSSGP_IE_ADDITIONAL_PTMSI] = { 4, "Additional P-TMSI" }, + [BSSGP_IE_UE_USAGE_TYPE] = { 1, "UE Usage Type" }, + [BSSGP_IE_MLAT_TIMER] = { 1, "Multilateration Timer" }, + [BSSGP_IE_MLAT_TA] = { 2, "Multilateration Timing Advance" }, + [BSSGP_IE_MS_SYNC_ACCURACY] = { 1, "MS Sync Accuracy" }, + [BSSGP_IE_BTS_RX_ACCURACY_LVL] = { 1, "BTS Reception Accuracy Level" }, + [BSSGP_IE_TA_REQ] = { 1, "Timing Advance Request (TAR)" }, + }, +}; + +#undef DL +#undef UL +#undef SIG +#undef PTP +#undef PTM + + const char *bssgp_cause_str(enum gprs_bssgp_cause cause) { return get_value_string(bssgp_cause_strings, cause); @@ -210,7 +548,7 @@ int bssgp_tx_simple_bvci(uint8_t pdu_type, uint16_t nsei, _bvci = osmo_htons(bvci); msgb_tvlv_put(msg, BSSGP_IE_BVCI, 2, (uint8_t *) &_bvci); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } /* Chapter 10.4.14: Status */ @@ -224,17 +562,17 @@ int bssgp_tx_status(uint8_t cause, uint16_t *bvci, struct msgb *orig_msg) cause is either "BVCI blocked" or "BVCI unknown" */ if (cause == BSSGP_CAUSE_UNKNOWN_BVCI || cause == BSSGP_CAUSE_BVCI_BLOCKED) { if (bvci == NULL) - LOGP(DBSSGP, LOGL_ERROR, "BSSGP Tx STATUS, cause=%s: " + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Tx STATUS, cause=%s: " "missing conditional BVCI\n", bssgp_cause_str(cause)); } else { if (bvci != NULL) - LOGP(DBSSGP, LOGL_ERROR, "BSSGP Tx STATUS, cause=%s: " + LOGP(DLBSSGP, LOGL_ERROR, "BSSGP Tx STATUS, cause=%s: " "unexpected conditional BVCI\n", bssgp_cause_str(cause)); } - LOGP(DBSSGP, LOGL_NOTICE, "BSSGP BVCI=%u Tx STATUS, cause=%s\n", + LOGP(DLBSSGP, LOGL_NOTICE, "BSSGP BVCI=%u Tx STATUS, cause=%s\n", bvci ? *bvci : 0, bssgp_cause_str(cause)); msgb_nsei(msg) = msgb_nsei(orig_msg); msgb_bvci(msg) = 0; @@ -248,5 +586,5 @@ int bssgp_tx_status(uint8_t cause, uint16_t *bvci, struct msgb *orig_msg) msgb_tvlv_put(msg, BSSGP_IE_PDU_IN_ERROR, msgb_bssgp_len(orig_msg), msgb_bssgph(orig_msg)); - return gprs_ns_sendmsg(bssgp_nsi, msg); + return bssgp_ns_send(bssgp_ns_send_data, msg); } diff --git a/src/gb/gprs_bssgp_vty.c b/src/gb/gprs_bssgp_vty.c index 5dab94e7..d04641d9 100644 --- a/src/gb/gprs_bssgp_vty.c +++ b/src/gb/gprs_bssgp_vty.c @@ -44,8 +44,6 @@ #include <osmocom/vty/telnet_interface.h> #include <osmocom/vty/misc.h> -#include "common_vty.h" - static void log_set_bvc_filter(struct log_target *target, struct bssgp_bvc_ctx *bctx) { @@ -207,15 +205,15 @@ DEFUN(logging_fltr_bvc, int bssgp_vty_init(void) { - install_element_ve(&show_bssgp_cmd); - install_element_ve(&show_bssgp_stats_cmd); - install_element_ve(&show_bvc_cmd); - install_element_ve(&logging_fltr_bvc_cmd); - install_element_ve(&bvc_reset_cmd); + install_lib_element_ve(&show_bssgp_cmd); + install_lib_element_ve(&show_bssgp_stats_cmd); + install_lib_element_ve(&show_bvc_cmd); + install_lib_element_ve(&logging_fltr_bvc_cmd); + install_lib_element_ve(&bvc_reset_cmd); - install_element(CFG_LOG_NODE, &logging_fltr_bvc_cmd); + install_lib_element(CFG_LOG_NODE, &logging_fltr_bvc_cmd); - install_element(CONFIG_NODE, &cfg_bssgp_cmd); + install_lib_element(CONFIG_NODE, &cfg_bssgp_cmd); install_node(&bssgp_node, config_write_bssgp); return 0; diff --git a/src/gb/gprs_ns.c b/src/gb/gprs_ns.c index c77ebb37..304fe7c1 100644 --- a/src/gb/gprs_ns.c +++ b/src/gb/gprs_ns.c @@ -29,7 +29,7 @@ * @{ * * GPRS Networks Service (NS) messages on the Gb interface - * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) * * Some introduction into NS: NS is used typically on top of frame relay, * but in the ip.access world it is encapsulated in UDP packets. It serves @@ -51,7 +51,7 @@ * * There can be multiple BSSGP virtual connections over one (group of) NSVC's. BSSGP will * therefore identify the BSSGP virtual connection by a BVCI passed down to NS. - * NS then has to firgure out which NSVC's are responsible for this BVCI. + * NS then has to figure out which NSVC's are responsible for this BVCI. * Those mappings are administratively configured. * * This implementation has the following limitations: @@ -318,7 +318,8 @@ struct gprs_nsvc *gprs_nsvc_create2(struct gprs_ns_inst *nsi, uint16_t nsvci, return NULL; } - LOGP(DNS, LOGL_INFO, "NSVCI=%u Creating NS-VC\n", nsvci); + LOGP(DNS, LOGL_INFO, "NSVCI=%u Creating NS-VC with Signal weight %u, Data weight %u\n", + nsvci, sig_weight, data_weight); nsvc = talloc_zero(nsi, struct gprs_nsvc); if (!nsvc) @@ -326,7 +327,7 @@ struct gprs_nsvc *gprs_nsvc_create2(struct gprs_ns_inst *nsi, uint16_t nsvci, nsvc->nsvci = nsvci; nsvc->nsvci_is_valid = 1; /* before RESET procedure: BLOCKED and DEAD */ - if (nsi->bss_sns_fi) + if (nsi->bss_sns_fi || !nsi->nsip.use_reset_block_unblock) ns_set_state(nsvc, 0); else ns_set_state(nsvc, NSE_S_BLOCKED); @@ -346,19 +347,12 @@ struct gprs_nsvc *gprs_nsvc_create2(struct gprs_ns_inst *nsi, uint16_t nsvci, return nsvc; } -/*! Old API for creating a NS-VC. Uses gprs_nsvc_create2 with fixed weights. */ -struct gprs_nsvc *gprs_nsvc_create(struct gprs_ns_inst *nsi, uint16_t nsvci) -{ - return gprs_nsvc_create2(nsi, nsvci, 1, 1); -} - /*! Delete given NS-VC * \param[in] nsvc gprs_nsvc to be deleted */ void gprs_nsvc_delete(struct gprs_nsvc *nsvc) { - if (osmo_timer_pending(&nsvc->timer)) - osmo_timer_del(&nsvc->timer); + osmo_timer_del(&nsvc->timer); llist_del(&nsvc->list); rate_ctr_group_free(nsvc->ctrg); osmo_stat_item_group_free(nsvc->statg); @@ -491,8 +485,8 @@ static int gprs_ns_tx(struct gprs_nsvc *nsvc, struct msgb *msg) } /* Increment number of Uplink bytes */ - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_PKTS_OUT]); - rate_ctr_add(&nsvc->ctrg->ctr[NS_CTR_BYTES_OUT], msgb_l2len(msg)); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_PKTS_OUT)); + rate_ctr_add(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_BYTES_OUT), msgb_l2len(msg)); switch (nsvc->ll) { case GPRS_NS_LL_UDP: @@ -538,7 +532,7 @@ static int gprs_ns_tx_simple(struct gprs_nsvc *nsvc, uint8_t pdu_type) /*! Transmit a NS-RESET on a given NSVC * \param[in] nsvc NS-VC used for transmission - * \paam[in] cause Numeric NS cause value + * \param[in] cause Numeric NS cause value */ int gprs_ns_tx_reset(struct gprs_nsvc *nsvc, uint8_t cause) { @@ -624,7 +618,7 @@ int gprs_ns_tx_status(struct gprs_nsvc *nsvc, uint8_t cause, return gprs_ns_tx(nsvc, msg); } -/*! Transmit a NS-BLOCK on a tiven NS-VC +/*! Transmit a NS-BLOCK on a given NS-VC * \param[in] nsvc NS-VC on which the NS-BLOCK is to be transmitted * \param[in] cause Numeric NS Cause value * \returns 0 in case of success @@ -648,7 +642,7 @@ int gprs_ns_tx_block(struct gprs_nsvc *nsvc, uint8_t cause) /* be conservative and mark it as blocked even now! */ ns_mark_blocked(nsvc); - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_BLOCKED]); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_BLOCKED)); msg->l2h = msgb_put(msg, sizeof(*nsh)); nsh = (struct gprs_ns_hdr *) msg->l2h; @@ -755,8 +749,7 @@ static void nsvc_start_timer(struct gprs_nsvc *nsvc, enum nsvc_timer_mode mode) nsvc->nsei, get_value_string(timer_mode_strs, mode), seconds); - if (osmo_timer_pending(&nsvc->timer)) - osmo_timer_del(&nsvc->timer); + osmo_timer_del(&nsvc->timer); osmo_gettimeofday(&nsvc->timer_started, NULL); nsvc->timer_mode = mode; @@ -786,15 +779,15 @@ static void gprs_ns_timer_cb(void *data) switch (nsvc->timer_mode) { case NSVC_TIMER_TNS_ALIVE: /* Tns-alive case: we expired without response ! */ - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_LOST_ALIVE]); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_LOST_ALIVE)); nsvc->alive_retries++; if (nsvc->alive_retries > nsvc->nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES]) { /* mark as dead (and blocked unless IP-SNS) */ - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_DEAD]); - if (!nsvc->nsi->bss_sns_fi) { + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_DEAD)); + if (!nsvc->nsi->bss_sns_fi && nsvc->nsi->nsip.use_reset_block_unblock) { ns_set_state(nsvc, NSE_S_BLOCKED); - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_BLOCKED]); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_BLOCKED)); } else ns_set_state(nsvc, 0); LOGP(DNS, LOGL_NOTICE, @@ -803,7 +796,7 @@ static void gprs_ns_timer_cb(void *data) nsvc->nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES]); ns_osmo_signal_dispatch(nsvc, S_NS_ALIVE_EXP, 0); /* FIXME: should we send this signal in case of SNS? */ - if (!nsvc->nsi->bss_sns_fi) + if (!nsvc->nsi->bss_sns_fi && nsvc->nsi->nsip.use_reset_block_unblock) ns_osmo_signal_dispatch(nsvc, S_NS_BLOCK, NS_CAUSE_NSVC_BLOCKED); return; } @@ -821,7 +814,7 @@ static void gprs_ns_timer_cb(void *data) nsvc_start_timer(nsvc, NSVC_TIMER_TNS_ALIVE); break; case NSVC_TIMER_TNS_RESET: - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_LOST_RESET]); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_LOST_RESET)); if (!(nsvc->state & NSE_S_RESET)) LOGP(DNS, LOGL_NOTICE, "NSEI=%u Reset timed out but RESET flag is not set\n", @@ -1078,10 +1071,10 @@ int gprs_ns_tx_sns_size_ack(struct gprs_nsvc *nsvc, uint8_t *cause) * \param[in] msg struct msgb to be trasnmitted * * This function obtains the NS-VC by the msgb_nsei(msg) and then checks - * if the NS-VC is ALIVEV and not BLOCKED. After that, it adds a NS + * if the NS-VC is ALIVE and not BLOCKED. After that, it adds a NS * header for the NS-UNITDATA message type and sends it off. * - * Section 9.2.10: transmit side / NS-UNITDATA-REQUEST primitive + * Section 9.2.10: transmit side / NS-UNITDATA-REQUEST primitive */ int gprs_ns_sendmsg(struct gprs_ns_inst *nsi, struct msgb *msg) { @@ -1256,7 +1249,7 @@ static int gprs_ns_rx_reset(struct gprs_nsvc **nsvc, struct msgb *msg) ns_osmo_signal_dispatch_mismatch(*nsvc, msg, NS_PDUT_RESET, NS_IE_VCI); - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_INV_VCI]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_INV_VCI)); gprs_ns_tx_reset_ack(*nsvc); return 0; } @@ -1268,7 +1261,7 @@ static int gprs_ns_rx_reset(struct gprs_nsvc **nsvc, struct msgb *msg) nsvci, (*nsvc)->nsvci, gprs_ns_ll_str(*nsvc)); orig_nsvc = *nsvc; - *nsvc = gprs_nsvc_create((*nsvc)->nsi, nsvci); + *nsvc = gprs_nsvc_create2((*nsvc)->nsi, nsvci, 1, 1); (*nsvc)->nsei = nsei; } } @@ -1282,14 +1275,14 @@ static int gprs_ns_rx_reset(struct gprs_nsvc **nsvc, struct msgb *msg) ns_osmo_signal_dispatch_mismatch(*nsvc, msg, NS_PDUT_RESET, NS_IE_NSEI); - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_INV_NSEI]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_INV_NSEI)); rc = gprs_ns_tx_reset_ack(*nsvc); CHECK_TX_RC(rc, *nsvc); return 0; } /* NSEI has changed */ - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_NSEI_CHG]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_NSEI_CHG)); (*nsvc)->nsei = nsei; } @@ -1297,7 +1290,7 @@ static int gprs_ns_rx_reset(struct gprs_nsvc **nsvc, struct msgb *msg) ns_set_state(*nsvc, NSE_S_BLOCKED | NSE_S_ALIVE); if (orig_nsvc) { - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_REPLACED]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_REPLACED)); ns_osmo_signal_dispatch_replaced(*nsvc, orig_nsvc); /* Update the ll info fields */ @@ -1395,7 +1388,7 @@ static int gprs_ns_rx_reset_ack(struct gprs_nsvc **nsvc, struct msgb *msg) ns_osmo_signal_dispatch_mismatch(*nsvc, msg, NS_PDUT_RESET_ACK, NS_IE_VCI); - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_INV_VCI]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_INV_VCI)); LOGP(DNS, LOGL_ERROR, "NS RESET ACK Unknown NS-VCI %d (%s NSEI=%d) " "from %s\n", @@ -1406,7 +1399,7 @@ static int gprs_ns_rx_reset_ack(struct gprs_nsvc **nsvc, struct msgb *msg) } /* Notify others */ - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_REPLACED]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_REPLACED)); ns_osmo_signal_dispatch_replaced(*nsvc, orig_nsvc); /* Update the ll info fields */ @@ -1420,7 +1413,7 @@ static int gprs_ns_rx_reset_ack(struct gprs_nsvc **nsvc, struct msgb *msg) ns_osmo_signal_dispatch_mismatch(*nsvc, msg, NS_PDUT_RESET_ACK, NS_IE_NSEI); - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_INV_NSEI]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_INV_NSEI)); LOGP(DNS, LOGL_ERROR, "NS RESET ACK Unknown NSEI %d (NS-VCI=%u) from %s\n", nsei, nsvci, gprs_ns_ll_str(*nsvc)); @@ -1428,14 +1421,14 @@ static int gprs_ns_rx_reset_ack(struct gprs_nsvc **nsvc, struct msgb *msg) } /* NSEI has changed */ - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_NSEI_CHG]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_NSEI_CHG)); (*nsvc)->nsei = nsei; } /* Mark NS-VC as blocked and alive */ ns_set_state(*nsvc, NSE_S_BLOCKED | NSE_S_ALIVE); ns_set_remote_state(*nsvc, NSE_S_BLOCKED | NSE_S_ALIVE); - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_BLOCKED]); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_BLOCKED)); if ((*nsvc)->persistent || (*nsvc)->remote_end_is_sgsn) { /* stop RESET timer */ osmo_timer_del(&(*nsvc)->timer); @@ -1476,7 +1469,7 @@ static int gprs_ns_rx_block(struct gprs_nsvc *nsvc, struct msgb *msg) //nsvci = (uint16_t *) TLVP_VAL(&tp, NS_IE_VCI); ns_osmo_signal_dispatch(nsvc, S_NS_BLOCK, *cause); - rate_ctr_inc(&nsvc->ctrg->ctr[NS_CTR_BLOCKED]); + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_BLOCKED)); return gprs_ns_tx_block_ack(nsvc); } @@ -1690,7 +1683,7 @@ int gprs_ns_vc_create(struct gprs_ns_inst *nsi, struct msgb *msg, * simply have changed addresses, or it is a SGSN */ existing_nsvc = gprs_nsvc_by_nsvci(nsi, nsvci); if (!existing_nsvc) { - *new_nsvc = gprs_nsvc_create(nsi, 0xffff); + *new_nsvc = gprs_nsvc_create2(nsi, 0xffff, 1, 1); (*new_nsvc)->nsvci_is_valid = 0; log_set_context(LOG_CTX_GB_NSVC, *new_nsvc); gprs_ns_ll_copy(*new_nsvc, fallback_nsvc); @@ -1710,7 +1703,7 @@ int gprs_ns_vc_create(struct gprs_ns_inst *nsi, struct msgb *msg, existing_nsvc->nsei = nsei; /* Do statistics */ - rate_ctr_inc(&existing_nsvc->ctrg->ctr[NS_CTR_NSEI_CHG]); + rate_ctr_inc(rate_ctr_group_get_ctr(existing_nsvc->ctrg, NS_CTR_NSEI_CHG)); } *new_nsvc = existing_nsvc; @@ -1739,8 +1732,8 @@ int gprs_ns_process_msg(struct gprs_ns_inst *nsi, struct msgb *msg, log_set_context(LOG_CTX_GB_NSVC, *nsvc); /* Increment number of Incoming bytes */ - rate_ctr_inc(&(*nsvc)->ctrg->ctr[NS_CTR_PKTS_IN]); - rate_ctr_add(&(*nsvc)->ctrg->ctr[NS_CTR_BYTES_IN], msgb_l2len(msg)); + rate_ctr_inc(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_PKTS_IN)); + rate_ctr_add(rate_ctr_group_get_ctr((*nsvc)->ctrg, NS_CTR_BYTES_IN), msgb_l2len(msg)); if (nsvc_is_not_used(*nsvc) && !ns_is_sns(nsh->pdu_type) && nsh->pdu_type != NS_PDUT_STATUS) { LOGP(DNS, LOGL_NOTICE, "NSEI=%u Rx %s on unused/pre-configured endpoint, discarding\n", @@ -1757,13 +1750,17 @@ int gprs_ns_process_msg(struct gprs_ns_inst *nsi, struct msgb *msg, * fine. */ if ((*nsvc)->state == NSE_S_BLOCKED) rc = gprs_nsvc_reset((*nsvc), NS_CAUSE_PDU_INCOMP_PSTATE); - else if (!((*nsvc)->state & NSE_S_RESET)) + else if (!((*nsvc)->state & NSE_S_RESET)) { + /* if we're not alive, we cannot transmit the ACK; set ALIVE */ + if (!((*nsvc)->state & NSE_S_ALIVE)) + ns_mark_alive(*nsvc); rc = gprs_ns_tx_alive_ack(*nsvc); + } break; case NS_PDUT_ALIVE_ACK: ns_mark_alive(*nsvc); if ((*nsvc)->timer_mode == NSVC_TIMER_TNS_ALIVE) - osmo_stat_item_set((*nsvc)->statg->items[NS_STAT_ALIVE_DELAY], + osmo_stat_item_set(osmo_stat_item_group_get_item((*nsvc)->statg, NS_STAT_ALIVE_DELAY), nsvc_timer_elapsed_ms(*nsvc)); /* stop Tns-alive and start Tns-test */ nsvc_start_timer(*nsvc, NSVC_TIMER_TNS_TEST); @@ -1884,13 +1881,18 @@ static bool gprs_sns_fsm_registered = false; */ struct gprs_ns_inst *gprs_ns_instantiate(gprs_ns_cb_t *cb, void *ctx) { - struct gprs_ns_inst *nsi = talloc_zero(ctx, struct gprs_ns_inst); + struct gprs_ns_inst *nsi; if (!gprs_sns_fsm_registered) { - gprs_sns_init(); + int rc = gprs_sns_init(); + if (rc < 0) + return NULL; gprs_sns_fsm_registered = true; } + nsi = talloc_zero(ctx, struct gprs_ns_inst); + if (!nsi) + return NULL; nsi->cb = cb; INIT_LLIST_HEAD(&nsi->gprs_nsvcs); nsi->timeout[NS_TOUT_TNS_BLOCK] = 3; @@ -1904,11 +1906,16 @@ struct gprs_ns_inst *gprs_ns_instantiate(gprs_ns_cb_t *cb, void *ctx) /* Create the dummy NSVC that we use for sending * messages to non-existant/unknown NS-VC's */ - nsi->unknown_nsvc = gprs_nsvc_create(nsi, 0xfffe); + nsi->unknown_nsvc = gprs_nsvc_create2(nsi, 0xfffe, 1, 1); nsi->unknown_nsvc->nsvci_is_valid = 0; llist_del(&nsi->unknown_nsvc->list); INIT_LLIST_HEAD(&nsi->unknown_nsvc->list); + /* By default we are in IPA compatible mode, that is we use NS-RESET, NS-BLOCK + * and NS-UNBLOCK procedures even for an IP/UDP based Gb interface, in violation + * of 3GPP TS 48.016. */ + nsi->nsip.use_reset_block_unblock = true; + return nsi; } @@ -2059,7 +2066,8 @@ int gprs_ns_nsip_listen(struct gprs_ns_inst *nsi) osmo_sock_init2_ofd(&nsi->nsip.fd, AF_INET, SOCK_DGRAM, IPPROTO_UDP, inet_ntoa(in), nsi->nsip.local_port, remote_str, - nsi->nsip.remote_port, OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT); + nsi->nsip.remote_port, OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT | + OSMO_SOCK_F_BIND | OSMO_SOCK_F_DSCP(nsi->nsip.dscp)); LOGP(DNS, LOGL_NOTICE, "Listening for nsip packets from %s:%u on %s:%u\n", @@ -2067,7 +2075,8 @@ int gprs_ns_nsip_listen(struct gprs_ns_inst *nsi) } else { /* Accept UDP packets from any source IP/Port */ ret = osmo_sock_init_ofd(&nsi->nsip.fd, AF_INET, SOCK_DGRAM, - IPPROTO_UDP, inet_ntoa(in), nsi->nsip.local_port, OSMO_SOCK_F_BIND); + IPPROTO_UDP, inet_ntoa(in), nsi->nsip.local_port, + OSMO_SOCK_F_BIND | OSMO_SOCK_F_DSCP(nsi->nsip.dscp)); LOGP(DNS, LOGL_NOTICE, "Listening for nsip packets on %s:%u\n", inet_ntoa(in), nsi->nsip.local_port); } @@ -2078,13 +2087,6 @@ int gprs_ns_nsip_listen(struct gprs_ns_inst *nsi) return ret; } - ret = setsockopt(nsi->nsip.fd.fd, IPPROTO_IP, IP_TOS, - &nsi->nsip.dscp, sizeof(nsi->nsip.dscp)); - if (ret < 0) - LOGP(DNS, LOGL_ERROR, - "Failed to set the DSCP to %d with ret(%d) errno(%d)\n", - nsi->nsip.dscp, ret, errno); - LOGP(DNS, LOGL_NOTICE, "NS UDP socket at %s:%d\n", inet_ntoa(in), nsi->nsip.local_port); return ret; @@ -2140,7 +2142,7 @@ struct gprs_nsvc *gprs_ns_nsip_connect(struct gprs_ns_inst *nsi, nsvc = gprs_nsvc_by_rem_addr(nsi, dest); if (!nsvc) - nsvc = gprs_nsvc_create(nsi, nsvci); + nsvc = gprs_nsvc_create2(nsi, nsvci, 1, 1); nsvc->ip.bts_addr = *dest; nsvc->nsei = nsei; nsvc->remote_end_is_sgsn = 1; diff --git a/src/gb/gprs_ns2.c b/src/gb/gprs_ns2.c new file mode 100644 index 00000000..4e496c1f --- /dev/null +++ b/src/gb/gprs_ns2.c @@ -0,0 +1,1695 @@ +/*! \file gprs_ns2.c + * GPRS Networks Service (NS) messages on the Gb interface. + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2009-2018 by Harald Welte <laforge@gnumonks.org> + * (C) 2016-2017,2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +/*! \addtogroup libgb + * @{ + * + * GPRS Networks Service (NS) messages on the Gb interface + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * + * Some introduction into NS: NS is used typically on top of frame relay, + * but in the ip.access world it is encapsulated in UDP packets. It serves + * as an intermediate shim betwen BSSGP and the underlying medium. It doesn't + * do much, apart from providing congestion notification and status indication. + * + * Terms: + * + * NS Network Service + * NSVC NS Virtual Connection + * NSEI NS Entity Identifier + * NSVL NS Virtual Link + * NSVLI NS Virtual Link Identifier + * BVC BSSGP Virtual Connection + * BVCI BSSGP Virtual Connection Identifier + * NSVCG NS Virtual Connection Goup + * Blocked NS-VC cannot be used for user traffic + * Alive Ability of a NS-VC to provide communication + * + * There can be multiple BSSGP virtual connections over one (group of) NSVC's. BSSGP will + * therefore identify the BSSGP virtual connection by a BVCI passed down to NS. + * NS then has to figure out which NSVC's are responsible for this BVCI. + * Those mappings are administratively configured. + * + * This implementation has the following limitations: + * - NSVCI 65535 and 65534 are reserved for internal use + * - There are no BLOCK and UNBLOCK timers (yet?) + * + * \file gprs_ns2.c */ + +#include <stdlib.h> +#include <unistd.h> +#include <errno.h> +#include <stdint.h> + +#include <sys/types.h> +#include <sys/socket.h> +#include <arpa/inet.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/core/stats.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/talloc.h> +#include <osmocom/gprs/gprs_msgb.h> +#include <osmocom/gsm/prim.h> +#include <osmocom/gsm/tlv.h> + +#include "gprs_ns2_internal.h" + +#define ns_set_state(ns_, st_) ns_set_state_with_log(ns_, st_, false, __FILE__, __LINE__) +#define ns_set_remote_state(ns_, st_) ns_set_state_with_log(ns_, st_, true, __FILE__, __LINE__) +#define ns_mark_blocked(ns_) ns_set_state(ns_, (ns_)->state | NSE_S_BLOCKED) +#define ns_mark_unblocked(ns_) ns_set_state(ns_, (ns_)->state & (~NSE_S_BLOCKED)); +#define ns_mark_alive(ns_) ns_set_state(ns_, (ns_)->state | NSE_S_ALIVE) +#define ns_mark_dead(ns_) ns_set_state(ns_, (ns_)->state & (~NSE_S_ALIVE)); + +/* HACK: The NS_IE_IP_ADDR does not follow any known TLV rules. + * Since it's a hard ABI break to implement 16 bit tag with fixed length entries to workaround it, + * the parser will be called with ns_att_tlvdef1 and if it's failed with ns_att_tlvdef2. + * The TLV parser depends on 8bit tag in many places. + * The NS_IE_IP_ADDR is only valid for SNS_ACK SNS_ADD and SNS_DELETE. + */ +static const struct tlv_definition ns_att_tlvdef1 = { + .def = { + [NS_IE_CAUSE] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_VCI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_PDU] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_BVCI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_NSEI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_IPv4_LIST] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_IPv6_LIST] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_MAX_NR_NSVC] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_IPv4_EP_NR] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_IPv6_EP_NR] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_RESET_FLAG] = { TLV_TYPE_TV, 0 }, + /* NS_IE_IP_ADDR in the IPv4 version */ + [NS_IE_IP_ADDR] = { TLV_TYPE_FIXED, 5 }, + }, +}; + +static const struct tlv_definition ns_att_tlvdef2 = { + .def = { + [NS_IE_CAUSE] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_VCI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_PDU] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_BVCI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_NSEI] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_IPv4_LIST] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_IPv6_LIST] = { TLV_TYPE_TvLV, 0 }, + [NS_IE_MAX_NR_NSVC] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_IPv4_EP_NR] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_IPv6_EP_NR] = { TLV_TYPE_FIXED, 2 }, + [NS_IE_RESET_FLAG] = { TLV_TYPE_TV, 0 }, + /* NS_IE_IP_ADDR in the IPv6 version */ + [NS_IE_IP_ADDR] = { TLV_TYPE_FIXED, 17 }, + }, +}; + + +/* Section 10.3.2, Table 13 */ +const struct value_string gprs_ns2_cause_strs[] = { + { NS_CAUSE_TRANSIT_FAIL, "Transit network failure" }, + { NS_CAUSE_OM_INTERVENTION, "O&M intervention" }, + { NS_CAUSE_EQUIP_FAIL, "Equipment failure" }, + { NS_CAUSE_NSVC_BLOCKED, "NS-VC blocked" }, + { NS_CAUSE_NSVC_UNKNOWN, "NS-VC unknown" }, + { NS_CAUSE_BVCI_UNKNOWN, "BVCI unknown" }, + { NS_CAUSE_SEM_INCORR_PDU, "Semantically incorrect PDU" }, + { NS_CAUSE_PDU_INCOMP_PSTATE, "PDU not compatible with protocol state" }, + { NS_CAUSE_PROTO_ERR_UNSPEC, "Protocol error, unspecified" }, + { NS_CAUSE_INVAL_ESSENT_IE, "Invalid essential IE" }, + { NS_CAUSE_MISSING_ESSENT_IE, "Missing essential IE" }, + { NS_CAUSE_INVAL_NR_IPv4_EP, "Invalid Number of IPv4 Endpoints" }, + { NS_CAUSE_INVAL_NR_IPv6_EP, "Invalid Number of IPv6 Endpoints" }, + { NS_CAUSE_INVAL_NR_NS_VC, "Invalid Number of NS-VCs" }, + { NS_CAUSE_INVAL_WEIGH, "Invalid Weights" }, + { NS_CAUSE_UNKN_IP_EP, "Unknown IP Endpoint" }, + { NS_CAUSE_UNKN_IP_ADDR, "Unknown IP Address" }, + { NS_CAUSE_UNKN_IP_TEST_FAILED, "IP Test Failed" }, + { 0, NULL } +}; + +static const struct rate_ctr_desc ns_ctr_description[] = { + [NS_CTR_PKTS_IN] = { "packets:in", "Packets at NS Level ( In)" }, + [NS_CTR_PKTS_OUT] = { "packets:out", "Packets at NS Level (Out)" }, + [NS_CTR_PKTS_OUT_DROP] = { "packets:out:drop", "Dropped Packets (Out)" }, + [NS_CTR_BYTES_IN] = { "bytes:in", "Bytes at NS Level ( In)" }, + [NS_CTR_BYTES_OUT] = { "bytes:out", "Bytes at NS Level (Out)" }, + [NS_CTR_BYTES_OUT_DROP] = { "bytes:out:drop", "Dropped Bytes (Out)" }, + [NS_CTR_BLOCKED] = { "blocked", "NS-VC Block count " }, + [NS_CTR_UNBLOCKED] = { "unblocked", "NS-VC Unblock count " }, + [NS_CTR_DEAD] = { "dead", "NS-VC gone dead count " }, + [NS_CTR_REPLACED] = { "replaced", "NS-VC replaced other count" }, + [NS_CTR_NSEI_CHG] = { "nsei-chg", "NS-VC changed NSEI count " }, + [NS_CTR_INV_VCI] = { "inv-nsvci", "NS-VCI was invalid count " }, + [NS_CTR_INV_NSEI] = { "inv-nsei", "NSEI was invalid count " }, + [NS_CTR_LOST_ALIVE] = { "lost:alive", "ALIVE ACK missing count " }, + [NS_CTR_LOST_RESET] = { "lost:reset", "RESET ACK missing count " }, +}; + +static const struct rate_ctr_group_desc nse_ctrg_desc = { + .group_name_prefix = "ns:nse", + .group_description = "NSE Peer Statistics", + .num_ctr = ARRAY_SIZE(ns_ctr_description), + .ctr_desc = ns_ctr_description, + .class_id = OSMO_STATS_CLASS_PEER, +}; + +static const struct rate_ctr_group_desc nsvc_ctrg_desc = { + .group_name_prefix = "ns:nsvc", + .group_description = "NSVC Peer Statistics", + .num_ctr = ARRAY_SIZE(ns_ctr_description), + .ctr_desc = ns_ctr_description, + .class_id = OSMO_STATS_CLASS_PEER, +}; + + +static const struct osmo_stat_item_desc nsvc_stat_description[] = { + [NS_STAT_ALIVE_DELAY] = { "alive.delay", "ALIVE response time ", "ms", 16, 0 }, +}; + +static const struct osmo_stat_item_group_desc nsvc_statg_desc = { + .group_name_prefix = "ns.nsvc", + .group_description = "NSVC Peer Statistics", + .num_items = ARRAY_SIZE(nsvc_stat_description), + .item_desc = nsvc_stat_description, + .class_id = OSMO_STATS_CLASS_PEER, +}; + +const struct osmo_stat_item_desc nsbind_stat_description[] = { + [NS2_BIND_STAT_BACKLOG_LEN] = { "tx_backlog_length", "Transmit backlog length", "packets", 16, 0 }, +}; + +static const struct osmo_stat_item_group_desc nsbind_statg_desc = { + .group_name_prefix = "ns.bind", + .group_description = "NS Bind Statistics", + .num_items = ARRAY_SIZE(nsbind_stat_description), + .item_desc = nsbind_stat_description, + .class_id = OSMO_STATS_CLASS_PEER, +}; + +const struct value_string gprs_ns2_aff_cause_prim_strs[] = { + { GPRS_NS2_AFF_CAUSE_VC_FAILURE, "NSVC failure" }, + { GPRS_NS2_AFF_CAUSE_VC_RECOVERY, "NSVC recovery" }, + { GPRS_NS2_AFF_CAUSE_FAILURE, "NSE failure" }, + { GPRS_NS2_AFF_CAUSE_RECOVERY, "NSE recovery" }, + { GPRS_NS2_AFF_CAUSE_SNS_CONFIGURED, "NSE SNS configured" }, + { GPRS_NS2_AFF_CAUSE_SNS_FAILURE, "NSE SNS failure" }, + { GPRS_NS2_AFF_CAUSE_SNS_NO_ENDPOINTS, "NSE SNS no endpoints"}, + { GPRS_NS2_AFF_CAUSE_MTU_CHANGE, "NSE MTU changed" }, + { 0, NULL } +}; + +const struct value_string gprs_ns2_prim_strs[] = { + { GPRS_NS2_PRIM_UNIT_DATA, "UNIT DATA" }, + { GPRS_NS2_PRIM_CONGESTION, "CONGESTION" }, + { GPRS_NS2_PRIM_STATUS, "STATUS" }, + { 0, NULL } +}; + +const struct value_string gprs_ns2_lltype_strs[] = { + { GPRS_NS2_LL_UDP, "UDP" }, + { GPRS_NS2_LL_FR_GRE, "FR_GRE" }, + { GPRS_NS2_LL_FR, "FR" }, + { 0, NULL } +}; + +/*! string-format a given NS-VC into a user-supplied buffer. + * \param[in] buf user-allocated output buffer + * \param[in] buf_len size of user-allocated output buffer in bytes + * \param[in] nsvc NS-VC to be string-formatted + * \return pointer to buf on success; NULL on error */ +char *gprs_ns2_ll_str_buf(char *buf, size_t buf_len, struct gprs_ns2_vc *nsvc) +{ + const struct osmo_sockaddr *local; + const struct osmo_sockaddr *remote; + struct osmo_sockaddr_str local_str; + struct osmo_sockaddr_str remote_str; + + if (!buf_len) + return NULL; + + switch (nsvc->nse->ll) { + case GPRS_NS2_LL_UDP: + if (!gprs_ns2_is_ip_bind(nsvc->bind)) { + buf[0] = '\0'; + return buf; + } + + local = gprs_ns2_ip_bind_sockaddr(nsvc->bind); + remote = gprs_ns2_ip_vc_remote(nsvc); + if (osmo_sockaddr_str_from_sockaddr(&local_str, &local->u.sas)) + strcpy(local_str.ip, "invalid"); + if (osmo_sockaddr_str_from_sockaddr(&remote_str, &remote->u.sas)) + strcpy(remote_str.ip, "invalid"); + + if (nsvc->nsvci_is_valid) + snprintf(buf, buf_len, "udp)[%s]:%u<%u>[%s]:%u", + local_str.ip, local_str.port, + nsvc->nsvci, + remote_str.ip, remote_str.port); + else + snprintf(buf, buf_len, "udp)[%s]:%u<>[%s]:%u", + local_str.ip, local_str.port, + remote_str.ip, remote_str.port); + break; + case GPRS_NS2_LL_FR_GRE: + snprintf(buf, buf_len, "frgre)"); + break; + case GPRS_NS2_LL_FR: + snprintf(buf, buf_len, "fr)netif: %s dlci: %u", gprs_ns2_fr_bind_netif(nsvc->bind), + gprs_ns2_fr_nsvc_dlci(nsvc)); + break; + default: + snprintf(buf, buf_len, "unknown)"); + break; + } + + buf[buf_len - 1] = '\0'; + + return buf; +} + +/* udp is the longest: udp)[IP6]:65536<65536>[IP6]:65536 */ +#define NS2_LL_MAX_STR 4+2*(INET6_ADDRSTRLEN+9)+8 + +/*! string-format a given NS-VC to a thread-local static buffer. + * \param[in] nsvc NS-VC to be string-formatted + * \return pointer to the string on success; NULL on error */ +const char *gprs_ns2_ll_str(struct gprs_ns2_vc *nsvc) +{ + static __thread char buf[NS2_LL_MAX_STR]; + return gprs_ns2_ll_str_buf(buf, sizeof(buf), nsvc); +} + +/*! string-format a given NS-VC to a dynamically allocated string. + * \param[in] ctx talloc context from which to allocate + * \param[in] nsvc NS-VC to be string-formatted + * \return pointer to the string on success; NULL on error */ +char *gprs_ns2_ll_str_c(const void *ctx, struct gprs_ns2_vc *nsvc) +{ + char *buf = talloc_size(ctx, NS2_LL_MAX_STR); + if (!buf) + return buf; + return gprs_ns2_ll_str_buf(buf, NS2_LL_MAX_STR, nsvc); +} + +/*! Return the current state name of a given NS-VC to a thread-local static buffer. + * \param[in] nsvc NS-VC to return the state of + * \return pointer to the string on success; NULL on error */ +const char *gprs_ns2_nsvc_state_name(struct gprs_ns2_vc *nsvc) +{ + return osmo_fsm_inst_state_name(nsvc->fi); +} + +/* select a signalling NSVC and respect sig_counter + * param[out] reset_counter - all counter has to be resetted to their signal weight + * return the chosen nsvc or NULL + */ +static struct gprs_ns2_vc *ns2_load_sharing_signal(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc = NULL, *last = NULL, *tmp; + + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (tmp->sig_weight == 0) + continue; + if (!ns2_vc_is_unblocked(tmp)) + continue; + if (tmp->sig_counter == 0) { + last = tmp; + continue; + } + + tmp->sig_counter--; + nsvc = tmp; + break; + } + + /* all counter were zero, but there are valid nsvc */ + if (!nsvc && last) { + llist_for_each_entry(tmp, &nse->nsvc, list) { + tmp->sig_counter = tmp->sig_weight; + } + + last->sig_counter--; + return last; + } else { + return nsvc; + } +} + +/* 4.4.1 Load Sharing function for the Frame Relay Sub-Network */ +static struct gprs_ns2_vc *ns2_load_sharing_modulo( + struct gprs_ns2_nse *nse, + uint16_t bvci, + uint32_t load_selector) +{ + struct gprs_ns2_vc *tmp; + uint32_t mod; + uint32_t i = 0; + + if (nse->nsvc_count == 0) + return NULL; + + mod = (bvci + load_selector) % nse->nsvc_count; + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (!ns2_vc_is_unblocked(tmp)) + continue; + if (i == mod) + return tmp; + i++; + } + + return NULL; +} + +/* 4.4.2 Load Sharing function for the IP Sub-Network + * + * Implement a simple approach for UDP load sharing of data weight based on the modulo of the lsp. + * + * E.g. 3 NSVC: 1st weight 5, 2nd weight 3, 3rd weight 1, lsp = 3. + * sum all weights = 9 + * target_weight = lsp % sum = 3 + * + * 1st NSVC will be the target for 0-4 + * 2nd NSVC will be the target for 5-7 + * 3rd NSVC will be the target for 8 + * + * The 1st NSVC will be used. + * E.g. lsp = 7. The 2nd NSVC will used. + */ +static struct gprs_ns2_vc *ns2_load_sharing_weight_modulo( + struct gprs_ns2_nse *nse, + uint16_t bvci, + uint32_t load_selector) +{ + struct gprs_ns2_vc *tmp; + uint32_t mod; + uint32_t i = 0; + + if (nse->nsvc_count == 0) + return NULL; + + mod = (bvci + load_selector) % nse->sum_data_weight; + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (tmp->data_weight == 0) + continue; + if (!ns2_vc_is_unblocked(tmp)) + continue; + if (i == mod || mod < i + tmp->data_weight) + return tmp; + i += tmp->data_weight; + } + + return NULL; +} + +/* pick the first available data NSVC - no load sharing */ +struct gprs_ns2_vc *ns2_load_sharing_first(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc = NULL, *tmp; + + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (!ns2_vc_is_unblocked(tmp)) + continue; + if (tmp->data_weight == 0) + continue; + + nsvc = tmp; + break; + } + + return nsvc; +} + + +static struct gprs_ns2_vc *ns2_load_sharing( + struct gprs_ns2_nse *nse, + uint16_t bvci, + uint32_t link_selector) +{ + struct gprs_ns2_vc *nsvc = NULL; + + switch (nse->ll) { + case GPRS_NS2_LL_FR: + nsvc = ns2_load_sharing_modulo(nse, bvci, link_selector); + break; + case GPRS_NS2_LL_UDP: + default: + if (bvci == 0) { + /* signalling */ + nsvc = ns2_load_sharing_signal(nse); + } else { + /* data with load sharing parameter */ + nsvc = ns2_load_sharing_weight_modulo(nse, bvci, link_selector); + } + break; + } + + return nsvc; +} + +/*! Receive a primitive from the NS User (Gb). + * \param[in] nsi NS instance to which the primitive is issued + * \param[in] oph The primitive + * \return 0 on success; negative on error */ +int gprs_ns2_recv_prim(struct gprs_ns2_inst *nsi, struct osmo_prim_hdr *oph) +{ + /* TODO: implement resource distribution */ + /* TODO: check for empty PDUs which can be sent to Request/Confirm + * the IP endpoint */ + struct osmo_gprs_ns2_prim *nsp; + struct gprs_ns2_nse *nse = NULL; + struct gprs_ns2_vc *nsvc = NULL; + uint16_t bvci, nsei; + uint8_t sducontrol = 0; + int rc = 0; + + if (oph->sap != SAP_NS) { + rc = -EINVAL; + goto out; + } + + nsp = container_of(oph, struct osmo_gprs_ns2_prim, oph); + + if (oph->operation != PRIM_OP_REQUEST || oph->primitive != GPRS_NS2_PRIM_UNIT_DATA) { + rc = -EINVAL; + goto out; + } + + if (!oph->msg) { + rc = -EINVAL; + goto out; + } + + bvci = nsp->bvci; + nsei = nsp->nsei; + + nse = gprs_ns2_nse_by_nsei(nsi, nsei); + if (!nse) { + rc = -EINVAL; + goto out; + } + + if (!nse->alive) { + goto out; + } + + nsvc = ns2_load_sharing(nse, bvci, nsp->u.unitdata.link_selector); + + /* TODO: send a status primitive back */ + if (!nsvc) + goto out; + + if (nsp->u.unitdata.change == GPRS_NS2_ENDPOINT_REQUEST_CHANGE) + sducontrol = 1; + else if (nsp->u.unitdata.change == GPRS_NS2_ENDPOINT_CONFIRM_CHANGE) + sducontrol = 2; + + return ns2_tx_unit_data(nsvc, bvci, sducontrol, oph->msg); + +out: + msgb_free(oph->msg); + return rc; +} + +/*! Send a STATUS.ind primitive to the specified NS instance user. + * \param[in] nsi NS instance on which we operate + * \param[in] nsei NSEI to which the statue relates + * \param[in] bvci BVCI to which the status relates + * \param[in] cause The cause of the status */ +void ns2_prim_status_ind(struct gprs_ns2_nse *nse, + struct gprs_ns2_vc *nsvc, + uint16_t bvci, + enum gprs_ns2_affecting_cause cause) +{ + char nsvc_str[NS2_LL_MAX_STR]; + struct osmo_gprs_ns2_prim nsp = {}; + nsp.nsei = nse->nsei; + nsp.bvci = bvci; + nsp.u.status.cause = cause; + nsp.u.status.transfer = ns2_count_transfer_cap(nse, bvci); + nsp.u.status.first = nse->first; + nsp.u.status.persistent = nse->persistent; + if (nse->mtu < 4) + nsp.u.status.mtu = 0; + else + nsp.u.status.mtu = nse->mtu - 4; /* 1 Byte NS PDU type, 1 Byte NS SDU control, 2 Byte BVCI */ + + if (nsvc) { + nsp.u.status.nsvc = gprs_ns2_ll_str_buf(nsvc_str, sizeof(nsvc_str), nsvc); + LOGNSVC(nsvc, LOGL_NOTICE, "NS-STATUS.ind(bvci=%05u): cause=%s, transfer=%d, first=%d, mtu=%d\n", + nsp.bvci, gprs_ns2_aff_cause_prim_str(nsp.u.status.cause), + nsp.u.status.transfer, nsp.u.status.first, nsp.u.status.mtu); + } else { + LOGNSE(nse, LOGL_NOTICE, "NS-STATUS.ind(bvci=%05u): cause=%s, transfer=%d, first=%d, mtu=%d\n", + nsp.bvci, gprs_ns2_aff_cause_prim_str(nsp.u.status.cause), + nsp.u.status.transfer, nsp.u.status.first, nsp.u.status.mtu); + } + + osmo_prim_init(&nsp.oph, SAP_NS, GPRS_NS2_PRIM_STATUS, PRIM_OP_INDICATION, NULL); + nse->nsi->cb(&nsp.oph, nse->nsi->cb_data); +} + +/*! Allocate a NS-VC within the given bind + NSE. + * \param[in] bind The 'bind' on which we operate + * \param[in] nse The NS Entity on which we operate + * \param[in] initiater - if this is an incoming remote (!initiater) or a local outgoing connection (initater) + * \param[in] id - human-readable identifier + * \return newly allocated NS-VC on success; NULL on error */ +struct gprs_ns2_vc *ns2_vc_alloc(struct gprs_ns2_vc_bind *bind, struct gprs_ns2_nse *nse, bool initiater, + enum gprs_ns2_vc_mode vc_mode, const char *id) +{ + /* Sanity check */ + OSMO_ASSERT(bind->ll == nse->ll); + + struct gprs_ns2_vc *nsvc = talloc_zero(bind, struct gprs_ns2_vc); + + if (!nsvc) + return NULL; + + nsvc->bind = bind; + nsvc->nse = nse; + nsvc->mode = vc_mode; + nsvc->sig_weight = 1; + nsvc->data_weight = 1; + + nsvc->ctrg = rate_ctr_group_alloc(nsvc, &nsvc_ctrg_desc, bind->nsi->nsvc_rate_ctr_idx); + if (!nsvc->ctrg) { + goto err; + } + nsvc->statg = osmo_stat_item_group_alloc(nsvc, &nsvc_statg_desc, bind->nsi->nsvc_rate_ctr_idx); + if (!nsvc->statg) + goto err_group; + if (!ns2_vc_fsm_alloc(nsvc, id, initiater)) + goto err_statg; + + bind->nsi->nsvc_rate_ctr_idx++; + + rate_ctr_group_set_name(nsvc->ctrg, id); + osmo_stat_item_group_set_name(nsvc->statg, id); + + llist_add_tail(&nsvc->list, &nse->nsvc); + llist_add_tail(&nsvc->blist, &bind->nsvc); + osmo_clock_gettime(CLOCK_MONOTONIC, &nsvc->ts_alive_change); + ns2_nse_update_mtu(nse); + + return nsvc; + +err_statg: + osmo_stat_item_group_free(nsvc->statg); +err_group: + rate_ctr_group_free(nsvc->ctrg); +err: + talloc_free(nsvc); + + return NULL; +} + +/*! Destroy/release given NS-VC. + * \param[in] nsvc NS-VC to destroy */ +void gprs_ns2_free_nsvc(struct gprs_ns2_vc *nsvc) +{ + if (!nsvc || nsvc->freed) + return; + nsvc->freed = true; + ns2_prim_status_ind(nsvc->nse, nsvc, 0, GPRS_NS2_AFF_CAUSE_VC_FAILURE); + + llist_del(&nsvc->list); + llist_del(&nsvc->blist); + + /* notify nse this nsvc is unavailable */ + ns2_nse_notify_unblocked(nsvc, false); + + /* check if sns is using this VC */ + ns2_sns_replace_nsvc(nsvc); + osmo_fsm_inst_term(nsvc->fi, OSMO_FSM_TERM_REQUEST, NULL); + + /* let the driver/bind clean up it's internal state */ + if (nsvc->priv && nsvc->bind->free_vc) + nsvc->bind->free_vc(nsvc); + + osmo_stat_item_group_free(nsvc->statg); + rate_ctr_group_free(nsvc->ctrg); + + talloc_free(nsvc); +} + +void ns2_free_nsvcs(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc; + + /* prevent recursive free() when the user reacts on a down event and free() a second time */ + while (!llist_empty(&nse->nsvc)) { + nsvc = llist_first_entry(&nse->nsvc, struct gprs_ns2_vc, list); + gprs_ns2_free_nsvc(nsvc); + } +} + +/*! Destroy/release all NS-VC of given NSE + * \param[in] nse NSE + */ +void gprs_ns2_free_nsvcs(struct gprs_ns2_nse *nse) +{ + if (!nse || nse->freed) + return; + + if (nse->bss_sns_fi) { + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_FREE_NSVCS, NULL); + } else { + ns2_free_nsvcs(nse); + } +} + +/*! Allocate a message buffer for use with the NS2 stack. */ +struct msgb *ns2_msgb_alloc(void) +{ + struct msgb *msg = msgb_alloc_headroom(NS_ALLOC_SIZE, NS_ALLOC_HEADROOM, + "GPRS/NS"); + if (!msg) { + LOGP(DLNS, LOGL_ERROR, "Failed to allocate NS message of size %d\n", + NS_ALLOC_SIZE); + } + return msg; +} + +/*! Create a status message to be sent over a new connection. + * \param[in] orig_msg the original message + * \param[in] tp TLVP parsed of the original message + * \param[out] reject callee-allocated message buffer of the generated NS-STATUS + * \param[in] cause Cause for the rejection + * \return 0 on success */ +static int reject_status_msg(struct msgb *orig_msg, struct tlv_parsed *tp, struct msgb **reject, enum ns_cause cause) +{ + struct msgb *msg = ns2_msgb_alloc(); + struct gprs_ns_hdr *nsh; + bool have_vci = false; + uint8_t _cause = cause; + uint16_t nsei = 0; + + if (!msg) + return -ENOMEM; + + if (TLVP_PRES_LEN(tp, NS_IE_NSEI, 2)) { + nsei = tlvp_val16be(tp, NS_IE_NSEI); + + LOGP(DLNS, LOGL_NOTICE, "NSEI=%u Rejecting message without NSVCI. Tx NS STATUS (cause=%s)\n", + nsei, gprs_ns2_cause_str(cause)); + } + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = NS_PDUT_STATUS; + + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, &_cause); + have_vci = TLVP_PRES_LEN(tp, NS_IE_VCI, 2); + + /* Section 9.2.7.1: Static conditions for NS-VCI */ + if (cause == NS_CAUSE_NSVC_BLOCKED || + cause == NS_CAUSE_NSVC_UNKNOWN) { + if (!have_vci) { + msgb_free(msg); + return -EINVAL; + } + + msgb_tvlv_put(msg, NS_IE_VCI, 2, TLVP_VAL(tp, NS_IE_VCI)); + } + + /* Section 9.2.7.2: Static conditions for NS PDU */ + switch (cause) { + case NS_CAUSE_SEM_INCORR_PDU: + case NS_CAUSE_PDU_INCOMP_PSTATE: + case NS_CAUSE_PROTO_ERR_UNSPEC: + case NS_CAUSE_INVAL_ESSENT_IE: + case NS_CAUSE_MISSING_ESSENT_IE: + msgb_tvlv_put(msg, NS_IE_PDU, msgb_l2len(orig_msg), + orig_msg->l2h); + break; + default: + break; + } + + *reject = msg; + return 0; +} + +/*! Resolve a NS Entity based on its NSEI. + * \param[in] nsi NS Instance in which we do the look-up + * \param[in] nsei NSEI to look up + * \return NS Entity in successful case; NULL if none found */ +struct gprs_ns2_nse *gprs_ns2_nse_by_nsei(struct gprs_ns2_inst *nsi, uint16_t nsei) +{ + struct gprs_ns2_nse *nse; + + llist_for_each_entry(nse, &nsi->nse, list) { + if (nse->nsei == nsei) + return nse; + } + + return NULL; +} + +/*! Resolve a NS-VC Entity based on its NS-VCI. + * \param[in] nsi NS Instance in which we do the look-up + * \param[in] nsvci NS-VCI to look up + * \return NS-VC Entity in successful case; NULL if none found */ +struct gprs_ns2_vc *gprs_ns2_nsvc_by_nsvci(struct gprs_ns2_inst *nsi, uint16_t nsvci) +{ + struct gprs_ns2_nse *nse; + struct gprs_ns2_vc *nsvc; + + llist_for_each_entry(nse, &nsi->nse, list) { + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (nsvc->nsvci_is_valid && nsvc->nsvci == nsvci) + return nsvc; + } + } + + return NULL; +} + +/*! Create a NS Entity within given NS instance. + * \param[in] nsi NS instance in which to create NS Entity + * \param[in] nsei NS Entity Identifier of to-be-created NSE + * \param[in] ip_sns_role_sgsn Does local side implement SGSN role? + * \returns newly-allocated NS-E in successful case; NULL on error */ +struct gprs_ns2_nse *gprs_ns2_create_nse2(struct gprs_ns2_inst *nsi, uint16_t nsei, + enum gprs_ns2_ll linklayer, enum gprs_ns2_dialect dialect, + bool ip_sns_role_sgsn) +{ + struct gprs_ns2_nse *nse; + + nse = gprs_ns2_nse_by_nsei(nsi, nsei); + if (nse) { + LOGNSE(nse, LOGL_ERROR, "Can not create a NSE with already taken NSEI\n"); + return nse; + } + + nse = talloc_zero(nsi, struct gprs_ns2_nse); + if (!nse) + return NULL; + nse->dialect = GPRS_NS2_DIALECT_UNDEF; + nse->ip_sns_role_sgsn = ip_sns_role_sgsn; + + if (ns2_nse_set_dialect(nse, dialect) < 0) { + talloc_free(nse); + return NULL; + } + + nse->ctrg = rate_ctr_group_alloc(nse, &nse_ctrg_desc, nsei); + if (!nse->ctrg) { + talloc_free(nse); + return NULL; + } + + nse->ll = linklayer; + nse->nsei = nsei; + nse->nsi = nsi; + nse->first = true; + nse->mtu = 0; + llist_add_tail(&nse->list, &nsi->nse); + INIT_LLIST_HEAD(&nse->nsvc); + osmo_clock_gettime(CLOCK_MONOTONIC, &nse->ts_alive_change); + + return nse; +} + +int ns2_nse_set_dialect(struct gprs_ns2_nse *nse, enum gprs_ns2_dialect dialect) +{ + char sns[16]; + + if (nse->dialect == dialect) + return 0; + + switch (nse->dialect) { + case GPRS_NS2_DIALECT_UNDEF: + if (dialect == GPRS_NS2_DIALECT_SNS) { + snprintf(sns, sizeof(sns), "NSE%05u-SNS", nse->nsei); + if (nse->ip_sns_role_sgsn) + nse->bss_sns_fi = ns2_sns_sgsn_fsm_alloc(nse, sns); + else + nse->bss_sns_fi = ns2_sns_bss_fsm_alloc(nse, sns); + if (!nse->bss_sns_fi) + return -1; + } + nse->dialect = dialect; + break; + default: + if (dialect == GPRS_NS2_DIALECT_UNDEF) { + if (nse->bss_sns_fi) + osmo_fsm_inst_term(nse->bss_sns_fi, OSMO_FSM_TERM_REQUEST, NULL); + nse->bss_sns_fi = NULL; + nse->dialect = GPRS_NS2_DIALECT_UNDEF; + } else { + /* we don't support arbitrary changes without going through UNDEF first */ + return -EPERM; + } + } + + return 0; +} + +/*! Create a NS Entity within given NS instance. + * \param[in] nsi NS instance in which to create NS Entity + * \param[in] nsei NS Entity Identifier of to-be-created NSE + * \returns newly-allocated NS-E in successful case; NULL on error */ +struct gprs_ns2_nse *gprs_ns2_create_nse(struct gprs_ns2_inst *nsi, uint16_t nsei, + enum gprs_ns2_ll linklayer, enum gprs_ns2_dialect dialect) +{ + return gprs_ns2_create_nse2(nsi, nsei, linklayer, dialect, false); +} + +/*! Return the NSEI + * \param[in] nse NS Entity + * \return the nsei. + */ +uint16_t gprs_ns2_nse_nsei(struct gprs_ns2_nse *nse) +{ + return nse->nsei; +} + +/*! Destroy given NS Entity. + * \param[in] nse NS Entity to destroy */ +void gprs_ns2_free_nse(struct gprs_ns2_nse *nse) +{ + if (!nse || nse->freed) + return; + + nse->freed = true; + nse->alive = false; + if (nse->bss_sns_fi) { + osmo_fsm_inst_term(nse->bss_sns_fi, OSMO_FSM_TERM_REQUEST, NULL); + nse->bss_sns_fi = NULL; + } + + gprs_ns2_free_nsvcs(nse); + ns2_prim_status_ind(nse, NULL, 0, GPRS_NS2_AFF_CAUSE_FAILURE); + rate_ctr_group_free(nse->ctrg); + ns2_free_nsvcs(nse); + + llist_del(&nse->list); + talloc_free(nse); +} + +void gprs_ns2_free_nses(struct gprs_ns2_inst *nsi) +{ + struct gprs_ns2_nse *nse; + + /* prevent recursive free() when the user reacts on a down event and free() a second time */ + while (!llist_empty(&nsi->nse)) { + nse = llist_first_entry(&nsi->nse, struct gprs_ns2_nse, list); + gprs_ns2_free_nse(nse); + } +} + +static inline int ns2_tlv_parse(struct tlv_parsed *dec, + const uint8_t *buf, int buf_len, uint8_t lv_tag, + uint8_t lv_tag2) +{ + /* workaround for NS_IE_IP_ADDR not following any known TLV rules. + * See comment of ns_att_tlvdef1. */ + int rc = tlv_parse(dec, &ns_att_tlvdef1, buf, buf_len, lv_tag, lv_tag2); + if (rc < 0) + return tlv_parse(dec, &ns_att_tlvdef2, buf, buf_len, lv_tag, lv_tag2); + return rc; +} + +static enum ns2_cs ns2_create_vc_sns(struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *remote, + struct gprs_ns2_vc **success, uint16_t nsei) +{ + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse; + + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, remote); + /* ns2_create_vc() is only called if no NS-VC could be found */ + OSMO_ASSERT(!nsvc); + + nse = gprs_ns2_nse_by_nsei(bind->nsi, nsei); + if (!nse) { + if (!bind->accept_sns) { + struct osmo_sockaddr_str remote_str; + osmo_sockaddr_str_from_sockaddr(&remote_str, &remote->u.sas); + /* no dynamic creation of IP-SNS NSE permitted */ + LOGP(DLNS, LOGL_ERROR, "[%s]:%u: Dynamic creation of NSE(%05u) via IP-SNS not " + "permitted. Check your config.\n", remote_str.ip, remote_str.port, nsei); + return NS2_CS_ERROR; + } + nse = gprs_ns2_create_nse2(bind->nsi, nsei, bind->ll, GPRS_NS2_DIALECT_SNS, true); + if (!nse) { + LOGP(DLNS, LOGL_ERROR, "Failed to create NSE(%05u)\n", nsei); + return NS2_CS_ERROR; + } + /* add configured list of default binds; if that fails, use only current bind */ + if (!ns2_sns_add_sns_default_binds(nse)) + gprs_ns2_sns_add_bind(nse, bind); + } else { + /* nsei already known */ + if (nse->ll != bind->ll) { + LOGNSE(nse, LOGL_ERROR, "Received NS-RESET with wrong linklayer(%s)" + " for already known NSE(%s)\n", gprs_ns2_lltype_str(bind->ll), + gprs_ns2_lltype_str(nse->ll)); + return NS2_CS_SKIPPED; + } + } + + nsvc = ns2_ip_bind_connect(bind, nse, remote); + if (!nsvc) + return NS2_CS_SKIPPED; + + nsvc->nsvci_is_valid = false; + + *success = nsvc; + + return NS2_CS_CREATED; +} + +/*! Create a new NS-VC based on a [received] message. Depending on the bind it might create a NSE. + * \param[in] bind the bind through which msg was received + * \param[in] msg the actual received message + * \param[in] remote address of remote peer sending message + * \param[in] logname A name to describe the VC. E.g. ip address pair + * \param[out] reject A message filled to be sent back. Only used in failure cases. + * \param[out] success A pointer which will be set to the new VC on success + * \return enum value indicating the status, e.g. GPRS_NS2_CS_CREATED */ +enum ns2_cs ns2_create_vc(struct gprs_ns2_vc_bind *bind, + struct msgb *msg, + const struct osmo_sockaddr *remote, + const char *logname, + struct msgb **reject, + struct gprs_ns2_vc **success) +{ + struct gprs_ns_hdr *nsh = (struct gprs_ns_hdr *)msg->l2h; + struct tlv_parsed tp; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse; + enum gprs_ns2_dialect dialect; + enum gprs_ns2_vc_mode vc_mode; + uint16_t nsvci; + uint16_t nsei; + const struct osmo_sockaddr *local; + char idbuf[256], tmp[INET6_ADDRSTRLEN + 8]; + + int rc, tlv; + + if (msg->len < sizeof(struct gprs_ns_hdr)) + return NS2_CS_ERROR; + + /* parse the tlv early to allow reject status msg to + * work with valid tp. + * Ignore the return code until the pdu type is parsed because + * an unknown pdu type should be ignored */ + tlv = ns2_tlv_parse(&tp, nsh->data, + msgb_l2len(msg) - sizeof(*nsh), 0, 0); + + if (bind->ll == GPRS_NS2_LL_UDP && nsh->pdu_type == SNS_PDUT_SIZE && tlv >= 0) { + uint16_t nsei; + + if (!TLVP_PRES_LEN(&tp, NS_IE_NSEI, 2)) { + rc = reject_status_msg(msg, &tp, reject, NS_CAUSE_MISSING_ESSENT_IE); + if (rc < 0) + LOGP(DLNS, LOGL_ERROR, "Failed to generate reject message (%d)\n", rc); + return NS2_CS_REJECTED; + } + nsei = tlvp_val16be(&tp, NS_IE_NSEI); + /* Create NS-VC, and if required, even NSE dynamically */ + return ns2_create_vc_sns(bind, remote, success, nsei); + } + + switch (nsh->pdu_type) { + case NS_PDUT_STATUS: + /* Do not respond, see 3GPP TS 08.16, 7.5.1 */ + LOGP(DLNS, LOGL_INFO, "Ignoring NS STATUS from %s " + "for non-existing NS-VC\n", + logname); + return NS2_CS_SKIPPED; + case NS_PDUT_ALIVE_ACK: + /* Ignore this, see 3GPP TS 08.16, 7.4.1 */ + LOGP(DLNS, LOGL_INFO, "Ignoring NS ALIVE ACK from %s " + "for non-existing NS-VC\n", + logname); + return NS2_CS_SKIPPED; + case NS_PDUT_RESET_ACK: + /* Ignore this, see 3GPP TS 08.16, 7.3.1 */ + LOGP(DLNS, LOGL_INFO, "Ignoring NS RESET ACK from %s " + "for non-existing NS-VC\n", + logname); + return NS2_CS_SKIPPED; + case NS_PDUT_RESET: + /* accept PDU RESET when vc_mode matches */ + if (bind->accept_ipaccess) { + dialect = GPRS_NS2_DIALECT_IPACCESS; + break; + } + + rc = reject_status_msg(msg, &tp, reject, NS_CAUSE_PDU_INCOMP_PSTATE); + if (rc < 0) + LOGP(DLNS, LOGL_ERROR, "Failed to generate reject message (%d)\n", rc); + return NS2_CS_REJECTED; + default: + rc = reject_status_msg(msg, &tp, reject, NS_CAUSE_PDU_INCOMP_PSTATE); + if (rc < 0) + LOGP(DLNS, LOGL_ERROR, "Failed to generate reject message (%d)\n", rc); + return NS2_CS_REJECTED; + } + + if (tlv < 0) { + /* TODO: correct behaviour would checking what's wrong. + * If it's an essential TLV for the PDU return NS_CAUSE_INVAL_ESSENT_IE. + * Otherwise ignore the non-essential TLV. */ + LOGP(DLNS, LOGL_ERROR, "Rx NS RESET Error %d during " + "TLV Parse\n", tlv); + rc = reject_status_msg(msg, &tp, reject, NS_CAUSE_PROTO_ERR_UNSPEC); + if (rc < 0) + LOGP(DLNS, LOGL_ERROR, "Failed to generate reject message (%d)\n", rc); + return NS2_CS_REJECTED; + } + + if (!TLVP_PRES_LEN(&tp, NS_IE_CAUSE, 1) || + !TLVP_PRES_LEN(&tp, NS_IE_VCI, 2) || !TLVP_PRES_LEN(&tp, NS_IE_NSEI, 2)) { + LOGP(DLNS, LOGL_ERROR, "NS RESET Missing mandatory IE\n"); + rc = reject_status_msg(msg, &tp, reject, NS_CAUSE_MISSING_ESSENT_IE); + if (rc < 0) + LOGP(DLNS, LOGL_ERROR, "Failed to generate reject message (%d)\n", rc); + return NS2_CS_REJECTED; + } + + nsei = tlvp_val16be(&tp, NS_IE_NSEI); + nsvci = tlvp_val16be(&tp, NS_IE_VCI); + + /* find or create NSE */ + nse = gprs_ns2_nse_by_nsei(bind->nsi, nsei); + if (!nse) { + /* only create nse for udp & ipaccess */ + if (bind->ll != GPRS_NS2_LL_UDP || dialect != GPRS_NS2_DIALECT_IPACCESS) + return NS2_CS_SKIPPED; + + if (!bind->accept_ipaccess) + return NS2_CS_SKIPPED; + + nse = gprs_ns2_create_nse(bind->nsi, nsei, bind->ll, dialect); + if (!nse) { + LOGP(DLNS, LOGL_ERROR, "Failed to create NSE(%05u)\n", nsei); + return NS2_CS_ERROR; + } + } else { + /* nsei already known */ + if (nse->ll != bind->ll) { + LOGNSE(nse, LOGL_ERROR, "Received NS-RESET NS-VCI(%05u) with wrong linklayer(%s)" + " for already known NSE(%s)\n", nsvci, gprs_ns2_lltype_str(bind->ll), + gprs_ns2_lltype_str(nse->ll)); + return NS2_CS_SKIPPED; + } + } + + nsvc = gprs_ns2_nsvc_by_nsvci(bind->nsi, nsvci); + if (nsvc) { + if (nsvc->persistent) { + LOGNSVC(nsvc, LOGL_ERROR, "Received NS-RESET for a persistent NSE over wrong connection.\n"); + return NS2_CS_SKIPPED; + } + /* destroy old dynamic nsvc */ + gprs_ns2_free_nsvc(nsvc); + } + + /* do nse persistent check late to be more precise on the error message */ + if (nse->persistent) { + LOGNSE(nse, LOGL_ERROR, "Received NS-RESET for a persistent NSE but the unknown " + "NS-VCI(%05u)\n", nsvci); + return NS2_CS_SKIPPED; + } + + nsvci = tlvp_val16be(&tp, NS_IE_VCI); + vc_mode = ns2_dialect_to_vc_mode(dialect); + + local = gprs_ns2_ip_bind_sockaddr(bind); + osmo_sockaddr_to_str_buf(tmp, sizeof(tmp), local); + snprintf(idbuf, sizeof(idbuf), "%s-NSE%05u-NSVC%05u-%s-%s", gprs_ns2_lltype_str(nse->ll), + nse->nsei, nsvci, tmp, osmo_sockaddr_to_str(remote)); + osmo_identifier_sanitize_buf(idbuf, NULL, '_'); + nsvc = ns2_vc_alloc(bind, nse, false, vc_mode, idbuf); + if (!nsvc) + return NS2_CS_SKIPPED; + + nsvc->nsvci = nsvci; + nsvc->nsvci_is_valid = true; + + *success = nsvc; + + return NS2_CS_CREATED; +} + +/*! Create, and connect an inactive, new IP-based NS-VC + * \param[in] bind bind in which the new NS-VC is to be created + * \param[in] remote remote address to which to connect + * \param[in] nse NS Entity in which the NS-VC is to be created + * \param[in] nsvci is only required when bind->vc_mode == NS2_VC_MODE_BLOCKRESET + * \return pointer to newly-allocated, connected and inactive NS-VC; NULL on error */ +struct gprs_ns2_vc *gprs_ns2_ip_connect_inactive(struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *remote, + struct gprs_ns2_nse *nse, + uint16_t nsvci) +{ + struct gprs_ns2_vc *nsvc; + + nsvc = ns2_ip_bind_connect(bind, nse, remote); + if (!nsvc) + return NULL; + + if (nsvc->mode == GPRS_NS2_VC_MODE_BLOCKRESET) { + nsvc->nsvci = nsvci; + nsvc->nsvci_is_valid = true; + } + + return nsvc; +} + +/*! Create, connect and activate a new IP-based NS-VC + * \param[in] bind bind in which the new NS-VC is to be created + * \param[in] remote remote address to which to connect + * \param[in] nse NS Entity in which the NS-VC is to be created + * \param[in] nsvci is only required when bind->vc_mode == NS2_VC_MODE_BLOCKRESET + * \return pointer to newly-allocated, connected and activated NS-VC; NULL on error */ +struct gprs_ns2_vc *gprs_ns2_ip_connect(struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *remote, + struct gprs_ns2_nse *nse, + uint16_t nsvci) +{ + struct gprs_ns2_vc *nsvc; + nsvc = gprs_ns2_ip_connect_inactive(bind, remote, nse, nsvci); + if (!nsvc) + return NULL; + + ns2_vc_fsm_start(nsvc); + + return nsvc; +} + +/*! Create, connect and activate a new IP-based NS-VC + * \param[in] bind bind in which the new NS-VC is to be created + * \param[in] remote remote address to which to connect + * \param[in] nsei NSEI of the NS Entity in which the NS-VC is to be created + * \param[in] nsvci is only required when bind->vc_mode == NS2_VC_MODE_BLOCKRESET + * \return pointer to newly-allocated, connected and activated NS-VC; NULL on error */ +struct gprs_ns2_vc *gprs_ns2_ip_connect2(struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *remote, + uint16_t nsei, + uint16_t nsvci, + enum gprs_ns2_dialect dialect) +{ + struct gprs_ns2_nse *nse = gprs_ns2_nse_by_nsei(bind->nsi, nsei); + + if (!nse) { + nse = gprs_ns2_create_nse(bind->nsi, nsei, GPRS_NS2_LL_UDP, dialect); + if (!nse) + return NULL; + } + + return gprs_ns2_ip_connect(bind, remote, nse, nsvci); +} + +/*! Find NS-VC for given socket address. + * \param[in] nse NS Entity in which to search + * \param[in] sockaddr socket address to search for + * \return NS-VC matching sockaddr; NULL if none found */ +struct gprs_ns2_vc *gprs_ns2_nsvc_by_sockaddr_nse(struct gprs_ns2_nse *nse, + const struct osmo_sockaddr *sockaddr) +{ + struct gprs_ns2_vc *nsvc; + const struct osmo_sockaddr *remote; + + OSMO_ASSERT(nse); + OSMO_ASSERT(sockaddr); + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + remote = gprs_ns2_ip_vc_remote(nsvc); + if (!osmo_sockaddr_cmp(sockaddr, remote)) + return nsvc; + } + + return NULL; +} + +/*! + * Iterate over all nsvc of a NS Entity and call the callback. + * If the callback returns < 0 it aborts the loop and returns the callback return code. + * \param[in] nse NS Entity to iterate over all nsvcs + * \param[in] cb the callback to call + * \param[inout] cb_data the private data of the callback + * \return 0 if the loop completes. If a callback returns < 0 it will returns this value. + */ +int gprs_ns2_nse_foreach_nsvc(struct gprs_ns2_nse *nse, gprs_ns2_foreach_nsvc_cb cb, void *cb_data) +{ + struct gprs_ns2_vc *nsvc, *tmp; + int rc = 0; + llist_for_each_entry_safe(nsvc, tmp, &nse->nsvc, list) { + rc = cb(nsvc, cb_data); + if (rc < 0) + return rc; + } + + return 0; +} + + + +/*! Bottom-side entry-point for received NS PDU from the driver/bind + * \param[in] nsvc NS-VC for which the message was received + * \param msg the received message. Ownership is transferred, caller must not free it! + * \return 0 on success; negative on error */ +int ns2_recv_vc(struct gprs_ns2_vc *nsvc, + struct msgb *msg) +{ + struct gprs_ns_hdr *nsh = (struct gprs_ns_hdr *) msg->l2h; + struct tlv_parsed tp = { }; + int rc = 0; + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_IN); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_IN, msg->len); + + if (msg->len < sizeof(struct gprs_ns_hdr)) { + rc = -EINVAL; + goto freemsg; + } + + if (nsh->pdu_type != NS_PDUT_UNITDATA) + LOG_NS_RX_SIGNAL(nsvc, nsh->pdu_type); + else + LOG_NS_DATA(nsvc, "Rx", nsh->pdu_type, LOGL_INFO, "\n"); + + switch (nsh->pdu_type) { + case SNS_PDUT_CONFIG: + /* one additional byte ('end flag') before the TLV part starts */ + rc = ns2_tlv_parse(&tp, nsh->data+1, + msgb_l2len(msg) - sizeof(*nsh)-1, 0, 0); + if (rc < 0) { + LOGP(DLNS, LOGL_NOTICE, "Error during TLV Parse in %s\n", msgb_hexdump(msg)); + goto freemsg; + } + /* All sub-network service related message types */ + return ns2_sns_rx(nsvc, msg, &tp); + case SNS_PDUT_ACK: + case SNS_PDUT_ADD: + case SNS_PDUT_CHANGE_WEIGHT: + case SNS_PDUT_DELETE: + /* weird layout: NSEI TLV, then value-only transaction IE, then TLV again */ + rc = ns2_tlv_parse(&tp, nsh->data+5, + msgb_l2len(msg) - sizeof(*nsh)-5, 0, 0); + if (rc < 0) { + LOGP(DLNS, LOGL_NOTICE, "Error during TLV Parse in %s\n", msgb_hexdump(msg)); + goto freemsg; + } + tp.lv[NS_IE_NSEI].val = nsh->data+2; + tp.lv[NS_IE_NSEI].len = 2; + tp.lv[NS_IE_TRANS_ID].val = nsh->data+4; + tp.lv[NS_IE_TRANS_ID].len = 1; + return ns2_sns_rx(nsvc, msg, &tp); + case SNS_PDUT_CONFIG_ACK: + case SNS_PDUT_SIZE: + case SNS_PDUT_SIZE_ACK: + rc = ns2_tlv_parse(&tp, nsh->data, + msgb_l2len(msg) - sizeof(*nsh), 0, 0); + if (rc < 0) { + LOGP(DLNS, LOGL_NOTICE, "Error during TLV Parse in %s\n", msgb_hexdump(msg)); + goto freemsg; + } + /* All sub-network service related message types */ + return ns2_sns_rx(nsvc, msg, &tp); + case NS_PDUT_UNITDATA: + return ns2_vc_rx(nsvc, msg, &tp); + default: + rc = ns2_tlv_parse(&tp, nsh->data, + msgb_l2len(msg) - sizeof(*nsh), 0, 0); + if (rc < 0) { + LOGP(DLNS, LOGL_NOTICE, "Error during TLV Parse\n"); + if (nsh->pdu_type != NS_PDUT_STATUS) + ns2_tx_status(nsvc, NS_CAUSE_PROTO_ERR_UNSPEC, 0, msg, NULL); + return rc; + } + return ns2_vc_rx(nsvc, msg, &tp); + } +freemsg: + msgb_free(msg); + + return rc; +} + +/* summarize all active data nsvcs */ +void ns2_nse_data_sum(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc; + + nse->nsvc_count = 0; + nse->sum_data_weight = 0; + nse->sum_sig_weight = 0; + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (!ns2_vc_is_unblocked(nsvc)) + continue; + + nse->nsvc_count++; + nse->sum_data_weight += nsvc->data_weight; + nse->sum_sig_weight += nsvc->sig_weight; + } +} + +/*! Notify a nse about the change of a NS-VC. + * \param[in] nsvc NS-VC which has detected the change (and shall not be notified). + * \param[in] unblocked whether the NSE should be marked as unblocked (true) or blocked (false) */ +void ns2_nse_notify_unblocked(struct gprs_ns2_vc *nsvc, bool unblocked) +{ + struct gprs_ns2_nse *nse = nsvc->nse; + struct gprs_ns2_inst *nsi = nse->nsi; + uint16_t nsei = nse->nsei; + + ns2_nse_data_sum(nse); + ns2_sns_notify_alive(nse, nsvc, unblocked); + + /* NSE could have been freed, try to get it again */ + nse = gprs_ns2_nse_by_nsei(nsi, nsei); + + if (!nse || unblocked == nse->alive) + return; + + /* wait until both data_weight and sig_weight are != 0 before declaring NSE as alive */ + if (unblocked && nse->sum_data_weight && nse->sum_sig_weight) { + nse->alive = true; + osmo_clock_gettime(CLOCK_MONOTONIC, &nse->ts_alive_change); + ns2_prim_status_ind(nse, NULL, 0, GPRS_NS2_AFF_CAUSE_RECOVERY); + nse->first = false; + return; + } + + if (nse->alive && (nse->sum_data_weight == 0 || nse->sum_sig_weight == 0)) { + /* nse became unavailable */ + nse->alive = false; + osmo_clock_gettime(CLOCK_MONOTONIC, &nse->ts_alive_change); + ns2_prim_status_ind(nse, NULL, 0, GPRS_NS2_AFF_CAUSE_FAILURE); + } +} + +/*! Create a new GPRS NS instance + * \param[in] ctx a talloc context to allocate NS instance from + * \param[in] cb Call-back function for dispatching primitives to the user. The Call-back must free all msgb* given in the primitive. + * \param[in] cb_data transparent user data passed to Call-back + * \returns dynamically allocated gprs_ns_inst; NULL on error */ +struct gprs_ns2_inst *gprs_ns2_instantiate(void *ctx, osmo_prim_cb cb, void *cb_data) +{ + struct gprs_ns2_inst *nsi; + + nsi = talloc_zero(ctx, struct gprs_ns2_inst); + if (!nsi) + return NULL; + + nsi->cb = cb; + nsi->cb_data = cb_data; + INIT_LLIST_HEAD(&nsi->binding); + INIT_LLIST_HEAD(&nsi->nse); + + nsi->timeout[NS_TOUT_TNS_BLOCK] = 3; + nsi->timeout[NS_TOUT_TNS_BLOCK_RETRIES] = 3; + nsi->timeout[NS_TOUT_TNS_RESET] = 3; + nsi->timeout[NS_TOUT_TNS_RESET_RETRIES] = 3; + nsi->timeout[NS_TOUT_TNS_TEST] = 30; + nsi->timeout[NS_TOUT_TNS_ALIVE] = 3; + nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES] = 10; + nsi->timeout[NS_TOUT_TSNS_PROV] = 3; /* 1..10 */ + nsi->timeout[NS_TOUT_TSNS_SIZE_RETRIES] = 3; + nsi->timeout[NS_TOUT_TSNS_CONFIG_RETRIES] = 3; + nsi->timeout[NS_TOUT_TSNS_PROCEDURES_RETRIES] = 3; + + nsi->txqueue_max_length = NS_DEFAULT_TXQUEUE_MAX_LENGTH; + + return nsi; +} + +/*! Destroy a NS Instance (including all its NSEs, binds, ...). + * \param[in] nsi NS instance to destroy */ +void gprs_ns2_free(struct gprs_ns2_inst *nsi) +{ + if (!nsi) + return; + + gprs_ns2_free_nses(nsi); + gprs_ns2_free_binds(nsi); + + talloc_free(nsi); +} + +/*! Start the NS-ALIVE FSM in all NS-VCs of given NSE. + * \param[in] nse NS Entity in whihc to start NS-ALIVE FSMs */ +void gprs_ns2_start_alive_all_nsvcs(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc; + OSMO_ASSERT(nse); + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + /* A pre-configured endpoint shall not be used for NSE data or signalling traffic + * (with the exception of Size and Configuration procedures) unless it is + * configured by the SGSN using the auto-configuration procedures */ + if (nsvc->sns_only) + continue; + + ns2_vc_fsm_start(nsvc); + } +} + +/*! Destroy a given bind. + * \param[in] bind the bind we want to destroy */ +void gprs_ns2_free_bind(struct gprs_ns2_vc_bind *bind) +{ + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse; + if (!bind || bind->freed) + return; + bind->freed = true; + + if (gprs_ns2_is_ip_bind(bind)) { + llist_for_each_entry(nse, &bind->nsi->nse, list) { + gprs_ns2_sns_del_bind(nse, bind); + } + } + + /* prevent recursive free() when the user reacts on a down event and free() a second time */ + while (!llist_empty(&bind->nsvc)) { + nsvc = llist_first_entry(&bind->nsvc, struct gprs_ns2_vc, blist); + gprs_ns2_free_nsvc(nsvc); + } + + if (bind->driver->free_bind) + bind->driver->free_bind(bind); + + llist_del(&bind->list); + osmo_stat_item_group_free(bind->statg); + talloc_free((char *)bind->name); + talloc_free(bind); +} + +void gprs_ns2_free_binds(struct gprs_ns2_inst *nsi) +{ + struct gprs_ns2_vc_bind *bind; + + /* prevent recursive free() when the user reacts on a down event and free() a second time */ + while (!llist_empty(&nsi->binding)) { + bind = llist_first_entry(&nsi->binding, struct gprs_ns2_vc_bind, list); + gprs_ns2_free_bind(bind); + } +} + +/*! Search for a bind with a unique name + * \param[in] nsi NS instance on which we operate + * \param[in] name The unique bind name to search for + * \return the bind or NULL if not found + */ +struct gprs_ns2_vc_bind *gprs_ns2_bind_by_name(struct gprs_ns2_inst *nsi, const char *name) +{ + struct gprs_ns2_vc_bind *bind; + + llist_for_each_entry(bind, &nsi->binding, list) { + if (!strcmp(bind->name, name)) + return bind; + } + + return NULL; +} + +enum gprs_ns2_vc_mode ns2_dialect_to_vc_mode(enum gprs_ns2_dialect dialect) +{ + switch (dialect) { + case GPRS_NS2_DIALECT_SNS: + case GPRS_NS2_DIALECT_STATIC_ALIVE: + return GPRS_NS2_VC_MODE_ALIVE; + case GPRS_NS2_DIALECT_STATIC_RESETBLOCK: + case GPRS_NS2_DIALECT_IPACCESS: + return GPRS_NS2_VC_MODE_BLOCKRESET; + default: + return -1; + } +} + +static void add_bind_array(struct gprs_ns2_vc_bind **array, + struct gprs_ns2_vc_bind *bind, int size) +{ + int i; + for (i=0; i < size; i++) { + if (array[i] == bind) + return; + if (!array[i]) + break; + } + + if (i == size) + return; + + array[i] = bind; +} + +void ns2_nse_update_mtu(struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc; + int mtu = 0; + + if (llist_empty(&nse->nsvc)) { + nse->mtu = 0; + return; + } + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (mtu == 0) + mtu = nsvc->bind->mtu; + else if (mtu > nsvc->bind->mtu) + mtu = nsvc->bind->mtu; + } + + if (nse->mtu == mtu) + return; + + nse->mtu = mtu; + if (nse->alive) + ns2_prim_status_ind(nse, NULL, 0, GPRS_NS2_AFF_CAUSE_MTU_CHANGE); +} + +/*! calculate the transfer capabilities for a nse + * \param nse the nse to count the transfer capability + * \param bvci a bvci - unused + * \return the transfer capability in mbit. On error < 0. + */ +int ns2_count_transfer_cap(struct gprs_ns2_nse *nse, + uint16_t bvci) +{ + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_vc_bind **active_binds; + int i, active_nsvcs = 0, transfer_cap = 0; + + /* calculate the transfer capabilities based on the binds. + * A bind has a transfer capability which is shared across all NSVCs. + * Take care the bind cap is not counted twice within a NSE. + * This should be accurate for FR and UDP but not for FR/GRE. */ + + if (!nse->alive) + return 0; + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (ns2_vc_is_unblocked(nsvc)) + active_nsvcs++; + } + + if (!active_nsvcs) + return 0; + + active_binds = talloc_zero_array(nse, struct gprs_ns2_vc_bind*, active_nsvcs); + if (!active_binds) + return -ENOMEM; + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (!ns2_vc_is_unblocked(nsvc)) + continue; + add_bind_array(active_binds, nsvc->bind, active_nsvcs); + } + + /* TODO: change calcuation for FR/GRE */ + for (i = 0; i < active_nsvcs; i++) { + if (active_binds[i]) + transfer_cap += active_binds[i]->transfer_capability; + } + + talloc_free(active_binds); + return transfer_cap; +} + +/*! common allocation + low-level initialization of a bind. Called by vc-drivers */ +int ns2_bind_alloc(struct gprs_ns2_inst *nsi, const char *name, + struct gprs_ns2_vc_bind **result) +{ + struct gprs_ns2_vc_bind *bind; + + if (!name) + return -EINVAL; + + if (gprs_ns2_bind_by_name(nsi, name)) + return -EALREADY; + + bind = talloc_zero(nsi, struct gprs_ns2_vc_bind); + if (!bind) + return -ENOMEM; + + bind->name = talloc_strdup(bind, name); + if (!bind->name) { + talloc_free(bind); + return -ENOMEM; + } + + bind->statg = osmo_stat_item_group_alloc(bind, &nsbind_statg_desc, nsi->bind_rate_ctr_idx); + if (!bind->statg) { + talloc_free(bind); + return -ENOMEM; + } + + bind->sns_sig_weight = 1; + bind->sns_data_weight = 1; + bind->nsi = nsi; + INIT_LLIST_HEAD(&bind->nsvc); + llist_add_tail(&bind->list, &nsi->binding); + + nsi->bind_rate_ctr_idx++; + + if (result) + *result = bind; + + return 0; +} + +/*! @} */ diff --git a/src/gb/gprs_ns2_fr.c b/src/gb/gprs_ns2_fr.c new file mode 100644 index 00000000..f6bee39c --- /dev/null +++ b/src/gb/gprs_ns2_fr.c @@ -0,0 +1,988 @@ +/*! \file gprs_ns2_fr.c + * NS-over-FR-over-GRE implementation. + * GPRS Networks Service (NS) messages on the Gb interface. + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2009-2021 by Harald Welte <laforge@gnumonks.org> + * (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> +#include <string.h> +#include <unistd.h> + +#include <sys/socket.h> +#include <netinet/in.h> +#include <netinet/ip.h> +#include <netinet/ip6.h> +#include <arpa/inet.h> +#include <linux/if.h> + +#include <sys/ioctl.h> +#include <netpacket/packet.h> +#include <linux/if_ether.h> +#include <linux/hdlc.h> +#include <linux/hdlc/ioctl.h> +#include <linux/sockios.h> + +#include <osmocom/gprs/frame_relay.h> +#include <osmocom/core/byteswap.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/talloc.h> +#include <osmocom/gprs/gprs_ns2.h> +#include <osmocom/core/netdev.h> +#include <osmocom/gprs/protocol/gsm_08_16.h> +#include <osmocom/gprs/protocol/gsm_08_18.h> + +#include "config.h" +#include "common_vty.h" +#include "gprs_ns2_internal.h" + +#define GRE_PTYPE_FR 0x6559 +#define GRE_PTYPE_IPv4 0x0800 +#define GRE_PTYPE_IPv6 0x86dd +#define GRE_PTYPE_KAR 0x0000 /* keepalive response */ + +#ifndef IPPROTO_GRE +# define IPPROTO_GRE 47 +#endif + +#define E1_LINERATE 2048000 +#define E1_SLOTS_TOTAL 32 +#define E1_SLOTS_USED 31 +/* usable bitrate of the E1 superchannel with 31 of 32 timeslots */ +#define SUPERCHANNEL_LINERATE (E1_LINERATE*E1_SLOTS_USED)/E1_SLOTS_TOTAL +/* nanoseconds per bit (504) */ +#define BIT_DURATION_NS (1000000000 / SUPERCHANNEL_LINERATE) + +static void free_bind(struct gprs_ns2_vc_bind *bind); +static int fr_dlci_rx_cb(void *cb_data, struct msgb *msg); + +struct gprs_ns2_vc_driver vc_driver_fr = { + .name = "GB frame relay", + .free_bind = free_bind, +}; + +struct priv_bind { + struct osmo_netdev *netdev; + char netif[IFNAMSIZ]; + struct osmo_fr_link *link; + int ifindex; + bool if_running; + /* backlog queue for AF_PACKET / ENOBUFS handling (see OS#4995) */ + struct { + /* file-descriptor for AF_PACKET socket */ + struct osmo_fd ofd; + /* LMI bucket (we only store the last LMI message, no need to queue */ + struct msgb *lmi_msg; + /* list of NS msgb (backlog) */ + struct llist_head list; + /* timer to trigger next attempt of AF_PACKET write */ + struct osmo_timer_list timer; + /* re-try after that many micro-seconds */ + uint32_t retry_us; + } backlog; +}; + +struct priv_vc { + struct osmo_sockaddr remote; + uint16_t dlci; + struct osmo_fr_dlc *dlc; +}; + +static void free_vc(struct gprs_ns2_vc *nsvc) +{ + if (!nsvc) + return; + + if (!nsvc->priv) + return; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(nsvc->bind)); + talloc_free(nsvc->priv); + nsvc->priv = NULL; +} + +static void dump_vty(const struct gprs_ns2_vc_bind *bind, struct vty *vty, bool stats) +{ + struct priv_bind *priv; + struct gprs_ns2_vc *nsvc; + struct osmo_fr_link *fr_link; + + if (!bind) + return; + + priv = bind->priv; + fr_link = priv->link; + + vty_out(vty, "FR bind: %s, role: %s, link: %s%s", priv->netif, + osmo_fr_role_str(fr_link->role), priv->if_running ? "UP" : "DOWN", VTY_NEWLINE); + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + ns2_vty_dump_nsvc(vty, nsvc, stats); + } + + priv = bind->priv; +} + +/*! clean up all private driver state. Should be only called by gprs_ns2_free_bind() */ +static void free_bind(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + struct msgb *msg, *msg2; + + if (!bind) + return; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + priv = bind->priv; + + OSMO_ASSERT(llist_empty(&bind->nsvc)); + + osmo_timer_del(&priv->backlog.timer); + llist_for_each_entry_safe(msg, msg2, &priv->backlog.list, list) { + msgb_free(msg); + } + msgb_free(priv->backlog.lmi_msg); + + osmo_netdev_free(priv->netdev); + osmo_fr_link_free(priv->link); + osmo_fd_close(&priv->backlog.ofd); + talloc_free(priv); +} + +static void fr_dlci_status_cb(struct osmo_fr_dlc *dlc, void *cb_data, bool active) +{ + struct gprs_ns2_vc *nsvc = cb_data; + + if (active) { + ns2_vc_fsm_start(nsvc); + } else { + ns2_vc_force_unconfigured(nsvc); + } +} + +static struct priv_vc *fr_alloc_vc(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_vc *nsvc, + uint16_t dlci) +{ + struct priv_bind *privb = bind->priv; + struct priv_vc *priv = talloc_zero(bind, struct priv_vc); + if (!priv) + return NULL; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + nsvc->priv = priv; + priv->dlci = dlci; + priv->dlc = osmo_fr_dlc_alloc(privb->link, dlci); + if (!priv->dlc) { + nsvc->priv = NULL; + talloc_free(priv); + return NULL; + } + + priv->dlc->cb_data = nsvc; + priv->dlc->rx_cb = fr_dlci_rx_cb; + priv->dlc->status_cb = fr_dlci_status_cb; + + return priv; +} + +int gprs_ns2_find_vc_by_dlci(struct gprs_ns2_vc_bind *bind, + uint16_t dlci, + struct gprs_ns2_vc **result) +{ + struct gprs_ns2_vc *nsvc; + struct priv_vc *vcpriv; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + if (!result) + return -EINVAL; + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + vcpriv = nsvc->priv; + if (vcpriv->dlci != dlci) { + *result = nsvc; + return 0; + } + } + + return 1; +} + +/* PDU from the network interface towards the fr layer (upwards) */ +static int fr_netif_ofd_cb(struct osmo_fd *bfd, uint32_t what) +{ + struct gprs_ns2_vc_bind *bind = bfd->data; + struct priv_bind *priv = bind->priv; + struct msgb *msg; + struct sockaddr_ll sll; + socklen_t sll_len = sizeof(sll); + int rc = 0; + + /* we only handle read here. write to AF_PACKET sockets cannot be triggered + * by select or poll, see OS#4995 */ + if (!(what & OSMO_FD_READ)) + return 0; + + msg = msgb_alloc(NS_ALLOC_SIZE, "Gb/NS/FR Rx"); + if (!msg) + return -ENOMEM; + + rc = recvfrom(bfd->fd, msg->data, NS_ALLOC_SIZE, 0, (struct sockaddr *)&sll, &sll_len); + if (rc < 0) { + LOGBIND(bind, LOGL_ERROR, "recv error %s during NS-FR recv\n", strerror(errno)); + goto out_err; + } else if (rc == 0) { + goto out_err; + } + + /* ignore any packets that we might have received for a different interface, between + * the socket() and the bind() call */ + if (sll.sll_ifindex != priv->ifindex) + goto out_err; + + msgb_put(msg, rc); + msg->dst = priv->link; + return osmo_fr_rx(msg); + +out_err: + msgb_free(msg); + return rc; +} + +/* PDU from the frame relay towards the NS-VC (upwards) */ +static int fr_dlci_rx_cb(void *cb_data, struct msgb *msg) +{ + int rc; + struct gprs_ns2_vc *nsvc = cb_data; + + rc = ns2_recv_vc(nsvc, msg); + + return rc; +} + +static int fr_netif_write_one(struct gprs_ns2_vc_bind *bind, struct msgb *msg) +{ + struct priv_bind *priv = bind->priv; + unsigned int len = msgb_length(msg); + int rc; + + /* estimate the retry time based on the data rate it takes to transmit */ + priv->backlog.retry_us = (BIT_DURATION_NS * 8 * len) / 1000; + + rc = write(priv->backlog.ofd.fd, msgb_data(msg), len); + if (rc == len) { + msgb_free(msg); + return 0; + } else if (rc < 0) { + /* don't free, the caller might want to re-transmit */ + switch (errno) { + case EAGAIN: + case ENOBUFS: + /* not a real error, but more a normal event on AF_PACKET */ + /* don't free the message and let the caller re-enqueue */ + return -errno; + default: + /* an actual error, like -ENETDOWN, -EMSGSIZE */ + LOGBIND(bind, LOGL_ERROR, "error during write to AF_PACKET: %s\n", strerror(errno)); + msgb_free(msg); + return 0; + } + } else { + /* short write */ + LOGBIND(bind, LOGL_ERROR, "short write on AF_PACKET: %d < %d\n", rc, len); + msgb_free(msg); + return 0; + } +} + +/*! determine if given bind is for FR-GRE encapsulation. */ +int gprs_ns2_is_fr_bind(struct gprs_ns2_vc_bind *bind) +{ + return (bind->driver == &vc_driver_fr); +} + +/* PDU from the NS-VC towards the frame relay layer (downwards) */ +static int fr_vc_sendmsg(struct gprs_ns2_vc *nsvc, struct msgb *msg) +{ + struct priv_vc *vcpriv = nsvc->priv; + unsigned int vc_len = msgb_length(msg); + int rc; + + msg->dst = vcpriv->dlc; + rc = osmo_fr_tx_dlc(msg); + if (OSMO_LIKELY(rc >= 0)) { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT, vc_len); + } else { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT_DROP); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT_DROP, vc_len); + } + return rc; +} + +static void enqueue_at_head(struct gprs_ns2_vc_bind *bind, struct msgb *msg) +{ + struct priv_bind *priv = bind->priv; + llist_add(&msg->list, &priv->backlog.list); + osmo_stat_item_inc(osmo_stat_item_group_get_item(bind->statg, NS2_BIND_STAT_BACKLOG_LEN), 1); + osmo_timer_schedule(&priv->backlog.timer, 0, priv->backlog.retry_us); +} + +static void enqueue_at_tail(struct gprs_ns2_vc_bind *bind, struct msgb *msg) +{ + struct priv_bind *priv = bind->priv; + llist_add_tail(&msg->list, &priv->backlog.list); + osmo_stat_item_inc(osmo_stat_item_group_get_item(bind->statg, NS2_BIND_STAT_BACKLOG_LEN), 1); + osmo_timer_schedule(&priv->backlog.timer, 0, priv->backlog.retry_us); +} + +#define LMI_Q933A_DLCI 0 + +/* enqueue to backlog (LMI, signaling) or drop (userdata msg) */ +static int backlog_enqueue_or_free(struct gprs_ns2_vc_bind *bind, struct msgb *msg) +{ + struct priv_bind *priv = bind->priv; + uint8_t dlci = msg->data[0]; + uint8_t ns_pdu_type; + uint16_t bvci; + + if (msgb_length(msg) < 1) + goto out_free; + + /* we want to enqueue only Q.933 LMI traffic or NS signaling; NOT user traffic */ + switch (dlci) { + case LMI_Q933A_DLCI: + /* always store only the last LMI message in the lmi_msg bucket */ + msgb_free(priv->backlog.lmi_msg); + priv->backlog.lmi_msg = msg; + return 0; + default: + /* there's no point in trying to enqueue messages if the interface is down */ + if (!priv->if_running) + break; + + if (msgb_length(msg) < 3) + break; + ns_pdu_type = msg->data[2]; + switch (ns_pdu_type) { + case NS_PDUT_UNITDATA: + if (msgb_length(msg) < 6) + break; + bvci = osmo_load16be(msg->data + 4); + /* enqueue BVCI=0 traffic at tail of queue */ + if (bvci == BVCI_SIGNALLING) { + enqueue_at_tail(bind, msg); + return 0; + } + break; + default: + /* enqueue NS signaling traffic at head of queue */ + enqueue_at_head(bind, msg); + return 0; + } + break; + } + +out_free: + /* drop everything that is not LMI, NS-signaling or BVCI-0 */ + msgb_free(msg); + return -1; +} + +static void fr_backlog_timer_cb(void *data) +{ + struct gprs_ns2_vc_bind *bind = data; + struct priv_bind *priv = bind->priv; + int i, rc; + + /* first try to get rid of the LMI message, if any */ + if (priv->backlog.lmi_msg) { + rc = fr_netif_write_one(bind, priv->backlog.lmi_msg); + if (rc < 0) + goto restart_timer; + /* fr_netif_write_one() has just free'd it */ + priv->backlog.lmi_msg = NULL; + } + + /* attempt to send up to 10 messages in every timer */ + for (i = 0; i < 10; i++) { + struct msgb *msg = msgb_dequeue(&priv->backlog.list); + if (!msg) + break; + + rc = fr_netif_write_one(bind, msg); + if (rc < 0) { + /* re-add at head of list */ + llist_add(&msg->list, &priv->backlog.list); + break; + } + osmo_stat_item_dec(osmo_stat_item_group_get_item(bind->statg, NS2_BIND_STAT_BACKLOG_LEN), 1); + } + +restart_timer: + /* re-start timer if we still have data in the queue */ + if (!llist_empty(&priv->backlog.list)) + osmo_timer_schedule(&priv->backlog.timer, 0, priv->backlog.retry_us); +} + +/* PDU from the frame relay layer towards the network interface (downwards) */ +int fr_tx_cb(void *data, struct msgb *msg) +{ + struct gprs_ns2_vc_bind *bind = data; + struct priv_bind *priv = bind->priv; + int rc; + + if (llist_empty(&priv->backlog.list)) { + /* attempt to transmit right now */ + rc = fr_netif_write_one(bind, msg); + if (rc < 0) { + /* enqueue to backlog in case it fails */ + return backlog_enqueue_or_free(bind, msg); + } + } else { + /* enqueue to backlog */ + return backlog_enqueue_or_free(bind, msg); + } + + return 0; +} + +static int devname2ifindex(const char *ifname) +{ + struct ifreq ifr; + int sk, rc; + + sk = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + if (sk < 0) + return sk; + + + memset(&ifr, 0, sizeof(ifr)); + OSMO_STRLCPY_ARRAY(ifr.ifr_name, ifname); + + rc = ioctl(sk, SIOCGIFINDEX, &ifr); + close(sk); + if (rc < 0) + return rc; + + return ifr.ifr_ifindex; +} + +static int open_socket(int ifindex, const struct gprs_ns2_vc_bind *nsbind) +{ + struct sockaddr_ll addr; + int fd, rc; + + memset(&addr, 0, sizeof(addr)); + addr.sll_family = AF_PACKET; + addr.sll_protocol = htons(ETH_P_ALL); + addr.sll_ifindex = ifindex; + + fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_HDLC)); + if (fd < 0) { + LOGBIND(nsbind, LOGL_ERROR, "Can not create AF_PACKET socket. Are you root or have CAP_NET_RAW?\n"); + return fd; + } + + /* there's a race condition between the above syscall and the bind() call below, + * causing other packets to be received in between */ + + rc = bind(fd, (struct sockaddr *)&addr, sizeof(addr)); + if (rc < 0) { + LOGBIND(nsbind, LOGL_ERROR, "Can not bind AF_PACKET socket to ifindex %d\n", ifindex); + close(fd); + return rc; + } + + return fd; +} + +static int gprs_n2_fr_ifupdown_ind_cb(struct osmo_netdev *netdev, bool if_running) +{ + struct gprs_ns2_vc_bind *bind = osmo_netdev_get_priv_data(netdev); + struct priv_bind *bpriv = bind->priv; + struct msgb *msg, *msg2; + + if (bpriv->if_running == if_running) + return 0; + + LOGBIND(bind, LOGL_NOTICE, "FR net-device '%s': Physical link state changed: %s\n", + bpriv->netif, if_running ? "UP" : "DOWN"); + + /* free any backlog, both on IFUP and IFDOWN. Keep the LMI, as it makes + * sense to get one out of the door ASAP. */ + llist_for_each_entry_safe(msg, msg2, &bpriv->backlog.list, list) { + msgb_free(msg); + } + + if (if_running) { + /* interface just came up */ + if (bpriv->backlog.lmi_msg) + osmo_timer_schedule(&bpriv->backlog.timer, 0, bpriv->backlog.retry_us); + } else { + /* interface just went down; no need to retransmit */ + osmo_timer_del(&bpriv->backlog.timer); + } + + bpriv->if_running = if_running; + return 0; +} + +static int gprs_n2_fr_mtu_chg_cb(struct osmo_netdev *netdev, uint32_t new_mtu) +{ + struct gprs_ns2_vc_bind *bind = osmo_netdev_get_priv_data(netdev); + struct priv_bind *bpriv = bind->priv; + struct gprs_ns2_nse *nse; + + /* 2 byte DLCI header */ + if (new_mtu <= 2) + return 0; + new_mtu -= 2; + + if (new_mtu == bind->mtu) + return 0; + + LOGBIND(bind, LOGL_INFO, "MTU changed from %d to %d.\n", + bind->mtu + 2, new_mtu + 2); + + bind->mtu = new_mtu; + if (!bpriv->if_running) + return 0; + + llist_for_each_entry(nse, &bind->nsi->nse, list) { + ns2_nse_update_mtu(nse); + } + return 0; +} + +static int set_ifupdown(const char *netif, bool up) +{ + int sock, rc; + struct ifreq req; + + sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock < 0) + return sock; + + memset(&req, 0, sizeof req); + OSMO_STRLCPY_ARRAY(req.ifr_name, netif); + + rc = ioctl(sock, SIOCGIFFLAGS, &req); + if (rc < 0) { + close(sock); + return rc; + } + + if ((req.ifr_flags & IFF_UP) == up) { + close(sock); + return 0; + } + + if (up) + req.ifr_flags |= IFF_UP; + + rc = ioctl(sock, SIOCSIFFLAGS, &req); + close(sock); + return rc; +} + +static int setup_device(const char *netif, const struct gprs_ns2_vc_bind *bind) +{ + int sock, rc; + char buffer[128]; + fr_proto *fr = (void*)buffer; + struct ifreq req; + + sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP); + if (sock < 0) { + LOGBIND(bind, LOGL_ERROR, "%s: Unable to create socket: %s\n", + netif, strerror(errno)); + return sock; + } + + memset(&req, 0, sizeof(struct ifreq)); + memset(&buffer, 0, sizeof(buffer)); + OSMO_STRLCPY_ARRAY(req.ifr_name, netif); + req.ifr_settings.ifs_ifsu.sync = (void*)buffer; + req.ifr_settings.size = sizeof(buffer); + req.ifr_settings.type = IF_GET_PROTO; + + /* EINVAL is returned when no protocol has been set */ + rc = ioctl(sock, SIOCWANDEV, &req); + if (rc < 0 && errno != EINVAL) { + LOGBIND(bind, LOGL_ERROR, "%s: Unable to get FR protocol information: %s\n", + netif, strerror(errno)); + goto err; + } + + /* check if the device is good */ + if (rc == 0 && req.ifr_settings.type == IF_PROTO_FR && fr->lmi == LMI_NONE) { + LOGBIND(bind, LOGL_NOTICE, "%s: has correct frame relay mode and lmi\n", netif); + goto ifup; + } + + /* modify the device to match */ + rc = set_ifupdown(netif, false); + if (rc) { + LOGBIND(bind, LOGL_ERROR, "Unable to bring down the device %s: %s\n", + netif, strerror(errno)); + goto err; + } + + memset(&req, 0, sizeof(struct ifreq)); + memset(fr, 0, sizeof(fr_proto)); + OSMO_STRLCPY_ARRAY(req.ifr_name, netif); + req.ifr_settings.type = IF_PROTO_FR; + req.ifr_settings.size = sizeof(fr_proto); + req.ifr_settings.ifs_ifsu.fr = fr; + fr->lmi = LMI_NONE; + /* even those settings aren't used, they must be in the range */ + /* polling verification timer*/ + fr->t391 = 10; + /* link integrity verification polling timer */ + fr->t392 = 15; + /* full status polling counter*/ + fr->n391 = 6; + /* error threshold */ + fr->n392 = 3; + /* monitored events count */ + fr->n393 = 4; + + LOGBIND(bind, LOGL_INFO, "%s: Setting frame relay related parameters\n", netif); + rc = ioctl(sock, SIOCWANDEV, &req); + if (rc) { + LOGBIND(bind, LOGL_ERROR, "%s: Unable to set FR protocol on information: %s\n", + netif, strerror(errno)); + goto err; + } + +ifup: + rc = set_ifupdown(netif, true); + if (rc) + LOGBIND(bind, LOGL_ERROR, "Unable to bring up the device %s: %s\n", + netif, strerror(errno)); +err: + close(sock); + return rc; +} + +/*! Create a new bind for NS over FR. + * \param[in] nsi NS instance in which to create the bind + * \param[in] netif Network interface to bind to + * \param[in] fr_network + * \param[in] fr_role + * \param[out] result pointer to the created bind or if a bind with the name exists return the bind. + * \return 0 on success; negative on error. -EALREADY returned in case a bind with the name exists */ +int gprs_ns2_fr_bind(struct gprs_ns2_inst *nsi, + const char *name, + const char *netif, + struct osmo_fr_network *fr_network, + enum osmo_fr_role fr_role, + struct gprs_ns2_vc_bind **result) +{ + struct gprs_ns2_vc_bind *bind; + struct priv_bind *priv; + struct osmo_fr_link *fr_link; + int rc = 0; + + if (strlen(netif) > IFNAMSIZ) + return -EINVAL; + + bind = gprs_ns2_bind_by_name(nsi, name); + if (bind) { + if (result) + *result = bind; + return -EALREADY; + } + + rc = ns2_bind_alloc(nsi, name, &bind); + if (rc < 0) + return rc; + + bind->driver = &vc_driver_fr; + bind->ll = GPRS_NS2_LL_FR; + /* 2 mbit */ + bind->transfer_capability = 2; + bind->send_vc = fr_vc_sendmsg; + bind->free_vc = free_vc; + bind->dump_vty = dump_vty; + bind->mtu = FRAME_RELAY_SDU; + priv = bind->priv = talloc_zero(bind, struct priv_bind); + if (!priv) { + rc = -ENOMEM; + goto err_bind; + } + + INIT_LLIST_HEAD(&priv->backlog.list); + OSMO_STRLCPY_ARRAY(priv->netif, netif); + + /* FIXME: move fd handling into socket.c */ + fr_link = osmo_fr_link_alloc(fr_network, fr_role, netif); + if (!fr_link) { + rc = -EINVAL; + goto err_bind; + } + + fr_link->tx_cb = fr_tx_cb; + fr_link->cb_data = bind; + priv->link = fr_link; + + priv->ifindex = rc = devname2ifindex(netif); + if (rc < 0) { + LOGBIND(bind, LOGL_ERROR, "Can not get interface index for interface %s\n", netif); + goto err_fr; + } + + priv->netdev = osmo_netdev_alloc(bind, name); + if (!priv->netdev) { + rc = -ENOENT; + goto err_fr; + } + osmo_netdev_set_priv_data(priv->netdev, bind); + osmo_netdev_set_ifupdown_ind_cb(priv->netdev, gprs_n2_fr_ifupdown_ind_cb); + osmo_netdev_set_mtu_chg_cb(priv->netdev, gprs_n2_fr_mtu_chg_cb); + rc = osmo_netdev_set_ifindex(priv->netdev, priv->ifindex); + if (rc < 0) + goto err_free_netdev; + rc = osmo_netdev_register(priv->netdev); + if (rc < 0) + goto err_free_netdev; + + /* set protocol frame relay and lmi */ + rc = setup_device(priv->netif, bind); + if(rc < 0) { + LOGBIND(bind, LOGL_ERROR, "Failed to setup the interface %s for frame relay and lmi\n", netif); + goto err_free_netdev; + } + + rc = open_socket(priv->ifindex, bind); + if (rc < 0) + goto err_free_netdev; + priv->backlog.retry_us = 2500; /* start with some non-zero value; this corrsponds to 496 bytes */ + osmo_timer_setup(&priv->backlog.timer, fr_backlog_timer_cb, bind); + osmo_fd_setup(&priv->backlog.ofd, rc, OSMO_FD_READ, fr_netif_ofd_cb, bind, 0); + rc = osmo_fd_register(&priv->backlog.ofd); + if (rc < 0) + goto err_fd; + + if (result) + *result = bind; + + return rc; + +err_fd: + close(priv->backlog.ofd.fd); +err_free_netdev: + osmo_netdev_free(priv->netdev); + priv->netdev = NULL; +err_fr: + osmo_fr_link_free(fr_link); + priv->link = NULL; +err_bind: + gprs_ns2_free_bind(bind); + + return rc; +} + +/*! Return the frame relay role of a bind + * \param[in] bind The bind + * \return the frame relay role or -EINVAL if bind is not frame relay + */ +enum osmo_fr_role gprs_ns2_fr_bind_role(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + + if (bind->driver != &vc_driver_fr) + return -EINVAL; + + priv = bind->priv; + return priv->link->role; +} + +/*! Return the network interface of the bind + * \param[in] bind The bind + * \return the network interface + */ +const char *gprs_ns2_fr_bind_netif(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + + if (bind->driver != &vc_driver_fr) + return NULL; + + priv = bind->priv; + return priv->netif; +} + +/*! Find NS bind for a given network interface + * \param[in] nsi NS instance + * \param[in] netif the network interface to search for + * \return the bind or NULL if not found + */ +struct gprs_ns2_vc_bind *gprs_ns2_fr_bind_by_netif( + struct gprs_ns2_inst *nsi, + const char *netif) +{ + struct gprs_ns2_vc_bind *bind; + const char *_netif; + + OSMO_ASSERT(nsi); + OSMO_ASSERT(netif); + + llist_for_each_entry(bind, &nsi->binding, list) { + if (!gprs_ns2_is_fr_bind(bind)) + continue; + + _netif = gprs_ns2_fr_bind_netif(bind); + if (!strncmp(_netif, netif, IFNAMSIZ)) + return bind; + } + + return NULL; +} + +/*! Create, connect and activate a new FR-based NS-VC + * \param[in] bind bind in which the new NS-VC is to be created + * \param[in] nsei NSEI of the NS Entity in which the NS-VC is to be created + * \param[in] dlci Data Link connection identifier + * \return pointer to newly-allocated, connected and activated NS-VC; NULL on error */ +struct gprs_ns2_vc *gprs_ns2_fr_connect(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_nse *nse, + uint16_t nsvci, + uint16_t dlci) +{ + struct gprs_ns2_vc *nsvc = NULL; + struct priv_vc *priv = NULL; + struct priv_bind *bpriv = bind->priv; + char idbuf[64]; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + nsvc = gprs_ns2_fr_nsvc_by_dlci(bind, dlci); + if (nsvc) { + goto err; + } + + snprintf(idbuf, sizeof(idbuf), "NSE%05u-NSVC%05u-%s-%s-DLCI%u", nse->nsei, nsvci, + gprs_ns2_lltype_str(nse->ll), bpriv->netif, dlci); + osmo_identifier_sanitize_buf(idbuf, NULL, '_'); + nsvc = ns2_vc_alloc(bind, nse, true, GPRS_NS2_VC_MODE_BLOCKRESET, idbuf); + if (!nsvc) + goto err; + + nsvc->priv = priv = fr_alloc_vc(bind, nsvc, dlci); + if (!priv) + goto err; + + nsvc->nsvci = nsvci; + nsvc->nsvci_is_valid = true; + + return nsvc; + +err: + gprs_ns2_free_nsvc(nsvc); + return NULL; +} + + +/*! Create, connect and activate a new FR-based NS-VC + * \param[in] bind bind in which the new NS-VC is to be created + * \param[in] nsei NSEI of the NS Entity in which the NS-VC is to be created + * \param[in] dlci Data Link connection identifier + * \return pointer to newly-allocated, connected and activated NS-VC; NULL on error */ +struct gprs_ns2_vc *gprs_ns2_fr_connect2(struct gprs_ns2_vc_bind *bind, + uint16_t nsei, + uint16_t nsvci, + uint16_t dlci) +{ + bool created_nse = false; + struct gprs_ns2_vc *nsvc = NULL; + struct gprs_ns2_nse *nse; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + nse = gprs_ns2_nse_by_nsei(bind->nsi, nsei); + if (!nse) { + nse = gprs_ns2_create_nse(bind->nsi, nsei, GPRS_NS2_LL_FR, GPRS_NS2_DIALECT_STATIC_RESETBLOCK); + if (!nse) + return NULL; + created_nse = true; + } + + nsvc = gprs_ns2_fr_connect(bind, nse, nsvci, dlci); + if (!nsvc) + goto err_nse; + + return nsvc; + +err_nse: + if (created_nse) + gprs_ns2_free_nse(nse); + + return NULL; +} + +/*! Return the nsvc by dlci. + * \param[in] bind + * \param[in] dlci Data Link connection identifier + * \return the nsvc or NULL if not found + */ +struct gprs_ns2_vc *gprs_ns2_fr_nsvc_by_dlci(struct gprs_ns2_vc_bind *bind, + uint16_t dlci) +{ + struct gprs_ns2_vc *nsvc; + struct priv_vc *vcpriv; + + OSMO_ASSERT(gprs_ns2_is_fr_bind(bind)); + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + vcpriv = nsvc->priv; + + if (dlci == vcpriv->dlci) + return nsvc; + } + + return NULL; +} + +/*! Return the dlci of the nsvc + * \param[in] nsvc + * \return the dlci or 0 on error. 0 is not a valid dlci. + */ +uint16_t gprs_ns2_fr_nsvc_dlci(const struct gprs_ns2_vc *nsvc) +{ + struct priv_vc *vcpriv; + + if (!nsvc->bind) + return 0; + + if (nsvc->bind->driver != &vc_driver_fr) + return 0; + + vcpriv = nsvc->priv; + return vcpriv->dlci; +} diff --git a/src/gb/gprs_ns2_frgre.c b/src/gb/gprs_ns2_frgre.c new file mode 100644 index 00000000..b99761eb --- /dev/null +++ b/src/gb/gprs_ns2_frgre.c @@ -0,0 +1,616 @@ +/*! \file gprs_ns2_frgre.c + * NS-over-FR-over-GRE implementation. + * GPRS Networks Service (NS) messages on the Gb interface. + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2009-2010,2014,2017 by Harald Welte <laforge@gnumonks.org> + * (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> +#include <string.h> +#include <unistd.h> + +#include <sys/socket.h> +#include <netinet/in.h> +#include <netinet/ip.h> +#include <netinet/ip6.h> +#include <arpa/inet.h> + +#include <osmocom/core/byteswap.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/talloc.h> +#include <osmocom/gprs/gprs_ns2.h> + +#include "gprs_ns2_internal.h" + +#define GRE_PTYPE_FR 0x6559 +#define GRE_PTYPE_IPv4 0x0800 +#define GRE_PTYPE_IPv6 0x86dd +#define GRE_PTYPE_KAR 0x0000 /* keepalive response */ + +#ifndef IPPROTO_GRE +# define IPPROTO_GRE 47 +#endif + +struct gre_hdr { + uint16_t flags; + uint16_t ptype; +} __attribute__ ((packed)); + +#if defined(__FreeBSD__) || defined(__APPLE__) || defined(__CYGWIN__) +/** + * On BSD the IPv4 struct is called struct ip and instead of iXX + * the members are called ip_XX. One could change this code to use + * struct ip but that would require to define _BSD_SOURCE and that + * might have other complications. Instead make sure struct iphdr + * is present on FreeBSD. The below is taken from GLIBC. + * + * The GNU C Library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + */ +struct iphdr + { +#if BYTE_ORDER == LITTLE_ENDIAN + unsigned int ihl:4; + unsigned int version:4; +#elif BYTE_ORDER == BIG_ENDIAN + unsigned int version:4; + unsigned int ihl:4; +#endif + u_int8_t tos; + u_int16_t tot_len; + u_int16_t id; + u_int16_t frag_off; + u_int8_t ttl; + u_int8_t protocol; + u_int16_t check; + u_int32_t saddr; + u_int32_t daddr; + /*The options start here. */ + }; +#endif + + +static void free_bind(struct gprs_ns2_vc_bind *bind); +static inline int frgre_sendmsg(struct gprs_ns2_vc_bind *bind, + struct msgb *msg, + struct osmo_sockaddr *dest); + +struct gprs_ns2_vc_driver vc_driver_frgre = { + .name = "GB frame relay over GRE", + .free_bind = free_bind, +}; + +struct priv_bind { + struct osmo_fd fd; + struct osmo_sockaddr addr; + uint16_t dlci; + int dscp; +}; + +struct priv_vc { + struct osmo_sockaddr remote; + uint16_t dlci; +}; + +static void free_vc(struct gprs_ns2_vc *nsvc) +{ + OSMO_ASSERT(nsvc); + + if (!nsvc->priv) + return; + + talloc_free(nsvc->priv); + nsvc->priv = NULL; +} + + +/*! clean up all private driver state. Should be only called by gprs_ns2_free_bind() */ +static void free_bind(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + + if (!bind) + return; + + priv = bind->priv; + + OSMO_ASSERT(llist_empty(&bind->nsvc)); + + osmo_fd_close(&priv->fd); + talloc_free(priv); +} + +static struct priv_vc *frgre_alloc_vc(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_vc *nsvc, + struct osmo_sockaddr *remote, + uint16_t dlci) +{ + struct priv_vc *priv = talloc_zero(bind, struct priv_vc); + if (!priv) + return NULL; + + nsvc->priv = priv; + priv->remote = *remote; + priv->dlci = dlci; + + return priv; +} + +static int handle_rx_gre_ipv6(struct osmo_fd *bfd, struct msgb *msg, + struct ip6_hdr *ip6hdr, struct gre_hdr *greh) +{ + /* RFC 7676 IPv6 Support for Generic Routing Encapsulation (GRE) */ + struct gprs_ns2_vc_bind *bind = bfd->data; + struct priv_bind *priv = bind->priv; + int gre_payload_len; + struct ip6_hdr *inner_ip6h; + struct gre_hdr *inner_greh; + struct sockaddr_in6 daddr; + struct in6_addr ia6; + + gre_payload_len = msg->len - (sizeof(*ip6hdr) + sizeof(*greh)); + + inner_ip6h = (struct ip6_hdr *) ((uint8_t *)greh + sizeof(*greh)); + + if (gre_payload_len < sizeof(*ip6hdr) + sizeof(*inner_greh)) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive too short\n"); + return -EIO; + } + + if (!memcmp(&inner_ip6h->ip6_src, &ip6hdr->ip6_src, sizeof(struct in6_addr)) || + !memcmp(&inner_ip6h->ip6_dst, &ip6hdr->ip6_dst, sizeof(struct in6_addr))) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive with wrong tunnel addresses\n"); + return -EIO; + } + + /* Are IPv6 extensions header are allowed in the *inner*? In the outer they are */ + if (inner_ip6h->ip6_ctlun.ip6_un1.ip6_un1_nxt != IPPROTO_GRE) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive with wrong protocol\n"); + return -EIO; + } + + inner_greh = (struct gre_hdr *) ((uint8_t *)inner_ip6h + sizeof(struct ip6_hdr)); + if (inner_greh->ptype != osmo_htons(GRE_PTYPE_KAR)) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive inner GRE type != 0\n"); + return -EIO; + } + + /* Actually send the response back */ + + daddr.sin6_family = AF_INET6; + daddr.sin6_addr = inner_ip6h->ip6_dst; + daddr.sin6_port = IPPROTO_GRE; + + ia6 = ip6hdr->ip6_src; + char ip6str[INET6_ADDRSTRLEN] = {}; + inet_ntop(AF_INET6, &ia6, ip6str, INET6_ADDRSTRLEN); + LOGBIND(bind, LOGL_DEBUG, "GRE keepalive from %s, responding\n", ip6str); + + /* why does it reduce the gre_payload_len by the ipv6 header? + * make it similiar to ipv4 even this seems to be wrong */ + return sendto(priv->fd.fd, inner_greh, + gre_payload_len - sizeof(*inner_ip6h), 0, + (struct sockaddr *)&daddr, sizeof(daddr)); +} + +/* IPv4 messages inside the GRE tunnel might be GRE keepalives */ +static int handle_rx_gre_ipv4(struct osmo_fd *bfd, struct msgb *msg, + struct iphdr *iph, struct gre_hdr *greh) +{ + struct gprs_ns2_vc_bind *bind = bfd->data; + struct priv_bind *priv = bind->priv; + int gre_payload_len; + struct iphdr *inner_iph; + struct gre_hdr *inner_greh; + struct sockaddr_in daddr; + struct in_addr ia; + + gre_payload_len = msg->len - (iph->ihl*4 + sizeof(*greh)); + + inner_iph = (struct iphdr *) ((uint8_t *)greh + sizeof(*greh)); + + if (gre_payload_len < inner_iph->ihl*4 + sizeof(*inner_greh)) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive too short\n"); + return -EIO; + } + + if (inner_iph->saddr != iph->daddr || + inner_iph->daddr != iph->saddr) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive with wrong tunnel addresses\n"); + return -EIO; + } + + if (inner_iph->protocol != IPPROTO_GRE) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive with wrong protocol\n"); + return -EIO; + } + + inner_greh = (struct gre_hdr *) ((uint8_t *)inner_iph + iph->ihl*4); + if (inner_greh->ptype != osmo_htons(GRE_PTYPE_KAR)) { + LOGBIND(bind, LOGL_ERROR, "GRE keepalive inner GRE type != 0\n"); + return -EIO; + } + + /* Actually send the response back */ + + daddr.sin_family = AF_INET; + daddr.sin_addr.s_addr = inner_iph->daddr; + daddr.sin_port = IPPROTO_GRE; + + ia.s_addr = iph->saddr; + LOGBIND(bind, LOGL_DEBUG, "GRE keepalive from %s, responding\n", inet_ntoa(ia)); + + /* why does it reduce the gre_payload_len by the ipv4 header? */ + return sendto(priv->fd.fd, inner_greh, + gre_payload_len - inner_iph->ihl*4, 0, + (struct sockaddr *)&daddr, sizeof(daddr)); +} + +static struct msgb *read_nsfrgre_msg(struct osmo_fd *bfd, int *error, + struct osmo_sockaddr *saddr, uint16_t *dlci, + const struct gprs_ns2_vc_bind *bind) +{ + struct msgb *msg = msgb_alloc(NS_ALLOC_SIZE, "Gb/NS/FR/GRE Rx"); + int ret = 0; + socklen_t saddr_len = sizeof(*saddr); + struct iphdr *iph = NULL; + struct ip6_hdr *ip6h = NULL; + size_t ip46hdr; + struct gre_hdr *greh; + uint8_t *frh; + + if (!msg) { + *error = -ENOMEM; + return NULL; + } + + ret = recvfrom(bfd->fd, msg->data, NS_ALLOC_SIZE, 0, + &saddr->u.sa, &saddr_len); + if (ret < 0) { + LOGBIND(bind, LOGL_ERROR, "recv error %s during NS-FR-GRE recv\n", strerror(errno)); + *error = ret; + goto out_err; + } else if (ret == 0) { + *error = ret; + goto out_err; + } + + msgb_put(msg, ret); + + /* we've received a raw packet including the IPv4 or IPv6 header */ + switch (saddr->u.sa.sa_family) { + case AF_INET: + ip46hdr = sizeof(struct iphdr); + break; + case AF_INET6: + ip46hdr = sizeof(struct ip6_hdr); + break; + default: + *error = -EIO; + goto out_err; + break; + } + + /* TODO: add support for the extension headers */ + if (msg->len < ip46hdr + sizeof(*greh) + 2) { + LOGBIND(bind, LOGL_ERROR, "Short IP packet: %u bytes\n", msg->len); + *error = -EIO; + goto out_err; + } + + switch (saddr->u.sa.sa_family) { + case AF_INET: + iph = (struct iphdr *) msg->data; + if (msg->len < (iph->ihl*4 + sizeof(*greh) + 2)) { + LOGBIND(bind, LOGL_ERROR, "Short IP packet: %u bytes\n", msg->len); + *error = -EIO; + goto out_err; + } + break; + case AF_INET6: + ip6h = (struct ip6_hdr *) msg->data; + break; + } + + if (iph) + greh = (struct gre_hdr *) (msg->data + iph->ihl*4); + else + greh = (struct gre_hdr *) (msg->data + sizeof(struct ip6_hdr)); + + if (greh->flags) { + LOGBIND(bind, LOGL_NOTICE, "Unknown GRE flags 0x%04x\n", osmo_ntohs(greh->flags)); + } + + switch (osmo_ntohs(greh->ptype)) { + case GRE_PTYPE_IPv4: + /* IPv4 messages might be GRE keepalives */ + if (iph) + *error = handle_rx_gre_ipv4(bfd, msg, iph, greh); + else + *error = -EIO; + goto out_err; + break; + case GRE_PTYPE_IPv6: + if (ip6h) + *error = handle_rx_gre_ipv6(bfd, msg, ip6h, greh); + else + *error = -EIO; + goto out_err; + break; + case GRE_PTYPE_FR: + /* continue as usual */ + break; + default: + LOGBIND(bind, LOGL_NOTICE, "Unknown GRE protocol 0x%04x != FR\n", osmo_ntohs(greh->ptype)); + *error = -EIO; + goto out_err; + break; + } + + if (msg->len < sizeof(*greh) + 2) { + LOGBIND(bind, LOGL_ERROR, "Short FR header: %u bytes\n", msg->len); + *error = -EIO; + goto out_err; + } + + frh = (uint8_t *)greh + sizeof(*greh); + if (frh[0] & 0x01) { + LOGBIND(bind, LOGL_NOTICE, "Unsupported single-byte FR address\n"); + *error = -EIO; + goto out_err; + } + *dlci = ((frh[0] & 0xfc) << 2); + if ((frh[1] & 0x0f) != 0x01) { + LOGBIND(bind, LOGL_NOTICE, "Unknown second FR octet 0x%02x\n", frh[1]); + *error = -EIO; + goto out_err; + } + *dlci |= (frh[1] >> 4); + + msg->l2h = frh+2; + + return msg; + +out_err: + msgb_free(msg); + return NULL; +} + +static int ns2_find_vc_by_dlci(struct gprs_ns2_vc_bind *bind, + uint16_t dlci, + struct gprs_ns2_vc **result) +{ + struct gprs_ns2_vc *nsvc; + struct priv_vc *vcpriv; + + if (!result) + return -EINVAL; + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + vcpriv = nsvc->priv; + if (vcpriv->dlci != dlci) { + *result = nsvc; + return 0; + } + } + + return 1; +} + +static int handle_nsfrgre_read(struct osmo_fd *bfd) +{ + int rc; + struct osmo_sockaddr saddr; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_vc_bind *bind = bfd->data; + struct msgb *msg; + struct msgb *reject; + uint16_t dlci; + + msg = read_nsfrgre_msg(bfd, &rc, &saddr, &dlci, bind); + if (!msg) + return rc; + + if (dlci == 0 || dlci == 1023) { + LOGBIND(bind, LOGL_INFO, "Received FR on LMI DLCI %u - ignoring\n", dlci); + rc = 0; + goto out; + } + + rc = ns2_find_vc_by_dlci(bind, dlci, &nsvc); + if (rc) { + /* VC not found */ + rc = ns2_create_vc(bind, msg, &saddr, "newconnection", &reject, &nsvc); + switch (rc) { + case NS2_CS_FOUND: + break; + case NS2_CS_ERROR: + case NS2_CS_SKIPPED: + rc = 0; + goto out; + case NS2_CS_REJECTED: + /* nsip_sendmsg will free reject */ + rc = frgre_sendmsg(bind, reject, &saddr); + goto out; + case NS2_CS_CREATED: + frgre_alloc_vc(bind, nsvc, &saddr, dlci); + ns2_vc_fsm_start(nsvc); + break; + } + } + + rc = ns2_recv_vc(nsvc, msg); +out: + msgb_free(msg); + + return rc; +} + +static int handle_nsfrgre_write(struct osmo_fd *bfd) +{ + /* FIXME: actually send the data here instead of nsip_sendmsg() */ + return -EIO; +} + +static inline int frgre_sendmsg(struct gprs_ns2_vc_bind *bind, + struct msgb *msg, + struct osmo_sockaddr *dest) +{ + int rc; + struct priv_bind *priv = bind->priv; + + rc = sendto(priv->fd.fd, msg->data, msg->len, 0, + &dest->u.sa, sizeof(*dest)); + + msgb_free(msg); + + return rc; +} + +static int frgre_vc_sendmsg(struct gprs_ns2_vc *nsvc, struct msgb *msg) +{ + struct gprs_ns2_vc_bind *bind = nsvc->bind; + struct priv_vc *vcpriv = nsvc->priv; + struct priv_bind *bindpriv = bind->priv; + + uint16_t dlci = osmo_htons(bindpriv->dlci); + uint8_t *frh; + struct gre_hdr *greh; + unsigned int vc_len = msgb_length(msg); + int rc; + + /* Prepend the FR header */ + frh = msgb_push(msg, 2); + frh[0] = (dlci >> 2) & 0xfc; + frh[1] = ((dlci & 0xf)<<4) | 0x01; + + /* Prepend the GRE header */ + greh = (struct gre_hdr *) msgb_push(msg, sizeof(*greh)); + greh->flags = 0; + greh->ptype = osmo_htons(GRE_PTYPE_FR); + + rc = frgre_sendmsg(bind, msg, &vcpriv->remote); + if (OSMO_LIKELY(rc >= 0)) { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT, rc); + } else { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT_DROP); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT_DROP, vc_len); + } + return rc; +} + +static int frgre_fd_cb(struct osmo_fd *bfd, unsigned int what) +{ + int rc = 0; + + if (what & OSMO_FD_READ) + rc = handle_nsfrgre_read(bfd); + if (what & OSMO_FD_WRITE) + rc = handle_nsfrgre_write(bfd); + + return rc; +} + +/*! determine if given bind is for FR-GRE encapsulation. */ +int gprs_ns2_is_frgre_bind(struct gprs_ns2_vc_bind *bind) +{ + return (bind->driver == &vc_driver_frgre); +} + +/*! Create a new bind for NS over FR-GRE. + * \param[in] nsi NS instance in which to create the bind + * \param[in] local local address on which to bind + * \param[in] dscp DSCP/TOS bits to use for transmitted data on this bind + * \param[out] result pointer to the created bind or if a bind with the name exists return the bind. + * \return 0 on success; negative on error. -EALREADY returned in case a bind with the name exists */ +int gprs_ns2_frgre_bind(struct gprs_ns2_inst *nsi, + const char *name, + const struct osmo_sockaddr *local, + int dscp, + struct gprs_ns2_vc_bind **result) +{ + struct gprs_ns2_vc_bind *bind; + struct priv_bind *priv; + int rc; + + if (local->u.sa.sa_family != AF_INET && local->u.sa.sa_family != AF_INET6) + return -EINVAL; + + if (dscp < 0 || dscp > 63) + return -EINVAL; + + bind = gprs_ns2_bind_by_name(nsi, name); + if (bind) { + if (result) + *result = bind; + return -EALREADY; + } + + rc = ns2_bind_alloc(nsi, name, &bind); + if (rc < 0) + return rc; + + bind->driver = &vc_driver_frgre; + bind->ll = GPRS_NS2_LL_FR_GRE; + /* 2 mbit transfer capability. Counting should be done different for this. */ + bind->transfer_capability = 2; + bind->send_vc = frgre_vc_sendmsg; + bind->free_vc = free_vc; + bind->nsi = nsi; + /* TODO: allow to set the MTU via vty. It can not be automatic detected because it goes over an + * ethernet device and the MTU here must match the FR side of the FR-to-GRE gateway. + */ + bind->mtu = FRAME_RELAY_SDU; + + priv = bind->priv = talloc_zero(bind, struct priv_bind); + if (!priv) { + gprs_ns2_free_bind(bind); + return -ENOMEM; + } + priv->fd.cb = frgre_fd_cb; + priv->fd.data = bind; + priv->addr = *local; + INIT_LLIST_HEAD(&bind->nsvc); + priv->dscp = dscp; + + rc = osmo_sock_init_osa_ofd(&priv->fd, SOCK_RAW, IPPROTO_GRE, + local, NULL, + OSMO_SOCK_F_BIND | OSMO_SOCK_F_DSCP(priv->dscp)); + if (rc < 0) { + gprs_ns2_free_bind(bind); + return rc; + } + + if (result) + *result = bind; + + return rc; +} diff --git a/src/gb/gprs_ns2_internal.h b/src/gb/gprs_ns2_internal.h new file mode 100644 index 00000000..2e7dac3f --- /dev/null +++ b/src/gb/gprs_ns2_internal.h @@ -0,0 +1,503 @@ +/*! \file gprs_ns2_internal.h */ + +#pragma once + +#include <stdbool.h> +#include <stdint.h> + +#include <osmocom/core/logging.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/gprs/protocol/gsm_08_16.h> +#include <osmocom/gprs/gprs_ns2.h> + +#define LOGNSE(nse, lvl, fmt, args ...) \ + LOGP(DLNS, lvl, "NSE(%05u) " fmt, (nse)->nsei, ## args) + +#define LOGBIND(bind, lvl, fmt, args ...) \ + LOGP(DLNS, lvl, "BIND(%s) " fmt, (bind)->name, ## args) + +#define LOGNSVC_SS(ss, nsvc, lvl, fmt, args ...) \ + do { \ + if ((nsvc)->nsvci_is_valid) { \ + LOGP(ss, lvl, "NSE(%05u)-NSVC(%05u) " fmt, \ + (nsvc)->nse->nsei, (nsvc)->nsvci, ## args); \ + } else { \ + LOGP(ss, lvl, "NSE(%05u)-NSVC(none) " fmt, \ + (nsvc)->nse->nsei, ## args); \ + } \ + } while (0) + +#define LOGNSVC(nsvc, lvl, fmt, args ...) \ + LOGNSVC_SS(DLNS, nsvc, lvl, fmt, ## args) + +#define LOG_NS_SIGNAL(nsvc, direction, pdu_type, lvl, fmt, args ...) \ + LOGNSVC_SS(DLNSSIGNAL, nsvc, lvl, "%s %s" fmt, direction, get_value_string(gprs_ns_pdu_strings, pdu_type), ## args) + +#define LOG_NS_DATA(nsvc, direction, pdu_type, lvl, fmt, args ...) \ + LOGNSVC_SS(DLNSDATA, nsvc, lvl, "%s %s" fmt, direction, get_value_string(gprs_ns_pdu_strings, pdu_type), ## args) + +#define LOG_NS_RX_SIGNAL(nsvc, pdu_type) LOG_NS_SIGNAL(nsvc, "Rx", pdu_type, LOGL_INFO, "\n") +#define LOG_NS_TX_SIGNAL(nsvc, pdu_type) LOG_NS_SIGNAL(nsvc, "Tx", pdu_type, LOGL_INFO, "\n") + +#define RATE_CTR_INC_NS(nsvc, ctr) \ + do { \ + struct gprs_ns2_vc *_nsvc = (nsvc); \ + rate_ctr_inc(rate_ctr_group_get_ctr(_nsvc->ctrg, ctr)); \ + rate_ctr_inc(rate_ctr_group_get_ctr(_nsvc->nse->ctrg, ctr)); \ + } while (0) + +#define RATE_CTR_ADD_NS(nsvc, ctr, val) \ + do { \ + struct gprs_ns2_vc *_nsvc = (nsvc); \ + rate_ctr_add(rate_ctr_group_get_ctr(_nsvc->ctrg, ctr), val); \ + rate_ctr_add(rate_ctr_group_get_ctr(_nsvc->nse->ctrg, ctr), val); \ + } while (0) + + +struct osmo_fsm_inst; +struct tlv_parsed; +struct vty; + +struct gprs_ns2_vc_driver; +struct gprs_ns2_vc_bind; + +#define NS_TIMERS_COUNT 11 + +#define TNS_BLOCK_STR "tns-block" +#define TNS_BLOCK_RETRIES_STR "tns-block-retries" +#define TNS_RESET_STR "tns-reset" +#define TNS_RESET_RETRIES_STR "tns-reset-retries" +#define TNS_TEST_STR "tns-test" +#define TNS_ALIVE_STR "tns-alive" +#define TNS_ALIVE_RETRIES_STR "tns-alive-retries" +#define TSNS_PROV_STR "tsns-prov" +#define TSNS_SIZE_RETRIES_STR "tsns-size-retries" +#define TSNS_CONFIG_RETRIES_STR "tsns-config-retries" +#define TSNS_PROCEDURES_RETRIES_STR "tsns-procedures-retries" +#define NS_TIMERS "(" TNS_BLOCK_STR "|" TNS_BLOCK_RETRIES_STR "|" TNS_RESET_STR "|" TNS_RESET_RETRIES_STR "|" TNS_TEST_STR "|"\ + TNS_ALIVE_STR "|" TNS_ALIVE_RETRIES_STR "|" TSNS_PROV_STR "|" TSNS_SIZE_RETRIES_STR "|" TSNS_CONFIG_RETRIES_STR "|"\ + TSNS_PROCEDURES_RETRIES_STR ")" + +#define NS_TIMERS_HELP \ + "(un)blocking Timer (Tns-block) timeout\n" \ + "(un)blocking Timer (Tns-block) number of retries\n" \ + "Reset Timer (Tns-reset) timeout\n" \ + "Reset Timer (Tns-reset) number of retries\n" \ + "Test Timer (Tns-test) timeout\n" \ + "Alive Timer (Tns-alive) timeout\n" \ + "Alive Timer (Tns-alive) number of retries\n" \ + "SNS Provision Timer (Tsns-prov) timeout\n" \ + "SNS Size number of retries\n" \ + "SNS Config number of retries\n" \ + "SNS Procedures number of retries\n" \ + +/* Educated guess - LLC user payload is 1500 bytes plus possible headers */ +#define NS_ALLOC_SIZE 3072 +#define NS_ALLOC_HEADROOM 20 + +#define NS_DEFAULT_TXQUEUE_MAX_LENGTH 128 + +enum ns2_timeout { + NS_TOUT_TNS_BLOCK, + NS_TOUT_TNS_BLOCK_RETRIES, + NS_TOUT_TNS_RESET, + NS_TOUT_TNS_RESET_RETRIES, + NS_TOUT_TNS_TEST, + NS_TOUT_TNS_ALIVE, + NS_TOUT_TNS_ALIVE_RETRIES, + NS_TOUT_TSNS_PROV, + NS_TOUT_TSNS_SIZE_RETRIES, + NS_TOUT_TSNS_CONFIG_RETRIES, + NS_TOUT_TSNS_PROCEDURES_RETRIES, +}; + +enum nsvc_timer_mode { + /* standard timers */ + NSVC_TIMER_TNS_TEST, + NSVC_TIMER_TNS_ALIVE, + NSVC_TIMER_TNS_RESET, + _NSVC_TIMER_NR, +}; + +enum ns2_vc_stat { + NS_STAT_ALIVE_DELAY, +}; + +enum ns2_bind_stat { + NS2_BIND_STAT_BACKLOG_LEN, +}; + +/*! Osmocom NS2 VC create status */ +enum ns2_cs { + NS2_CS_CREATED, /*!< A NSVC object has been created */ + NS2_CS_FOUND, /*!< A NSVC object has been found */ + NS2_CS_REJECTED, /*!< Rejected and answered message */ + NS2_CS_SKIPPED, /*!< Skipped message */ + NS2_CS_ERROR, /*!< Failed to process message */ +}; + +enum ns_ctr { + NS_CTR_PKTS_IN, + NS_CTR_PKTS_OUT, + NS_CTR_PKTS_OUT_DROP, + NS_CTR_BYTES_IN, + NS_CTR_BYTES_OUT, + NS_CTR_BYTES_OUT_DROP, + NS_CTR_BLOCKED, + NS_CTR_UNBLOCKED, + NS_CTR_DEAD, + NS_CTR_REPLACED, + NS_CTR_NSEI_CHG, + NS_CTR_INV_VCI, + NS_CTR_INV_NSEI, + NS_CTR_LOST_ALIVE, + NS_CTR_LOST_RESET, +}; + +#define NSE_S_BLOCKED 0x0001 +#define NSE_S_ALIVE 0x0002 +#define NSE_S_RESET 0x0004 + +#define NS_DESC_B(st) ((st) & NSE_S_BLOCKED ? "BLOCKED" : "UNBLOCKED") +#define NS_DESC_A(st) ((st) & NSE_S_ALIVE ? "ALIVE" : "DEAD") +#define NS_DESC_R(st) ((st) & NSE_S_RESET ? "RESET" : "UNRESET") + +/*! An instance of the NS protocol stack */ +struct gprs_ns2_inst { + /*! callback to the user for incoming UNIT DATA IND */ + osmo_prim_cb cb; + + /*! callback data */ + void *cb_data; + + /*! linked lists of all NSVC binds (e.g. IPv4 bind, but could be also E1 */ + struct llist_head binding; + + /*! linked lists of all NSVC in this instance */ + struct llist_head nse; + + uint16_t timeout[NS_TIMERS_COUNT]; + + /*! workaround for rate counter until rate counter accepts char str as index */ + uint32_t nsvc_rate_ctr_idx; + uint32_t bind_rate_ctr_idx; + + uint32_t txqueue_max_length; +}; + + +/*! Structure repesenting a NSE. The BSS/PCU will only have a single NSE, while SGSN has one for each BSS/PCU */ +struct gprs_ns2_nse { + uint16_t nsei; + + /*! entry back to ns2_inst */ + struct gprs_ns2_inst *nsi; + + /*! llist entry for gprs_ns2_inst */ + struct llist_head list; + + /*! llist head to hold all nsvc */ + struct llist_head nsvc; + + /*! count all active NSVCs */ + int nsvc_count; + + /*! true if this NSE was created by VTY or pcu socket) */ + bool persistent; + + /*! true if this NSE wasn't yet alive at all. + * Will be true after the first status ind with NS_AFF_CAUSE_RECOVERY */ + bool first; + + /*! true if this NSE has at least one alive VC */ + bool alive; + + /*! which link-layer are we based on? */ + enum gprs_ns2_ll ll; + + /*! which dialect does this NSE speaks? */ + enum gprs_ns2_dialect dialect; + + struct osmo_fsm_inst *bss_sns_fi; + + /*! sum of all the data weight of _alive_ NS-VCs */ + uint32_t sum_data_weight; + + /*! sum of all the signalling weight of _alive_ NS-VCs */ + uint32_t sum_sig_weight; + + /*! MTU of a NS PDU. This is the lowest MTU of all NSVCs */ + uint16_t mtu; + + /*! are we implementing the SGSN role? */ + bool ip_sns_role_sgsn; + + /*! NSE-wide statistics */ + struct rate_ctr_group *ctrg; + + /*! recursive anchor */ + bool freed; + + /*! when the NSE became alive or dead */ + struct timespec ts_alive_change; +}; + +/*! Structure representing a single NS-VC */ +struct gprs_ns2_vc { + /*! list of NS-VCs within NSE */ + struct llist_head list; + + /*! list of NS-VCs within bind, bind is the owner! */ + struct llist_head blist; + + /*! pointer to NS Instance */ + struct gprs_ns2_nse *nse; + + /*! pointer to NS VL bind. bind own the memory of this instance */ + struct gprs_ns2_vc_bind *bind; + + /*! true if this NS was created by VTY or pcu socket) */ + bool persistent; + + /*! uniquely identifies NS-VC if VC contains nsvci */ + uint16_t nsvci; + + /*! signalling weight. 0 = don't use for signalling (BVCI == 0)*/ + uint8_t sig_weight; + + /*! signalling packet counter for the load sharing function */ + uint8_t sig_counter; + + /*! data weight. 0 = don't use for user data (BVCI != 0) */ + uint8_t data_weight; + + /*! can be used by the bind/driver of the virtual circuit. e.g. ipv4/ipv6/frgre/e1 */ + void *priv; + + bool nsvci_is_valid; + /*! should this NS-VC only be used for SNS-SIZE and SNS-CONFIG? */ + bool sns_only; + + struct rate_ctr_group *ctrg; + struct osmo_stat_item_group *statg; + + enum gprs_ns2_vc_mode mode; + + struct osmo_fsm_inst *fi; + + /*! recursive anchor */ + bool freed; + + /*! if blocked by O&M/vty */ + bool om_blocked; + + /*! when the NSVC became alive or dead */ + struct timespec ts_alive_change; +}; + +/*! Structure repesenting a bind instance. E.g. IPv4 listen port. */ +struct gprs_ns2_vc_bind { + /*! unique name */ + const char *name; + /*! list entry in nsi */ + struct llist_head list; + /*! list of all VC */ + struct llist_head nsvc; + /*! driver private structure */ + void *priv; + /*! a pointer back to the nsi */ + struct gprs_ns2_inst *nsi; + struct gprs_ns2_vc_driver *driver; + + bool accept_ipaccess; + bool accept_sns; + + /*! transfer capability in mbit */ + int transfer_capability; + + /*! MTU of a NS PDU on this bind. */ + uint16_t mtu; + + /*! which link-layer are we based on? */ + enum gprs_ns2_ll ll; + + /*! send a msg over a VC */ + int (*send_vc)(struct gprs_ns2_vc *nsvc, struct msgb *msg); + + /*! free the vc priv data */ + void (*free_vc)(struct gprs_ns2_vc *nsvc); + + /*! allow to show information for the vty */ + void (*dump_vty)(const struct gprs_ns2_vc_bind *bind, + struct vty *vty, bool stats); + + /*! the IP-SNS signalling weight when doing dynamic configuration */ + uint8_t sns_sig_weight; + /*! the IP-SNS data weight when doing dynamic configuration */ + uint8_t sns_data_weight; + + struct osmo_stat_item_group *statg; + + /*! recursive anchor */ + bool freed; +}; + +struct gprs_ns2_vc_driver { + const char *name; + void *priv; + void (*free_bind)(struct gprs_ns2_vc_bind *driver); +}; + +enum ns2_sns_event { + NS2_SNS_EV_REQ_SELECT_ENDPOINT, /*!< Select a SNS endpoint from the list */ + NS2_SNS_EV_RX_SIZE, + NS2_SNS_EV_RX_SIZE_ACK, + NS2_SNS_EV_RX_CONFIG, + NS2_SNS_EV_RX_CONFIG_END, /*!< SNS-CONFIG with end flag received */ + NS2_SNS_EV_RX_CONFIG_ACK, + NS2_SNS_EV_RX_ADD, + NS2_SNS_EV_RX_DELETE, + NS2_SNS_EV_RX_CHANGE_WEIGHT, + NS2_SNS_EV_RX_ACK, /*!< Rx of SNS-ACK (response to ADD/DELETE/CHG_WEIGHT */ + NS2_SNS_EV_REQ_NO_NSVC, /*!< no more NS-VC remaining (all dead) */ + NS2_SNS_EV_REQ_FREE_NSVCS, /*!< free all NS-VCs */ + NS2_SNS_EV_REQ_NSVC_ALIVE, /*!< a NS-VC became alive */ + NS2_SNS_EV_REQ_ADD_BIND, /*!< add a new local bind to this NSE */ + NS2_SNS_EV_REQ_DELETE_BIND, /*!< remove a local bind from this NSE */ + NS2_SNS_EV_REQ_CHANGE_WEIGHT, /*!< a bind changed its weight */ +}; + +enum ns2_cs ns2_create_vc(struct gprs_ns2_vc_bind *bind, + struct msgb *msg, + const struct osmo_sockaddr *remote, + const char *logname, + struct msgb **reject, + struct gprs_ns2_vc **success); + +int ns2_recv_vc(struct gprs_ns2_vc *nsvc, + struct msgb *msg); + +struct gprs_ns2_vc *ns2_vc_alloc(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_nse *nse, + bool initiater, + enum gprs_ns2_vc_mode vc_mode, + const char *id); + +void ns2_free_nsvcs(struct gprs_ns2_nse *nse); +int ns2_bind_alloc(struct gprs_ns2_inst *nsi, const char *name, + struct gprs_ns2_vc_bind **result); + +struct msgb *ns2_msgb_alloc(void); + +void ns2_sns_write_vty(struct vty *vty, const struct gprs_ns2_nse *nse); +void ns2_sns_dump_vty(struct vty *vty, const char *prefix, const struct gprs_ns2_nse *nse, bool stats); +void ns2_prim_status_ind(struct gprs_ns2_nse *nse, + struct gprs_ns2_vc *nsvc, + uint16_t bvci, + enum gprs_ns2_affecting_cause cause); +void ns2_nse_notify_alive(struct gprs_ns2_vc *nsvc, bool alive); +void ns2_nse_update_mtu(struct gprs_ns2_nse *nse); +int ns2_nse_set_dialect(struct gprs_ns2_nse *nse, enum gprs_ns2_dialect dialect); + +/* message */ +int ns2_validate(struct gprs_ns2_vc *nsvc, + uint8_t pdu_type, + struct msgb *msg, + struct tlv_parsed *tp, + uint8_t *cause); + +/* SNS messages */ +int ns2_tx_sns_ack(struct gprs_ns2_vc *nsvc, uint8_t trans_id, uint8_t *cause, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems); +int ns2_tx_sns_config(struct gprs_ns2_vc *nsvc, bool end_flag, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems); +int ns2_tx_sns_config_ack(struct gprs_ns2_vc *nsvc, uint8_t *cause); +int ns2_tx_sns_size(struct gprs_ns2_vc *nsvc, bool reset_flag, uint16_t max_nr_nsvc, + int ip4_ep_nr, int ip6_ep_nr); +int ns2_tx_sns_size_ack(struct gprs_ns2_vc *nsvc, uint8_t *cause); + +int ns2_tx_sns_add(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems); +int ns2_tx_sns_change_weight(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems); +int ns2_tx_sns_del(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems); + +/* transmit message over a VC */ +int ns2_tx_block(struct gprs_ns2_vc *nsvc, uint8_t cause, uint16_t *nsvci); +int ns2_tx_block_ack(struct gprs_ns2_vc *nsvc, uint16_t *nsvci); + +int ns2_tx_reset(struct gprs_ns2_vc *nsvc, uint8_t cause); +int ns2_tx_reset_ack(struct gprs_ns2_vc *nsvc); + +int ns2_tx_unblock(struct gprs_ns2_vc *nsvc); +int ns2_tx_unblock_ack(struct gprs_ns2_vc *nsvc); + +int ns2_tx_alive(struct gprs_ns2_vc *nsvc); +int ns2_tx_alive_ack(struct gprs_ns2_vc *nsvc); + +int ns2_tx_unit_data(struct gprs_ns2_vc *nsvc, + uint16_t bvci, uint8_t sducontrol, + struct msgb *msg); + +int ns2_tx_status(struct gprs_ns2_vc *nsvc, uint8_t cause, + uint16_t bvci, struct msgb *orig_msg, uint16_t *nsvci); + +/* driver */ +struct gprs_ns2_vc *ns2_ip_bind_connect(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_nse *nse, + const struct osmo_sockaddr *remote); +int ns2_ip_count_bind(struct gprs_ns2_inst *nsi, struct osmo_sockaddr *remote); +struct gprs_ns2_vc_bind *ns2_ip_get_bind_by_index(struct gprs_ns2_inst *nsi, + struct osmo_sockaddr *remote, + int index); +void ns2_ip_set_txqueue_max_length(struct gprs_ns2_vc_bind *bind, unsigned int max_length); + +/* sns */ +int ns2_sns_rx(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp); +struct osmo_fsm_inst *ns2_sns_bss_fsm_alloc(struct gprs_ns2_nse *nse, + const char *id); +struct osmo_fsm_inst *ns2_sns_sgsn_fsm_alloc(struct gprs_ns2_nse *nse, const char *id); +void ns2_sns_replace_nsvc(struct gprs_ns2_vc *nsvc); +void ns2_sns_notify_alive(struct gprs_ns2_nse *nse, struct gprs_ns2_vc *nsvc, bool alive); +void ns2_sns_update_weights(struct gprs_ns2_vc_bind *bind); + +/* vc */ +struct osmo_fsm_inst *ns2_vc_fsm_alloc(struct gprs_ns2_vc *nsvc, + const char *id, bool initiate); +int ns2_vc_fsm_start(struct gprs_ns2_vc *nsvc); +int ns2_vc_force_unconfigured(struct gprs_ns2_vc *nsvc); +int ns2_vc_rx(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp); +int ns2_vc_is_alive(struct gprs_ns2_vc *nsvc); +int ns2_vc_is_unblocked(struct gprs_ns2_vc *nsvc); +int ns2_vc_block(struct gprs_ns2_vc *nsvc); +int ns2_vc_reset(struct gprs_ns2_vc *nsvc); +int ns2_vc_unblock(struct gprs_ns2_vc *nsvc); +void ns2_vty_dump_nsvc(struct vty *vty, struct gprs_ns2_vc *nsvc, bool stats); + +/* nse */ +void ns2_nse_notify_unblocked(struct gprs_ns2_vc *nsvc, bool unblocked); +enum gprs_ns2_vc_mode ns2_dialect_to_vc_mode(enum gprs_ns2_dialect dialect); +int ns2_count_transfer_cap(struct gprs_ns2_nse *nse, + uint16_t bvci); + +/* vty */ +int ns2_sns_add_sns_default_binds(struct gprs_ns2_nse *nse); diff --git a/src/gb/gprs_ns2_message.c b/src/gb/gprs_ns2_message.c new file mode 100644 index 00000000..de63b7aa --- /dev/null +++ b/src/gb/gprs_ns2_message.c @@ -0,0 +1,848 @@ +/*! \file gprs_ns2_message.c + * NS-over-FR-over-GRE implementation. + * GPRS Networks Service (NS) messages on the Gb interface. + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> + +#include <osmocom/core/byteswap.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/stats.h> +#include <osmocom/gsm/tlv.h> +#include <osmocom/gprs/gprs_msgb.h> +#include <osmocom/gprs/gprs_ns2.h> +#include <osmocom/gprs/protocol/gsm_08_16.h> + +#include "gprs_ns2_internal.h" + +#define ERR_IF_NSVC_USES_SNS(nsvc, reason) \ + do { \ + if (!nsvc->nse->bss_sns_fi) \ + break; \ + LOGNSVC(nsvc, LOGL_DEBUG, "invalid packet %s with SNS\n", reason); \ + } while (0) + +static int ns2_validate_reset(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp, uint8_t *cause) +{ + if (!TLVP_PRES_LEN(tp, NS_IE_CAUSE, 1) || + !TLVP_PRES_LEN(tp, NS_IE_VCI, 2) || !TLVP_PRES_LEN(tp, NS_IE_NSEI, 2)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + return 0; +} + +static int ns2_validate_reset_ack(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp, uint8_t *cause) +{ + if (!TLVP_PRES_LEN(tp, NS_IE_VCI, 2) || !TLVP_PRES_LEN(tp, NS_IE_NSEI, 2)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + return 0; +} + +static int ns2_validate_block(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp, uint8_t *cause) +{ + if (!TLVP_PRES_LEN(tp, NS_IE_VCI, 2) || !TLVP_PRES_LEN(tp, NS_IE_CAUSE, 1)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + return 0; +} + +static int ns2_validate_block_ack(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp, uint8_t *cause) +{ + if (!TLVP_PRES_LEN(tp, NS_IE_VCI, 2)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + return 0; +} + +static int ns2_validate_status(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp, uint8_t *cause) +{ + + if (!TLVP_PRES_LEN(tp, NS_IE_CAUSE, 1)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + uint8_t _cause = tlvp_val8(tp, NS_IE_CAUSE, 0); + switch (_cause) { + case NS_CAUSE_NSVC_BLOCKED: + case NS_CAUSE_NSVC_UNKNOWN: + if (!TLVP_PRES_LEN(tp, NS_IE_VCI, 1)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + + if (nsvc->mode != GPRS_NS2_VC_MODE_BLOCKRESET) { + *cause = NS_CAUSE_PDU_INCOMP_PSTATE; + return -1; + } + break; + case NS_CAUSE_SEM_INCORR_PDU: + case NS_CAUSE_PDU_INCOMP_PSTATE: + case NS_CAUSE_PROTO_ERR_UNSPEC: + case NS_CAUSE_INVAL_ESSENT_IE: + case NS_CAUSE_MISSING_ESSENT_IE: + if (!TLVP_PRES_LEN(tp, NS_IE_PDU, 1)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + break; + case NS_CAUSE_BVCI_UNKNOWN: + if (!TLVP_PRES_LEN(tp, NS_IE_BVCI, 2)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + break; + case NS_CAUSE_UNKN_IP_TEST_FAILED: + if (!TLVP_PRESENT(tp, NS_IE_IPv4_LIST) && !TLVP_PRESENT(tp, NS_IE_IPv6_LIST)) { + *cause = NS_CAUSE_MISSING_ESSENT_IE; + return -1; + } + break; + } + + return 0; +} + +int ns2_validate(struct gprs_ns2_vc *nsvc, + uint8_t pdu_type, + struct msgb *msg, + struct tlv_parsed *tp, + uint8_t *cause) +{ + switch (pdu_type) { + case NS_PDUT_RESET: + return ns2_validate_reset(nsvc, msg, tp, cause); + case NS_PDUT_RESET_ACK: + return ns2_validate_reset_ack(nsvc, msg, tp, cause); + case NS_PDUT_BLOCK: + return ns2_validate_block(nsvc, msg, tp, cause); + case NS_PDUT_BLOCK_ACK: + return ns2_validate_block_ack(nsvc, msg, tp, cause); + case NS_PDUT_STATUS: + return ns2_validate_status(nsvc, msg, tp, cause); + + /* following PDUs doesn't have any payloads */ + case NS_PDUT_ALIVE: + case NS_PDUT_ALIVE_ACK: + case NS_PDUT_UNBLOCK: + case NS_PDUT_UNBLOCK_ACK: + if (msgb_l2len(msg) != sizeof(struct gprs_ns_hdr)) { + *cause = NS_CAUSE_PROTO_ERR_UNSPEC; + return -1; + } + break; + } + + return 0; +} + +static int ns_vc_tx(struct gprs_ns2_vc *nsvc, struct msgb *msg) +{ + return nsvc->bind->send_vc(nsvc, msg); +} + +/* transmit functions */ +static int ns2_tx_simple(struct gprs_ns2_vc *nsvc, uint8_t pdu_type) +{ + struct msgb *msg = ns2_msgb_alloc(); + struct gprs_ns_hdr *nsh; + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + if (!msg) + return -ENOMEM; + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = pdu_type; + + LOG_NS_TX_SIGNAL(nsvc, nsh->pdu_type); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-BLOCK on a given NS-VC. + * \param[in] vc NS-VC on which the NS-BLOCK is to be transmitted + * \param[in] cause Numeric NS Cause value + * \param[in] nsvci if given this NSVCI will be encoded. If NULL the nsvc->nsvci will be used. + * \returns 0 in case of success */ +int ns2_tx_block(struct gprs_ns2_vc *nsvc, uint8_t cause, uint16_t *nsvci) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t encoded_nsvci; + + if (nsvci) + encoded_nsvci = osmo_htons(*nsvci); + else + encoded_nsvci = osmo_htons(nsvc->nsvci); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS BLOCK"); + + msg = ns2_msgb_alloc(); + if (!msg) + return -ENOMEM; + + rate_ctr_inc(rate_ctr_group_get_ctr(nsvc->ctrg, NS_CTR_BLOCKED)); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = NS_PDUT_BLOCK; + + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, &cause); + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *) &encoded_nsvci); + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, " cause=%s\n", gprs_ns2_cause_str(cause)); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-BLOCK-ACK on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-BLOCK is to be transmitted + * \param[in] nsvci if given this NSVCI will be encoded. If NULL the nsvc->nsvci will be used. + * \returns 0 in case of success */ +int ns2_tx_block_ack(struct gprs_ns2_vc *nsvc, uint16_t *nsvci) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t encoded_nsvci; + + if (nsvci) + encoded_nsvci = osmo_htons(*nsvci); + else + encoded_nsvci = osmo_htons(nsvc->nsvci); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS BLOCK ACK"); + + msg = ns2_msgb_alloc(); + if (!msg) + return -ENOMEM; + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = NS_PDUT_BLOCK_ACK; + + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *) &encoded_nsvci); + + LOG_NS_TX_SIGNAL(nsvc, nsh->pdu_type); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-RESET on a given NS-VC. + * \param[in] nsvc NS-VC used for transmission + * \param[in] cause Numeric NS cause value + * \returns 0 in case of success */ +int ns2_tx_reset(struct gprs_ns2_vc *nsvc, uint8_t cause) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsvci = osmo_htons(nsvc->nsvci); + uint16_t nsei = osmo_htons(nsvc->nse->nsei); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS RESET"); + + msg = ns2_msgb_alloc(); + if (!msg) + return -ENOMEM; + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = NS_PDUT_RESET; + + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, &cause); + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *) &nsvci); + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *) &nsei); + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, " cause=%s\n", gprs_ns2_cause_str(cause)); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-RESET-ACK on a given NS-VC. + * \param[in] nsvc NS-VC used for transmission + * \returns 0 in case of success */ +int ns2_tx_reset_ack(struct gprs_ns2_vc *nsvc) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsvci, nsei; + + /* Section 9.2.6 */ + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS RESET ACK"); + + msg = ns2_msgb_alloc(); + if (!msg) + return -ENOMEM; + + nsvci = osmo_htons(nsvc->nsvci); + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = NS_PDUT_RESET_ACK; + + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *)&nsvci); + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + + LOG_NS_TX_SIGNAL(nsvc, nsh->pdu_type); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-UNBLOCK on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-UNBLOCK is to be transmitted + * \returns 0 in case of success */ +int ns2_tx_unblock(struct gprs_ns2_vc *nsvc) +{ + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS UNBLOCK"); + + return ns2_tx_simple(nsvc, NS_PDUT_UNBLOCK); +} + + +/*! Transmit a NS-UNBLOCK-ACK on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-UNBLOCK-ACK is to be transmitted + * \returns 0 in case of success */ +int ns2_tx_unblock_ack(struct gprs_ns2_vc *nsvc) +{ + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + ERR_IF_NSVC_USES_SNS(nsvc, "transmit NS UNBLOCK ACK"); + + return ns2_tx_simple(nsvc, NS_PDUT_UNBLOCK_ACK); +} + +/*! Transmit a NS-ALIVE on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-ALIVE is to be transmitted + * \returns 0 in case of success */ +int ns2_tx_alive(struct gprs_ns2_vc *nsvc) +{ + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + return ns2_tx_simple(nsvc, NS_PDUT_ALIVE); +} + +/*! Transmit a NS-ALIVE-ACK on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-ALIVE-ACK is to be transmitted + * \returns 0 in case of success */ +int ns2_tx_alive_ack(struct gprs_ns2_vc *nsvc) +{ + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + return ns2_tx_simple(nsvc, NS_PDUT_ALIVE_ACK); +} + +/*! Transmit NS-UNITDATA on a given NS-VC. + * \param[in] nsvc NS-VC on which the NS-UNITDATA is to be transmitted + * \param[in] bvci BVCI to encode in NS-UNITDATA header + * \param[in] sducontrol SDU control octet of NS header + * \param[in] msg message buffer containing payload + * \returns 0 in case of success */ +int ns2_tx_unit_data(struct gprs_ns2_vc *nsvc, + uint16_t bvci, uint8_t sducontrol, + struct msgb *msg) +{ + struct gprs_ns_hdr *nsh; + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + msg->l2h = msgb_push(msg, sizeof(*nsh) + 3); + nsh = (struct gprs_ns_hdr *) msg->l2h; + if (!nsh) { + LOGNSVC(nsvc, LOGL_ERROR, "Not enough headroom for NS header\n"); + msgb_free(msg); + return -EIO; + } + + nsh->pdu_type = NS_PDUT_UNITDATA; + nsh->data[0] = sducontrol; + nsh->data[1] = bvci >> 8; + nsh->data[2] = bvci & 0xff; + + LOG_NS_DATA(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, "\n"); + return ns_vc_tx(nsvc, msg); +} + +/*! Transmit a NS-STATUS on a given NS-VC. + * \param[in] nsvc NS-VC to be used for transmission + * \param[in] cause Numeric NS cause value + * \param[in] bvci BVCI to be reset within NSVC + * \param[in] orig_msg message causing the STATUS + * \param[in] nsvci if given this NSVCI will be encoded. If NULL the nsvc->nsvci will be used. + * \returns 0 in case of success */ +int ns2_tx_status(struct gprs_ns2_vc *nsvc, uint8_t cause, + uint16_t bvci, struct msgb *orig_msg, uint16_t *nsvci) +{ + struct msgb *msg = ns2_msgb_alloc(); + struct gprs_ns_hdr *nsh; + uint16_t encoded_nsvci; + unsigned int orig_len, max_orig_len; + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + + bvci = osmo_htons(bvci); + + if (!msg) + return -ENOMEM; + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = NS_PDUT_STATUS; + + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, &cause); + + switch (cause) { + case NS_CAUSE_NSVC_BLOCKED: + case NS_CAUSE_NSVC_UNKNOWN: + /* Section 9.2.7.1: Static conditions for NS-VCI */ + if (nsvci) + encoded_nsvci = osmo_htons(*nsvci); + else + encoded_nsvci = osmo_htons(nsvc->nsvci); + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *)&encoded_nsvci); + break; + case NS_CAUSE_SEM_INCORR_PDU: + case NS_CAUSE_PDU_INCOMP_PSTATE: + case NS_CAUSE_PROTO_ERR_UNSPEC: + case NS_CAUSE_INVAL_ESSENT_IE: + case NS_CAUSE_MISSING_ESSENT_IE: + /* Section 9.2.7.2: Static conditions for NS PDU */ + /* ensure the PDU doesn't exceed the MTU */ + orig_len = msgb_l2len(orig_msg); + max_orig_len = msgb_length(msg) + TVLV_GROSS_LEN(orig_len); + if (max_orig_len > nsvc->bind->mtu) + orig_len -= max_orig_len - nsvc->bind->mtu; + msgb_tvlv_put(msg, NS_IE_PDU, orig_len, orig_msg->l2h); + break; + case NS_CAUSE_BVCI_UNKNOWN: + /* Section 9.2.7.3: Static conditions for BVCI */ + msgb_tvlv_put(msg, NS_IE_VCI, 2, (uint8_t *)&bvci); + break; + + default: + break; + } + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, " cause=%s\n", gprs_ns2_cause_str(cause)); + return ns_vc_tx(nsvc, msg); +} + +/*! Encode + Transmit a SNS-ADD/SNS-CHANGE-WEIGHT as per Section 9.3.2/9.3.3. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG + * \param[in] pdu The PDU type to send out + * \param[in] trans_id The transaction id + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \param[in] ip6_elems Array of IPv6 Elements + * \param[in] num_ip6_elems number of ip6_elems + * \returns 0 on success; negative in case of error */ +static int ns2_tx_sns_procedure(struct gprs_ns2_vc *nsvc, + enum ns_pdu_type pdu, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + if (!nsvc) + return -EINVAL; + + if (!ip4_elems && !ip6_elems) + return -EINVAL; + + msg = ns2_msgb_alloc(); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + nsh->pdu_type = pdu; + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + msgb_v_put(msg, trans_id); + + /* List of IP4 Elements 10.3.2c */ + if (ip4_elems) { + msgb_tvlv_put(msg, NS_IE_IPv4_LIST, num_ip4_elems*sizeof(struct gprs_ns_ie_ip4_elem), + (const uint8_t *)ip4_elems); + } else if (ip6_elems) { + /* List of IP6 elements 10.3.2d */ + msgb_tvlv_put(msg, NS_IE_IPv6_LIST, num_ip6_elems*sizeof(struct gprs_ns_ie_ip6_elem), + (const uint8_t *)ip6_elems); + } + + return ns_vc_tx(nsvc, msg); +} + +/*! Encode + Transmit a SNS-ADD as per Section 9.3.2. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG + * \param[in] trans_id The transaction id + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \param[in] ip6_elems Array of IPv6 Elements + * \param[in] num_ip6_elems number of ip6_elems + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_add(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + return ns2_tx_sns_procedure(nsvc, SNS_PDUT_ADD, trans_id, ip4_elems, num_ip4_elems, ip6_elems, num_ip6_elems); +} + +/*! Encode + Transmit a SNS-CHANGE-WEIGHT as per Section 9.3.3. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG + * \param[in] trans_id The transaction id + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \param[in] ip6_elems Array of IPv6 Elements + * \param[in] num_ip6_elems number of ip6_elems + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_change_weight(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + return ns2_tx_sns_procedure(nsvc, SNS_PDUT_CHANGE_WEIGHT, trans_id, ip4_elems, num_ip4_elems, ip6_elems, num_ip6_elems); +} + +/*! Encode + Transmit a SNS-DEL as per Section 9.3.6. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG + * \param[in] trans_id The transaction id + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \param[in] ip6_elems Array of IPv6 Elements + * \param[in] num_ip6_elems number of ip6_elems + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_del(struct gprs_ns2_vc *nsvc, + uint8_t trans_id, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + /* TODO: IP Address field */ + return ns2_tx_sns_procedure(nsvc, SNS_PDUT_DELETE, trans_id, ip4_elems, num_ip4_elems, ip6_elems, num_ip6_elems); +} + + +/*! Encode + Transmit a SNS-ACK as per Section 9.3.1. + * \param[in] nsvc NS-VC through which to transmit the ACK + * \param[in] trans_id Transaction ID which to acknowledge + * \param[in] cause Pointer to cause value (NULL if no cause to be sent) + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_ack(struct gprs_ns2_vc *nsvc, uint8_t trans_id, uint8_t *cause, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + if (!nsvc) + return -1; + + msg = ns2_msgb_alloc(); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = SNS_PDUT_ACK; + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + msgb_v_put(msg, trans_id); + if (cause) + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, cause); + if (ip4_elems) { + /* List of IP4 Elements 10.3.2c */ + msgb_tvlv_put(msg, NS_IE_IPv4_LIST, + num_ip4_elems*sizeof(struct gprs_ns_ie_ip4_elem), + (const uint8_t *)ip4_elems); + } + if (ip6_elems) { + /* List of IP6 elements 10.3.2d */ + msgb_tvlv_put(msg, NS_IE_IPv6_LIST, + num_ip6_elems*sizeof(struct gprs_ns_ie_ip6_elem), + (const uint8_t *)ip6_elems); + } + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, + " (trans_id=%u, cause=%s, num_ip4=%u, num_ip6=%u)\n", + trans_id, cause ? gprs_ns2_cause_str(*cause) : "NULL", num_ip4_elems, num_ip6_elems); + return ns_vc_tx(nsvc, msg); +} + +/*! Encode + Transmit a SNS-CONFIG as per Section 9.3.4. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG + * \param[in] end_flag Whether or not this is the last SNS-CONFIG + * \param[in] ip4_elems Array of IPv4 Elements + * \param[in] num_ip4_elems number of ip4_elems + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_config(struct gprs_ns2_vc *nsvc, bool end_flag, + const struct gprs_ns_ie_ip4_elem *ip4_elems, + unsigned int num_ip4_elems, + const struct gprs_ns_ie_ip6_elem *ip6_elems, + unsigned int num_ip6_elems) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + if (!nsvc) + return -1; + + msg = ns2_msgb_alloc(); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = SNS_PDUT_CONFIG; + + msgb_v_put(msg, end_flag ? 0x01 : 0x00); + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + + /* List of IP4 Elements 10.3.2c */ + if (ip4_elems) { + msgb_tvlv_put(msg, NS_IE_IPv4_LIST, num_ip4_elems*sizeof(struct gprs_ns_ie_ip4_elem), + (const uint8_t *)ip4_elems); + } else if (ip6_elems) { + /* List of IP6 elements 10.3.2d */ + msgb_tvlv_put(msg, NS_IE_IPv6_LIST, num_ip6_elems*sizeof(struct gprs_ns_ie_ip6_elem), + (const uint8_t *)ip6_elems); + } + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, + " (end_flag=%u, num_ip4=%u, num_ip6=%u)\n", + end_flag, num_ip4_elems, num_ip6_elems); + return ns_vc_tx(nsvc, msg); +} + +/*! Encode + Transmit a SNS-CONFIG-ACK as per Section 9.3.5. + * \param[in] nsvc NS-VC through which to transmit the SNS-CONFIG-ACK + * \param[in] cause Pointer to cause value (NULL if no cause to be sent) + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_config_ack(struct gprs_ns2_vc *nsvc, uint8_t *cause) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + if (!nsvc) + return -1; + + msg = ns2_msgb_alloc(); + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = SNS_PDUT_CONFIG_ACK; + + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + if (cause) + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, cause); + + LOGNSVC(nsvc, LOGL_INFO, "Tx SNS-CONFIG-ACK (cause=%s)\n", + cause ? gprs_ns2_cause_str(*cause) : "NULL"); + LOG_NS_TX_SIGNAL(nsvc, nsh->pdu_type); + return ns_vc_tx(nsvc, msg); +} + + +/*! Encode + transmit a SNS-SIZE as per Section 9.3.7. + * \param[in] nsvc NS-VC through which to transmit the SNS-SIZE + * \param[in] reset_flag Whether or not to add a RESET flag + * \param[in] max_nr_nsvc Maximum number of NS-VCs + * \param[in] ip4_ep_nr Number of IPv4 endpoints (< 0 will omit the TLV) + * \param[in] ip6_ep_nr Number of IPv6 endpoints (< 0 will omit the TLV) + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_size(struct gprs_ns2_vc *nsvc, bool reset_flag, uint16_t max_nr_nsvc, + int ip4_ep_nr, int ip6_ep_nr) +{ + struct msgb *msg; + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + if (!nsvc) + return -1; + + msg = ns2_msgb_alloc(); + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = SNS_PDUT_SIZE; + + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + msgb_tv_put(msg, NS_IE_RESET_FLAG, reset_flag ? 0x01 : 0x00); + msgb_tv16_put(msg, NS_IE_MAX_NR_NSVC, max_nr_nsvc); + if (ip4_ep_nr >= 0) + msgb_tv16_put(msg, NS_IE_IPv4_EP_NR, ip4_ep_nr); + if (ip6_ep_nr >= 0) + msgb_tv16_put(msg, NS_IE_IPv6_EP_NR, ip6_ep_nr); + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, + " (reset=%u, max_nr_nsvc=%u, num_ip4=%d, num_ip6=%d)\n", + reset_flag, max_nr_nsvc, ip4_ep_nr, ip6_ep_nr); + return ns_vc_tx(nsvc, msg); +} + +/*! Encode + Transmit a SNS-SIZE-ACK as per Section 9.3.8. + * \param[in] nsvc NS-VC through which to transmit the SNS-SIZE-ACK + * \param[in] cause Pointer to cause value (NULL if no cause to be sent) + * \returns 0 on success; negative in case of error */ +int ns2_tx_sns_size_ack(struct gprs_ns2_vc *nsvc, uint8_t *cause) +{ + struct msgb *msg = ns2_msgb_alloc(); + struct gprs_ns_hdr *nsh; + uint16_t nsei; + + log_set_context(LOG_CTX_GB_NSE, nsvc->nse); + log_set_context(LOG_CTX_GB_NSVC, nsvc); + if (!msg) + return -ENOMEM; + + if (!nsvc->nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_ERROR, "Cannot transmit SNS on NSVC without SNS active\n"); + msgb_free(msg); + return -EIO; + } + + nsei = osmo_htons(nsvc->nse->nsei); + + msg->l2h = msgb_put(msg, sizeof(*nsh)); + nsh = (struct gprs_ns_hdr *) msg->l2h; + + nsh->pdu_type = SNS_PDUT_SIZE_ACK; + + msgb_tvlv_put(msg, NS_IE_NSEI, 2, (uint8_t *)&nsei); + if (cause) + msgb_tvlv_put(msg, NS_IE_CAUSE, 1, cause); + + LOG_NS_SIGNAL(nsvc, "Tx", nsh->pdu_type, LOGL_INFO, " cause=%s\n", + cause ? gprs_ns2_cause_str(*cause) : "NULL"); + return ns_vc_tx(nsvc, msg); +} diff --git a/src/gb/gprs_ns2_sns.c b/src/gb/gprs_ns2_sns.c new file mode 100644 index 00000000..0afc06ed --- /dev/null +++ b/src/gb/gprs_ns2_sns.c @@ -0,0 +1,3106 @@ +/*! \file gprs_ns2_sns.c + * NS Sub-Network Service Protocol implementation + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2018-2021 by Harald Welte <laforge@gnumonks.org> + * (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +/* The BSS NSE only has one SGSN IP address configured, and it will use the SNS procedures + * to communicated its local IPs/ports as well as all the SGSN side IPs/ports and + * associated weights. The BSS then uses this to establish a full mesh + * of NSVCs between all BSS-side IPs/ports and SGSN-side IPs/ports. + * + * Known limitation/expectation/bugs: + * - No concurrent dual stack. It supports either IPv4 or IPv6, but not both at the same time. + * - SNS Add/Change/Delete: Doesn't answer on the same NSVC as received SNS ADD/CHANGE/DELETE PDUs. + * - SNS Add/Change/Delete: Doesn't communicated the failed IPv4/IPv6 entries on the SNS_ACK. + */ + +#include <errno.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <stdint.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/gsm/tlv.h> +#include <osmocom/gprs/gprs_msgb.h> +#include <osmocom/gprs/gprs_ns2.h> +#include <osmocom/gprs/protocol/gsm_08_16.h> + +#include "gprs_ns2_internal.h" + +#define S(x) (1 << (x)) + +enum ns2_sns_role { + GPRS_SNS_ROLE_BSS, + GPRS_SNS_ROLE_SGSN, +}; + +/* BSS-side-only states _ST_BSS_; SGSN-side only states _ST_SGSN_; others shared */ +enum gprs_sns_bss_state { + GPRS_SNS_ST_UNCONFIGURED, + GPRS_SNS_ST_BSS_SIZE, /*!< SNS-SIZE procedure ongoing */ + GPRS_SNS_ST_BSS_CONFIG_BSS, /*!< SNS-CONFIG procedure (BSS->SGSN) ongoing */ + GPRS_SNS_ST_BSS_CONFIG_SGSN, /*!< SNS-CONFIG procedure (SGSN->BSS) ongoing */ + GPRS_SNS_ST_CONFIGURED, + GPRS_SNS_ST_SGSN_WAIT_CONFIG, /* !< SGSN role: Wait for CONFIG from BSS */ + GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK, /* !< SGSN role: Wait for CONFIG-ACK from BSS */ + GPRS_SNS_ST_LOCAL_PROCEDURE, /*!< in process of a ADD/DEL/CHANGE procedure towards SGSN (BSS->SGSN) */ +}; + +static const struct value_string gprs_sns_event_names[] = { + { NS2_SNS_EV_REQ_SELECT_ENDPOINT, "REQ_SELECT_ENDPOINT" }, + { NS2_SNS_EV_RX_SIZE, "RX_SIZE" }, + { NS2_SNS_EV_RX_SIZE_ACK, "RX_SIZE_ACK" }, + { NS2_SNS_EV_RX_CONFIG, "RX_CONFIG" }, + { NS2_SNS_EV_RX_CONFIG_END, "RX_CONFIG_END" }, + { NS2_SNS_EV_RX_CONFIG_ACK, "RX_CONFIG_ACK" }, + { NS2_SNS_EV_RX_ADD, "RX_ADD" }, + { NS2_SNS_EV_RX_DELETE, "RX_DELETE" }, + { NS2_SNS_EV_RX_ACK, "RX_ACK" }, + { NS2_SNS_EV_RX_CHANGE_WEIGHT, "RX_CHANGE_WEIGHT" }, + { NS2_SNS_EV_REQ_NO_NSVC, "REQ_NO_NSVC" }, + { NS2_SNS_EV_REQ_FREE_NSVCS, "REQ_FREE_NSVCS" }, + { NS2_SNS_EV_REQ_NSVC_ALIVE, "REQ_NSVC_ALIVE"}, + { NS2_SNS_EV_REQ_ADD_BIND, "REQ_ADD_BIND"}, + { NS2_SNS_EV_REQ_DELETE_BIND, "REQ_DELETE_BIND"}, + { NS2_SNS_EV_REQ_CHANGE_WEIGHT, "REQ_CHANGE_WEIGHT"}, + { 0, NULL } +}; + +#define GPRS_SNS_FLAG_KEEP_SELECT_ENDPOINT_ORDER (void *) 1 + +enum sns_procedure { + SNS_PROC_NONE, /*!< used as invalid/idle value */ + SNS_PROC_ADD, + SNS_PROC_DEL, + SNS_PROC_CHANGE_WEIGHT, +}; + +struct sns_endpoint { + struct llist_head list; + struct osmo_sockaddr saddr; +}; + +struct ns2_sns_bind { + struct llist_head list; + struct gprs_ns2_vc_bind *bind; + uint8_t change_weight_state; +}; + +struct ns2_sns_procedure { + struct llist_head list; + struct ns2_sns_bind *sbind; + uint16_t sig_weight; + uint16_t data_weight; + /* copy entry to protect against changes of gss->local */ + struct gprs_ns_ie_ip4_elem ip4; + struct gprs_ns_ie_ip6_elem ip6; + enum sns_procedure procedure; + uint8_t trans_id; + /* is the procedure in process */ + bool running; +}; + +struct ns2_sns_elems { + struct gprs_ns_ie_ip4_elem *ip4; + unsigned int num_ip4; + struct gprs_ns_ie_ip6_elem *ip6; + unsigned int num_ip6; +}; + +struct ns2_sns_state { + struct gprs_ns2_nse *nse; + + /* containing the address family AF_* */ + int family; + enum ns2_sns_role role; /* local role: BSS or SGSN */ + + /* holds the list of initial SNS endpoints */ + struct llist_head sns_endpoints; + /* list of used struct ns2_sns_bind */ + struct llist_head binds; + /* pointer to the bind which was used to initiate the SNS connection */ + struct ns2_sns_bind *initial_bind; + /* prevent recursive reselection */ + bool reselection_running; + + /* protection against recursive free() */ + bool block_no_nsvc_events; + + /* The current initial SNS endpoints. + * The initial connection will be moved into the NSE + * if configured via SNS. Otherwise it will be removed + * in configured state. */ + struct sns_endpoint *initial; + /* all SNS PDU will be sent over this nsvc */ + struct gprs_ns2_vc *sns_nsvc; + /* timer N */ + int N; + /* true if at least one nsvc is alive */ + bool alive; + + /* local configuration to send to the remote end */ + struct ns2_sns_elems local; + + /* local configuration after all local procedures applied */ + struct ns2_sns_elems local_procedure; + + /* remote configuration as received */ + struct ns2_sns_elems remote; + + /* local configuration about our capabilities in terms of connections to + * remote (SGSN) side */ + size_t num_max_nsvcs; + size_t num_max_ip4_remote; + size_t num_max_ip6_remote; + + struct llist_head procedures; + struct ns2_sns_procedure *current_procedure; + uint8_t trans_id; +}; + +static inline struct gprs_ns2_nse *nse_inst_from_fi(struct osmo_fsm_inst *fi) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + return gss->nse; +} + +/* The SNS has failed. Etither restart the SNS (BSS) or remove the SNS (SGSN) */ +#define sns_failed(fi, reason) \ + _sns_failed(fi, reason, __FILE__, __LINE__) +static void _sns_failed(struct osmo_fsm_inst *fi, const char *reason, const char *file, int line) +{ + struct ns2_sns_state *gss = fi->priv; + + if (reason) + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, "NSE %d: SNS failed: %s\n", gss->nse->nsei, reason); + + gss->alive = false; + if (gss->role == GPRS_SNS_ROLE_SGSN) { + if (!gss->nse->persistent) + gprs_ns2_free_nse(gss->nse); + else + _osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0, file, line); + } else { + _osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, NULL, file, line); + } +} + +/* helper function to compute the sum of all (data or signaling) weights */ +static int ip4_weight_sum(const struct ns2_sns_elems *elems, bool data_weight) +{ + unsigned int i; + int weight_sum = 0; + + for (i = 0; i < elems->num_ip4; i++) { + if (data_weight) + weight_sum += elems->ip4[i].data_weight; + else + weight_sum += elems->ip4[i].sig_weight; + } + return weight_sum; +} +#define ip4_weight_sum_data(elems) ip4_weight_sum(elems, true) +#define ip4_weight_sum_sig(elems) ip4_weight_sum(elems, false) + +/* helper function to compute the sum of all (data or signaling) weights */ +static int ip6_weight_sum(const struct ns2_sns_elems *elems, bool data_weight) +{ + unsigned int i; + int weight_sum = 0; + + for (i = 0; i < elems->num_ip6; i++) { + if (data_weight) + weight_sum += elems->ip6[i].data_weight; + else + weight_sum += elems->ip6[i].sig_weight; + } + return weight_sum; +} +#define ip6_weight_sum_data(elems) ip6_weight_sum(elems, true) +#define ip6_weight_sum_sig(elems) ip6_weight_sum(elems, false) + +static int ip46_weight_sum(const struct ns2_sns_elems *elems, bool data_weight) +{ + return ip4_weight_sum(elems, data_weight) + + ip6_weight_sum(elems, data_weight); +} +#define ip46_weight_sum_data(elems) ip46_weight_sum(elems, true) +#define ip46_weight_sum_sig(elems) ip46_weight_sum(elems, false) + +static struct gprs_ns2_vc *nsvc_by_ip4_elem(struct gprs_ns2_nse *nse, + const struct gprs_ns_ie_ip4_elem *ip4) +{ + struct osmo_sockaddr sa; + /* copy over. Both data structures use network byte order */ + sa.u.sin.sin_addr.s_addr = ip4->ip_addr; + sa.u.sin.sin_port = ip4->udp_port; + sa.u.sin.sin_family = AF_INET; + + return gprs_ns2_nsvc_by_sockaddr_nse(nse, &sa); +} + +static struct gprs_ns2_vc *nsvc_by_ip6_elem(struct gprs_ns2_nse *nse, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + struct osmo_sockaddr sa; + /* copy over. Both data structures use network byte order */ + sa.u.sin6.sin6_addr = ip6->ip_addr; + sa.u.sin6.sin6_port = ip6->udp_port; + sa.u.sin6.sin6_family = AF_INET; + + return gprs_ns2_nsvc_by_sockaddr_nse(nse, &sa); +} + +/*! Return the initial SNS remote socket address + * \param nse NS Entity + * \return address of the initial SNS connection; NULL in case of error + */ +const struct osmo_sockaddr *gprs_ns2_nse_sns_remote(struct gprs_ns2_nse *nse) +{ + struct ns2_sns_state *gss; + + if (!nse->bss_sns_fi) + return NULL; + + gss = (struct ns2_sns_state *) nse->bss_sns_fi->priv; + return &gss->initial->saddr; +} + +/*! called when a nsvc is beeing freed or the nsvc became dead */ +void ns2_sns_replace_nsvc(struct gprs_ns2_vc *nsvc) +{ + struct gprs_ns2_nse *nse = nsvc->nse; + struct gprs_ns2_vc *tmp; + struct osmo_fsm_inst *fi = nse->bss_sns_fi; + struct ns2_sns_state *gss; + + if (!fi) + return; + + gss = (struct ns2_sns_state *) fi->priv; + if (nsvc != gss->sns_nsvc) + return; + + gss->sns_nsvc = NULL; + if (gss->alive) { + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (ns2_vc_is_unblocked(tmp)) { + gss->sns_nsvc = tmp; + return; + } + } + } else { + /* the SNS is waiting for its first NS-VC to come up + * choose any other nsvc */ + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (nsvc != tmp) { + gss->sns_nsvc = tmp; + return; + } + } + } + + if (gss->block_no_nsvc_events) + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_REQ_NO_NSVC, NULL); +} + +static void ns2_clear_elems(struct ns2_sns_elems *elems) +{ + TALLOC_FREE(elems->ip4); + TALLOC_FREE(elems->ip6); + + elems->num_ip4 = 0; + elems->num_ip6 = 0; +} + +static void ns2_clear_procedures(struct ns2_sns_state *gss) +{ + struct ns2_sns_procedure *procedure, *tmp; + gss->current_procedure = NULL; + llist_for_each_entry_safe(procedure, tmp, &gss->procedures, list) { + llist_del(&procedure->list); + talloc_free(procedure); + } +} + +static void ns2_vc_create_ip(struct osmo_fsm_inst *fi, struct gprs_ns2_nse *nse, const struct osmo_sockaddr *remote, + uint8_t sig_weight, uint8_t data_weight) +{ + struct gprs_ns2_inst *nsi = nse->nsi; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_vc_bind *bind; + + /* for every bind, create a connection if bind type == IP */ + llist_for_each_entry(bind, &nsi->binding, list) { + if (bind->ll != GPRS_NS2_LL_UDP) + continue; + /* ignore failed connection */ + nsvc = gprs_ns2_ip_connect_inactive(bind, + remote, + nse, 0); + if (!nsvc) { + LOGPFSML(fi, LOGL_ERROR, "SNS-CONFIG: Failed to create NSVC\n"); + continue; + } + + nsvc->sig_weight = sig_weight; + nsvc->data_weight = data_weight; + } +} + +static void ns2_nsvc_create_ip4(struct osmo_fsm_inst *fi, + struct gprs_ns2_nse *nse, + const struct gprs_ns_ie_ip4_elem *ip4) +{ + struct osmo_sockaddr remote = { }; + /* copy over. Both data structures use network byte order */ + remote.u.sin.sin_family = AF_INET; + remote.u.sin.sin_addr.s_addr = ip4->ip_addr; + remote.u.sin.sin_port = ip4->udp_port; + + ns2_vc_create_ip(fi, nse, &remote, ip4->sig_weight, ip4->data_weight); +} + +static void ns2_nsvc_create_ip6(struct osmo_fsm_inst *fi, + struct gprs_ns2_nse *nse, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + struct osmo_sockaddr remote = {}; + /* copy over. Both data structures use network byte order */ + remote.u.sin6.sin6_family = AF_INET6; + remote.u.sin6.sin6_addr = ip6->ip_addr; + remote.u.sin6.sin6_port = ip6->udp_port; + + ns2_vc_create_ip(fi, nse, &remote, ip6->sig_weight, ip6->data_weight); +} + +static struct gprs_ns2_vc *nsvc_for_bind_and_remote(struct gprs_ns2_nse *nse, + struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *remote) +{ + struct gprs_ns2_vc *nsvc; + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (nsvc->bind != bind) + continue; + + if (!osmo_sockaddr_cmp(remote, gprs_ns2_ip_vc_remote(nsvc))) + return nsvc; + } + return NULL; +} + +static int create_missing_nsvcs(struct osmo_fsm_inst *fi) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_vc *nsvc; + struct ns2_sns_bind *sbind; + struct osmo_sockaddr remote = { }; + unsigned int i; + + /* iterate over all remote IPv4 endpoints */ + for (i = 0; i < gss->remote.num_ip4; i++) { + const struct gprs_ns_ie_ip4_elem *ip4 = &gss->remote.ip4[i]; + + remote.u.sin.sin_family = AF_INET; + remote.u.sin.sin_addr.s_addr = ip4->ip_addr; + remote.u.sin.sin_port = ip4->udp_port; + + /* iterate over all local binds within this SNS */ + llist_for_each_entry(sbind, &gss->binds, list) { + struct gprs_ns2_vc_bind *bind = sbind->bind; + + /* we only care about UDP binds */ + if (bind->ll != GPRS_NS2_LL_UDP) + continue; + + nsvc = nsvc_for_bind_and_remote(nse, bind, &remote); + if (!nsvc) { + nsvc = gprs_ns2_ip_connect_inactive(bind, &remote, nse, 0); + if (!nsvc) { + /* TODO: add to a list to send back a NS-STATUS */ + continue; + } + } + + /* update data / signalling weight */ + nsvc->data_weight = ip4->data_weight; + nsvc->sig_weight = ip4->sig_weight; + nsvc->sns_only = false; + } + } + + /* iterate over all remote IPv4 endpoints */ + for (i = 0; i < gss->remote.num_ip6; i++) { + const struct gprs_ns_ie_ip6_elem *ip6 = &gss->remote.ip6[i]; + + remote.u.sin6.sin6_family = AF_INET6; + remote.u.sin6.sin6_addr = ip6->ip_addr; + remote.u.sin6.sin6_port = ip6->udp_port; + + /* iterate over all local binds within this SNS */ + llist_for_each_entry(sbind, &gss->binds, list) { + struct gprs_ns2_vc_bind *bind = sbind->bind; + + if (bind->ll != GPRS_NS2_LL_UDP) + continue; + + /* we only care about UDP binds */ + nsvc = nsvc_for_bind_and_remote(nse, bind, &remote); + if (!nsvc) { + nsvc = gprs_ns2_ip_connect_inactive(bind, &remote, nse, 0); + if (!nsvc) { + /* TODO: add to a list to send back a NS-STATUS */ + continue; + } + } + + /* update data / signalling weight */ + nsvc->data_weight = ip6->data_weight; + nsvc->sig_weight = ip6->sig_weight; + nsvc->sns_only = false; + } + } + + + return 0; +} + +/* Add a given remote IPv4 element to gprs_sns_state */ +static int add_ip4_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip4_elem *ip4) +{ + /* check for duplicates */ + for (unsigned int i = 0; i < elems->num_ip4; i++) { + if (memcmp(&elems->ip4[i], ip4, sizeof(*ip4))) + continue; + return -1; + } + + elems->ip4 = talloc_realloc(gss, elems->ip4, struct gprs_ns_ie_ip4_elem, + elems->num_ip4+1); + elems->ip4[elems->num_ip4] = *ip4; + elems->num_ip4 += 1; + return 0; +} + +/* Remove a given remote IPv4 element from gprs_sns_state */ +static int remove_ip4_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip4_elem *ip4) +{ + unsigned int i; + + for (i = 0; i < elems->num_ip4; i++) { + if (memcmp(&elems->ip4[i], ip4, sizeof(*ip4))) + continue; + /* all array elements < i remain as they are; all > i are shifted left by one */ + memmove(&elems->ip4[i], &elems->ip4[i+1], elems->num_ip4-i-1); + elems->num_ip4 -= 1; + return 0; + } + return -1; +} + +/* update the weights for specified remote IPv4 */ +static int update_ip4_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip4_elem *ip4) +{ + unsigned int i; + + for (i = 0; i < elems->num_ip4; i++) { + if (elems->ip4[i].ip_addr != ip4->ip_addr || + elems->ip4[i].udp_port != ip4->udp_port) + continue; + + elems->ip4[i].sig_weight = ip4->sig_weight; + elems->ip4[i].data_weight = ip4->data_weight; + return 0; + } + return -1; +} + +/* Add a given remote IPv6 element to gprs_sns_state */ +static int add_ip6_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + /* check for duplicates */ + for (unsigned int i = 0; i < elems->num_ip6; i++) { + if (memcmp(&elems->ip6[i].ip_addr, &ip6->ip_addr, sizeof(ip6->ip_addr)) || + elems->ip6[i].udp_port != ip6->udp_port) + continue; + return -1; + } + + elems->ip6 = talloc_realloc(gss, elems->ip6, struct gprs_ns_ie_ip6_elem, + elems->num_ip6+1); + elems->ip6[elems->num_ip6] = *ip6; + elems->num_ip6 += 1; + return 0; +} + +/* Remove a given remote IPv6 element from gprs_sns_state */ +static int remove_ip6_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + unsigned int i; + + for (i = 0; i < elems->num_ip6; i++) { + if (memcmp(&elems->ip6[i], ip6, sizeof(*ip6))) + continue; + /* all array elements < i remain as they are; all > i are shifted left by one */ + memmove(&elems->ip6[i], &elems->ip6[i+1], elems->num_ip6-i-1); + elems->num_ip6 -= 1; + return 0; + } + return -1; +} + +/* update the weights for specified remote IPv6 */ +static int update_ip6_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + unsigned int i; + + for (i = 0; i < elems->num_ip6; i++) { + if (memcmp(&elems->ip6[i].ip_addr, &ip6->ip_addr, sizeof(ip6->ip_addr)) || + elems->ip6[i].udp_port != ip6->udp_port) + continue; + elems->ip6[i].sig_weight = ip6->sig_weight; + elems->ip6[i].data_weight = ip6->data_weight; + return 0; + } + return -1; +} + +static int remove_bind_elem(struct ns2_sns_state *gss, struct ns2_sns_elems *elems, struct ns2_sns_bind *sbind) +{ + struct gprs_ns_ie_ip4_elem ip4; + struct gprs_ns_ie_ip6_elem ip6; + const struct osmo_sockaddr *saddr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + + switch (saddr->u.sa.sa_family) { + case AF_INET: + ip4.ip_addr = saddr->u.sin.sin_addr.s_addr; + ip4.udp_port = saddr->u.sin.sin_port; + ip4.sig_weight = sbind->bind->sns_sig_weight; + ip4.data_weight = sbind->bind->sns_data_weight; + return remove_ip4_elem(gss, elems, &ip4); + case AF_INET6: + memcpy(&ip6.ip_addr, &saddr->u.sin6.sin6_addr, sizeof(struct in6_addr)); + ip6.udp_port = saddr->u.sin.sin_port; + ip6.sig_weight = sbind->bind->sns_sig_weight; + ip6.data_weight = sbind->bind->sns_data_weight; + return remove_ip6_elem(gss, elems, &ip6); + default: + return -1; + } + + return -1; +} + +static int do_sns_change_weight(struct osmo_fsm_inst *fi, const struct gprs_ns_ie_ip4_elem *ip4, const struct gprs_ns_ie_ip6_elem *ip6) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_vc *nsvc; + struct osmo_sockaddr sa = {}; + const struct osmo_sockaddr *remote; + uint8_t new_signal; + uint8_t new_data; + + /* TODO: Upon receiving an SNS-CHANGEWEIGHT PDU, if the resulting sum of the + * signalling weights of all the peer IP endpoints configured for this NSE is + * equal to zero or if the resulting sum of the data weights of all the peer IP + * endpoints configured for this NSE is equal to zero, the BSS/SGSN shall send an + * SNS-ACK PDU with a cause code of "Invalid weights". */ + + if (ip4) { + if (update_ip4_elem(gss, &gss->remote, ip4)) + return -NS_CAUSE_UNKN_IP_EP; + + /* copy over. Both data structures use network byte order */ + sa.u.sin.sin_addr.s_addr = ip4->ip_addr; + sa.u.sin.sin_port = ip4->udp_port; + sa.u.sin.sin_family = AF_INET; + new_signal = ip4->sig_weight; + new_data = ip4->data_weight; + } else if (ip6) { + if (update_ip6_elem(gss, &gss->remote, ip6)) + return -NS_CAUSE_UNKN_IP_EP; + + /* copy over. Both data structures use network byte order */ + sa.u.sin6.sin6_addr = ip6->ip_addr; + sa.u.sin6.sin6_port = ip6->udp_port; + sa.u.sin6.sin6_family = AF_INET6; + new_signal = ip6->sig_weight; + new_data = ip6->data_weight; + } else { + OSMO_ASSERT(false); + } + + llist_for_each_entry(nsvc, &nse->nsvc, list) { + remote = gprs_ns2_ip_vc_remote(nsvc); + /* all nsvc in NSE should be IP/UDP nsvc */ + OSMO_ASSERT(remote); + + if (osmo_sockaddr_cmp(&sa, remote)) + continue; + + LOGPFSML(fi, LOGL_INFO, "CHANGE-WEIGHT NS-VC %s data_weight %u->%u, sig_weight %u->%u\n", + gprs_ns2_ll_str(nsvc), nsvc->data_weight, new_data, + nsvc->sig_weight, new_signal); + + nsvc->data_weight = new_data; + nsvc->sig_weight = new_signal; + } + + return 0; +} + +static int do_sns_delete(struct osmo_fsm_inst *fi, + const struct gprs_ns_ie_ip4_elem *ip4, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_vc *nsvc, *tmp; + const struct osmo_sockaddr *remote; + struct osmo_sockaddr sa = {}; + + if (ip4) { + if (remove_ip4_elem(gss, &gss->remote, ip4) < 0) + return -NS_CAUSE_UNKN_IP_EP; + /* copy over. Both data structures use network byte order */ + sa.u.sin.sin_addr.s_addr = ip4->ip_addr; + sa.u.sin.sin_port = ip4->udp_port; + sa.u.sin.sin_family = AF_INET; + } else if (ip6) { + if (remove_ip6_elem(gss, &gss->remote, ip6)) + return -NS_CAUSE_UNKN_IP_EP; + + /* copy over. Both data structures use network byte order */ + sa.u.sin6.sin6_addr = ip6->ip_addr; + sa.u.sin6.sin6_port = ip6->udp_port; + sa.u.sin6.sin6_family = AF_INET6; + } else { + OSMO_ASSERT(false); + } + + llist_for_each_entry_safe(nsvc, tmp, &nse->nsvc, list) { + remote = gprs_ns2_ip_vc_remote(nsvc); + /* all nsvc in NSE should be IP/UDP nsvc */ + OSMO_ASSERT(remote); + if (osmo_sockaddr_cmp(&sa, remote)) + continue; + + LOGPFSML(fi, LOGL_INFO, "DELETE NS-VC %s\n", gprs_ns2_ll_str(nsvc)); + gprs_ns2_free_nsvc(nsvc); + } + + return 0; +} + +static int do_sns_add(struct osmo_fsm_inst *fi, + const struct gprs_ns_ie_ip4_elem *ip4, + const struct gprs_ns_ie_ip6_elem *ip6) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_vc *nsvc; + int rc = 0; + + /* Upon receiving an SNS-ADD PDU, if the consequent number of IPv4 endpoints + * exceeds the number of IPv4 endpoints supported by the NSE, the NSE shall send + * an SNS-ACK PDU with a cause code set to "Invalid number of IP4 Endpoints". */ + switch (gss->family) { + case AF_INET: + if (gss->remote.num_ip4 >= gss->num_max_ip4_remote) + return -NS_CAUSE_INVAL_NR_NS_VC; + /* TODO: log message duplicate */ + rc = add_ip4_elem(gss, &gss->remote, ip4); + break; + case AF_INET6: + if (gss->remote.num_ip6 >= gss->num_max_ip6_remote) + return -NS_CAUSE_INVAL_NR_NS_VC; + /* TODO: log message duplicate */ + rc = add_ip6_elem(gss, &gss->remote, ip6); + break; + default: + /* the gss->ip is initialized with the bss */ + OSMO_ASSERT(false); + } + + if (rc) + return -NS_CAUSE_PROTO_ERR_UNSPEC; + + /* Upon receiving an SNS-ADD PDU containing an already configured IP endpoint the + * NSE shall send an SNS-ACK PDU with the cause code "Protocol error - + * unspecified" */ + switch (gss->family) { + case AF_INET: + nsvc = nsvc_by_ip4_elem(nse, ip4); + if (nsvc) { + /* the nsvc should be already in sync with the ip4 / ip6 elements */ + return -NS_CAUSE_PROTO_ERR_UNSPEC; + } + + /* TODO: failure case */ + ns2_nsvc_create_ip4(fi, nse, ip4); + break; + case AF_INET6: + nsvc = nsvc_by_ip6_elem(nse, ip6); + if (nsvc) { + /* the nsvc should be already in sync with the ip4 / ip6 elements */ + return -NS_CAUSE_PROTO_ERR_UNSPEC; + } + + /* TODO: failure case */ + ns2_nsvc_create_ip6(fi, nse, ip6); + break; + } + + gprs_ns2_start_alive_all_nsvcs(nse); + + return 0; +} + + +static void ns2_sns_st_bss_unconfigured(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + /* empty state - SNS Select will start by ns2_sns_st_all_action() */ +} + +static void ns2_sns_st_bss_size(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_inst *nsi = nse->nsi; + struct tlv_parsed *tp = NULL; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + switch (event) { + case NS2_SNS_EV_RX_SIZE_ACK: + tp = data; + if (TLVP_VAL_MINLEN(tp, NS_IE_CAUSE, 1)) { + LOGPFSML(fi, LOGL_ERROR, "SNS-SIZE-ACK with cause %s\n", + gprs_ns2_cause_str(*TLVP_VAL(tp, NS_IE_CAUSE))); + /* TODO: What to do? */ + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_CONFIG_BSS, + nsi->timeout[NS_TOUT_TSNS_PROV], 2); + } + break; + default: + OSMO_ASSERT(0); + } +} + +static int ns2_sns_count_num_local_ep(struct osmo_fsm_inst *fi, int ip_proto) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct ns2_sns_bind *sbind; + int count = 0; + + llist_for_each_entry(sbind, &gss->binds, list) { + const struct osmo_sockaddr *sa = gprs_ns2_ip_bind_sockaddr(sbind->bind); + if (!sa) + continue; + + switch (ip_proto) { + case AF_INET: + if (sa->u.sas.ss_family == AF_INET) + count++; + break; + case AF_INET6: + if (sa->u.sas.ss_family == AF_INET6) + count++; + break; + } + } + return count; +} + +static int ns2_sns_copy_local_endpoints(struct ns2_sns_state *gss) +{ + switch (gss->family) { + case AF_INET: + gss->local_procedure.ip4 = talloc_realloc(gss, gss->local_procedure.ip4, struct gprs_ns_ie_ip4_elem, + gss->local.num_ip4); + if (!gss->local_procedure.ip4) + return -ENOMEM; + + gss->local_procedure.num_ip4 = gss->local.num_ip4; + memcpy(gss->local_procedure.ip4, gss->local.ip4, + sizeof(struct gprs_ns_ie_ip4_elem) * gss->local.num_ip4); + break; + case AF_INET6: + gss->local_procedure.ip6 = talloc_realloc(gss, gss->local_procedure.ip6, struct gprs_ns_ie_ip6_elem, + gss->local.num_ip6); + if (!gss->local_procedure.ip6) + return -ENOMEM; + + gss->local_procedure.num_ip6 = gss->local.num_ip6; + memcpy(gss->local_procedure.ip6, gss->local.ip6, + sizeof(struct gprs_ns_ie_ip6_elem) * gss->local.num_ip6); + break; + default: + OSMO_ASSERT(0); + } + + return 0; +} + +static void ns2_sns_compute_local_ep_from_binds(struct osmo_fsm_inst *fi) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns_ie_ip4_elem *ip4_elems; + struct gprs_ns_ie_ip6_elem *ip6_elems; + struct gprs_ns2_vc_bind *bind; + struct ns2_sns_bind *sbind; + const struct osmo_sockaddr *remote; + const struct osmo_sockaddr *sa; + struct osmo_sockaddr local; + int count; + + ns2_clear_elems(&gss->local); + + /* no initial available */ + if (gss->role == GPRS_SNS_ROLE_BSS) { + if (!gss->initial) + return; + remote = &gss->initial->saddr; + } else + remote = gprs_ns2_ip_vc_remote(gss->sns_nsvc); + + /* count how many bindings are available (only UDP binds) */ + count = llist_count(&gss->binds); + if (count == 0) { + LOGPFSML(fi, LOGL_ERROR, "No local binds for this NSE -> cannot determine IP endpoints\n"); + return; + } + + switch (gss->family) { + case AF_INET: + ip4_elems = talloc_realloc(fi, gss->local.ip4, struct gprs_ns_ie_ip4_elem, count); + if (!ip4_elems) + return; + + gss->local.ip4 = ip4_elems; + llist_for_each_entry(sbind, &gss->binds, list) { + bind = sbind->bind; + sa = gprs_ns2_ip_bind_sockaddr(bind); + if (!sa) + continue; + + if (sa->u.sas.ss_family != AF_INET) + continue; + + /* check if this is an specific bind */ + if (sa->u.sin.sin_addr.s_addr == 0) { + if (osmo_sockaddr_local_ip(&local, remote)) + continue; + + ip4_elems->ip_addr = local.u.sin.sin_addr.s_addr; + } else { + ip4_elems->ip_addr = sa->u.sin.sin_addr.s_addr; + } + + ip4_elems->udp_port = sa->u.sin.sin_port; + ip4_elems->sig_weight = bind->sns_sig_weight; + ip4_elems->data_weight = bind->sns_data_weight; + ip4_elems++; + } + + gss->local.num_ip4 = count; + gss->num_max_nsvcs = OSMO_MAX(gss->num_max_ip4_remote * gss->local.num_ip4, 8); + break; + case AF_INET6: + /* IPv6 */ + ip6_elems = talloc_realloc(fi, gss->local.ip6, struct gprs_ns_ie_ip6_elem, count); + if (!ip6_elems) + return; + + gss->local.ip6 = ip6_elems; + + llist_for_each_entry(sbind, &gss->binds, list) { + bind = sbind->bind; + sa = gprs_ns2_ip_bind_sockaddr(bind); + if (!sa) + continue; + + if (sa->u.sas.ss_family != AF_INET6) + continue; + + /* check if this is an specific bind */ + if (IN6_IS_ADDR_UNSPECIFIED(&sa->u.sin6.sin6_addr)) { + if (osmo_sockaddr_local_ip(&local, remote)) + continue; + + ip6_elems->ip_addr = local.u.sin6.sin6_addr; + } else { + ip6_elems->ip_addr = sa->u.sin6.sin6_addr; + } + + ip6_elems->udp_port = sa->u.sin.sin_port; + ip6_elems->sig_weight = bind->sns_sig_weight; + ip6_elems->data_weight = bind->sns_data_weight; + + ip6_elems++; + } + gss->local.num_ip6 = count; + gss->num_max_nsvcs = OSMO_MAX(gss->num_max_ip6_remote * gss->local.num_ip6, 8); + break; + } + + ns2_sns_copy_local_endpoints(gss); +} + +static void ns2_sns_choose_next_bind(struct ns2_sns_state *gss) +{ + /* take the first bind or take the next bind */ + if (!gss->initial_bind || gss->initial_bind->list.next == &gss->binds) + gss->initial_bind = llist_first_entry_or_null(&gss->binds, struct ns2_sns_bind, list); + else + gss->initial_bind = llist_entry(gss->initial_bind->list.next, struct ns2_sns_bind, list); +} + +/* setup all dynamic SNS settings, create a new nsvc and send the SIZE */ +static void ns2_sns_st_bss_size_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + /* on a generic failure, the timer callback will recover */ + if (old_state != GPRS_SNS_ST_UNCONFIGURED) + ns2_prim_status_ind(gss->nse, NULL, 0, GPRS_NS2_AFF_CAUSE_SNS_FAILURE); + if (old_state != GPRS_SNS_ST_BSS_SIZE) + gss->N = 0; + + ns2_clear_procedures(gss); + gss->alive = false; + + ns2_sns_compute_local_ep_from_binds(fi); + ns2_sns_choose_next_bind(gss); + + /* setup the NSVC */ + if (!gss->sns_nsvc) { + struct gprs_ns2_vc_bind *bind = gss->initial_bind->bind; + struct osmo_sockaddr *remote = &gss->initial->saddr; + gss->sns_nsvc = ns2_ip_bind_connect(bind, gss->nse, remote); + if (!gss->sns_nsvc) + return; + /* A pre-configured endpoint shall not be used for NSE data or signalling traffic + * (with the exception of Size and Configuration procedures) unless it is configured + * by the SGSN using the auto-configuration procedures */ + gss->sns_nsvc->sns_only = true; + } + + if (gss->num_max_ip4_remote > 0) + ns2_tx_sns_size(gss->sns_nsvc, true, gss->num_max_nsvcs, gss->local.num_ip4, -1); + else + ns2_tx_sns_size(gss->sns_nsvc, true, gss->num_max_nsvcs, -1, gss->local.num_ip6); +} + +static void ns2_sns_st_bss_config_bss(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct tlv_parsed *tp = NULL; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + switch (event) { + case NS2_SNS_EV_RX_CONFIG_ACK: + tp = (struct tlv_parsed *) data; + if (TLVP_VAL_MINLEN(tp, NS_IE_CAUSE, 1)) { + LOGPFSML(fi, LOGL_ERROR, "SNS-CONFIG-ACK with cause %s\n", + gprs_ns2_cause_str(*TLVP_VAL(tp, NS_IE_CAUSE))); + /* TODO: What to do? */ + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_CONFIG_SGSN, nse->nsi->timeout[NS_TOUT_TSNS_PROV], 3); + } + break; + default: + OSMO_ASSERT(0); + } +} + +static void ns2_sns_st_bss_config_bss_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + if (old_state != GPRS_SNS_ST_BSS_CONFIG_BSS) + gss->N = 0; + + /* Transmit SNS-CONFIG */ + switch (gss->family) { + case AF_INET: + ns2_tx_sns_config(gss->sns_nsvc, true, + gss->local.ip4, gss->local.num_ip4, + NULL, 0); + break; + case AF_INET6: + ns2_tx_sns_config(gss->sns_nsvc, true, + NULL, 0, + gss->local.ip6, gss->local.num_ip6); + break; + } +} + +/* calculate the timeout of the configured state. the configured + * state will fail if not at least one NS-VC is alive within X second. + */ +static inline int ns_sns_configured_timeout(struct osmo_fsm_inst *fi) +{ + int secs; + struct gprs_ns2_inst *nsi = nse_inst_from_fi(fi)->nsi; + secs = nsi->timeout[NS_TOUT_TNS_ALIVE] * nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES]; + secs += nsi->timeout[NS_TOUT_TNS_TEST]; + + return secs; +} + +/* append the remote endpoints from the parsed TLV array to the ns2_sns_state */ +static int ns_sns_append_remote_eps(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + if (TLVP_PRESENT(tp, NS_IE_IPv4_LIST)) { + const struct gprs_ns_ie_ip4_elem *v4_list; + unsigned int num_v4; + v4_list = (const struct gprs_ns_ie_ip4_elem *) TLVP_VAL(tp, NS_IE_IPv4_LIST); + num_v4 = TLVP_LEN(tp, NS_IE_IPv4_LIST) / sizeof(*v4_list); + + if (num_v4 && gss->remote.ip6) + return -NS_CAUSE_INVAL_NR_IPv4_EP; + + /* realloc to the new size */ + gss->remote.ip4 = talloc_realloc(gss, gss->remote.ip4, + struct gprs_ns_ie_ip4_elem, + gss->remote.num_ip4 + num_v4); + /* append the new entries to the end of the list */ + memcpy(&gss->remote.ip4[gss->remote.num_ip4], v4_list, num_v4*sizeof(*v4_list)); + gss->remote.num_ip4 += num_v4; + + LOGPFSML(fi, LOGL_INFO, "Rx SNS-CONFIG: Remote IPv4 list now %u entries\n", + gss->remote.num_ip4); + } + + if (TLVP_PRESENT(tp, NS_IE_IPv6_LIST)) { + const struct gprs_ns_ie_ip6_elem *v6_list; + unsigned int num_v6; + v6_list = (const struct gprs_ns_ie_ip6_elem *) TLVP_VAL(tp, NS_IE_IPv6_LIST); + num_v6 = TLVP_LEN(tp, NS_IE_IPv6_LIST) / sizeof(*v6_list); + + if (num_v6 && gss->remote.ip4) + return -NS_CAUSE_INVAL_NR_IPv6_EP; + + /* realloc to the new size */ + gss->remote.ip6 = talloc_realloc(gss, gss->remote.ip6, + struct gprs_ns_ie_ip6_elem, + gss->remote.num_ip6 + num_v6); + /* append the new entries to the end of the list */ + memcpy(&gss->remote.ip6[gss->remote.num_ip6], v6_list, num_v6*sizeof(*v6_list)); + gss->remote.num_ip6 += num_v6; + + LOGPFSML(fi, LOGL_INFO, "Rx SNS-CONFIG: Remote IPv6 list now %d entries\n", + gss->remote.num_ip6); + } + + return 0; +} + +static void ns2_sns_st_bss_config_sgsn_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + if (old_state != GPRS_SNS_ST_BSS_CONFIG_SGSN) + gss->N = 0; +} + +static void ns2_sns_st_bss_config_sgsn(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + uint8_t cause; + int rc; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_BSS); + + switch (event) { + case NS2_SNS_EV_RX_CONFIG_END: + case NS2_SNS_EV_RX_CONFIG: + rc = ns_sns_append_remote_eps(fi, data); + if (rc < 0) { + cause = -rc; + ns2_tx_sns_config_ack(gss->sns_nsvc, &cause); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0); + return; + } + if (event == NS2_SNS_EV_RX_CONFIG_END) { + /* check if sum of data / sig weights == 0 */ + if (ip46_weight_sum_data(&gss->remote) == 0 || ip46_weight_sum_sig(&gss->remote) == 0) { + cause = NS_CAUSE_INVAL_WEIGH; + ns2_tx_sns_config_ack(gss->sns_nsvc, &cause); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0); + return; + } + create_missing_nsvcs(fi); + ns2_tx_sns_config_ack(gss->sns_nsvc, NULL); + /* start the test procedure on ALL NSVCs! */ + gprs_ns2_start_alive_all_nsvcs(nse); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_CONFIGURED, 0, 0); + } else { + /* just send CONFIG-ACK */ + ns2_tx_sns_config_ack(gss->sns_nsvc, NULL); + osmo_timer_schedule(&fi->timer, nse->nsi->timeout[NS_TOUT_TSNS_PROV], 0); + } + break; + default: + OSMO_ASSERT(0); + } +} + +/* called when receiving NS2_SNS_EV_RX_ADD in state configure */ +static void ns2_sns_st_configured_add(struct osmo_fsm_inst *fi, + struct ns2_sns_state *gss, + struct tlv_parsed *tp) +{ + const struct gprs_ns_ie_ip4_elem *v4_list = NULL; + const struct gprs_ns_ie_ip6_elem *v6_list = NULL; + int num_v4 = 0, num_v6 = 0; + uint8_t trans_id, cause = 0xff; + unsigned int i; + int rc = 0; + + /* TODO: refactor EV_ADD/CHANGE/REMOVE by + * check uniqueness within the lists (no doublicate entries) + * check not-known-by-us and sent back a list of unknown/known values + * (abnormal behaviour according to 48.016) + */ + + trans_id = *TLVP_VAL(tp, NS_IE_TRANS_ID); + if (gss->family == AF_INET) { + if (!TLVP_PRESENT(tp, NS_IE_IPv4_LIST)) { + cause = NS_CAUSE_INVAL_NR_IPv4_EP; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + + v4_list = (const struct gprs_ns_ie_ip4_elem *) TLVP_VAL(tp, NS_IE_IPv4_LIST); + num_v4 = TLVP_LEN(tp, NS_IE_IPv4_LIST) / sizeof(*v4_list); + for (i = 0; i < num_v4; i++) { + unsigned int j; + rc = do_sns_add(fi, &v4_list[i], NULL); + if (rc < 0) { + /* rollback/undo to restore previous state */ + for (j = 0; j < i; j++) + do_sns_delete(fi, &v4_list[j], NULL); + cause = -rc; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + break; + } + } + } else { /* IPv6 */ + if (!TLVP_PRESENT(tp, NS_IE_IPv6_LIST)) { + cause = NS_CAUSE_INVAL_NR_IPv6_EP; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + + v6_list = (const struct gprs_ns_ie_ip6_elem *) TLVP_VAL(tp, NS_IE_IPv6_LIST); + num_v6 = TLVP_LEN(tp, NS_IE_IPv6_LIST) / sizeof(*v6_list); + for (i = 0; i < num_v6; i++) { + unsigned int j; + rc = do_sns_add(fi, NULL, &v6_list[i]); + if (rc < 0) { + /* rollback/undo to restore previous state */ + for (j = 0; j < i; j++) + do_sns_delete(fi, NULL, &v6_list[j]); + cause = -rc; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + break; + } + } + } + + /* TODO: correct behaviour is to answer to the *same* NSVC from which the SNS_ADD was received */ + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, NULL, v4_list, num_v4, v6_list, num_v6); +} + +static void ns2_sns_st_configured_delete(struct osmo_fsm_inst *fi, + struct ns2_sns_state *gss, + struct tlv_parsed *tp) +{ + const struct gprs_ns_ie_ip4_elem *v4_list = NULL; + const struct gprs_ns_ie_ip6_elem *v6_list = NULL; + int num_v4 = 0, num_v6 = 0; + uint8_t trans_id, cause = 0xff; + unsigned int i; + int rc = 0; + + /* TODO: split up delete into v4 + v6 + * TODO: check if IPv4_LIST or IP_ADDR(v4) is present on IPv6 and vice versa + * TODO: check if IPv4_LIST/IPv6_LIST and IP_ADDR is present at the same time + */ + trans_id = *TLVP_VAL(tp, NS_IE_TRANS_ID); + if (gss->family == AF_INET) { + if (TLVP_PRESENT(tp, NS_IE_IPv4_LIST)) { + v4_list = (const struct gprs_ns_ie_ip4_elem *) TLVP_VAL(tp, NS_IE_IPv4_LIST); + num_v4 = TLVP_LEN(tp, NS_IE_IPv4_LIST) / sizeof(*v4_list); + for ( i = 0; i < num_v4; i++) { + rc = do_sns_delete(fi, &v4_list[i], NULL); + if (rc < 0) { + cause = -rc; + /* continue to delete others */ + } + } + if (cause != 0xff) { + /* TODO: create list of not-deleted and return it */ + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + + } else if (TLVP_PRESENT(tp, NS_IE_IP_ADDR) && TLVP_LEN(tp, NS_IE_IP_ADDR) == 5) { + /* delete all NS-VCs for given IPv4 address */ + const uint8_t *ie = TLVP_VAL(tp, NS_IE_IP_ADDR); + struct gprs_ns_ie_ip4_elem *ip4_remote; + uint32_t ip_addr = *(uint32_t *)(ie+1); + if (ie[0] != 0x01) { /* Address Type != IPv4 */ + cause = NS_CAUSE_UNKN_IP_ADDR; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + /* make a copy as do_sns_delete() will change the array underneath us */ + ip4_remote = talloc_memdup(fi, gss->remote.ip4, + gss->remote.num_ip4 * sizeof(*v4_list)); + for (i = 0; i < gss->remote.num_ip4; i++) { + if (ip4_remote[i].ip_addr == ip_addr) { + rc = do_sns_delete(fi, &ip4_remote[i], NULL); + if (rc < 0) { + cause = -rc; + /* continue to delete others */ + } + } + } + talloc_free(ip4_remote); + if (cause != 0xff) { + /* TODO: create list of not-deleted and return it */ + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else { + cause = NS_CAUSE_INVAL_NR_IPv4_EP; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else { /* IPv6 */ + if (TLVP_PRESENT(tp, NS_IE_IPv6_LIST)) { + v6_list = (const struct gprs_ns_ie_ip6_elem *) TLVP_VAL(tp, NS_IE_IPv6_LIST); + num_v6 = TLVP_LEN(tp, NS_IE_IPv6_LIST) / sizeof(*v6_list); + for (i = 0; i < num_v6; i++) { + rc = do_sns_delete(fi, NULL, &v6_list[i]); + if (rc < 0) { + cause = -rc; + /* continue to delete others */ + } + } + if (cause != 0xff) { + /* TODO: create list of not-deleted and return it */ + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else if (TLVP_PRES_LEN(tp, NS_IE_IP_ADDR, 17)) { + /* delete all NS-VCs for given IPv4 address */ + const uint8_t *ie = TLVP_VAL(tp, NS_IE_IP_ADDR); + struct gprs_ns_ie_ip6_elem *ip6_remote; + struct in6_addr ip6_addr; + unsigned int i; + if (ie[0] != 0x02) { /* Address Type != IPv6 */ + cause = NS_CAUSE_UNKN_IP_ADDR; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + memcpy(&ip6_addr, (ie+1), sizeof(struct in6_addr)); + /* make a copy as do_sns_delete() will change the array underneath us */ + ip6_remote = talloc_memdup(fi, gss->remote.ip6, + gss->remote.num_ip6 * sizeof(*v4_list)); + for (i = 0; i < gss->remote.num_ip6; i++) { + if (!memcmp(&ip6_remote[i].ip_addr, &ip6_addr, sizeof(struct in6_addr))) { + rc = do_sns_delete(fi, NULL, &ip6_remote[i]); + if (rc < 0) { + cause = -rc; + /* continue to delete others */ + } + } + } + + talloc_free(ip6_remote); + if (cause != 0xff) { + /* TODO: create list of not-deleted and return it */ + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else { + cause = NS_CAUSE_INVAL_NR_IPv6_EP; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, NULL, v4_list, num_v4, v6_list, num_v6); +} + +static void ns2_sns_st_configured_change(struct osmo_fsm_inst *fi, + struct ns2_sns_state *gss, + struct tlv_parsed *tp) +{ + const struct gprs_ns_ie_ip4_elem *v4_list = NULL; + const struct gprs_ns_ie_ip6_elem *v6_list = NULL; + int num_v4 = 0, num_v6 = 0; + uint8_t trans_id, cause = 0xff; + int rc = 0; + unsigned int i; + + trans_id = *TLVP_VAL(tp, NS_IE_TRANS_ID); + if (TLVP_PRESENT(tp, NS_IE_IPv4_LIST)) { + v4_list = (const struct gprs_ns_ie_ip4_elem *) TLVP_VAL(tp, NS_IE_IPv4_LIST); + num_v4 = TLVP_LEN(tp, NS_IE_IPv4_LIST) / sizeof(*v4_list); + for (i = 0; i < num_v4; i++) { + rc = do_sns_change_weight(fi, &v4_list[i], NULL); + if (rc < 0) { + cause = -rc; + /* continue to others */ + } + } + if (cause != 0xff) { + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else if (TLVP_PRESENT(tp, NS_IE_IPv6_LIST)) { + v6_list = (const struct gprs_ns_ie_ip6_elem *) TLVP_VAL(tp, NS_IE_IPv6_LIST); + num_v6 = TLVP_LEN(tp, NS_IE_IPv6_LIST) / sizeof(*v6_list); + for (i = 0; i < num_v6; i++) { + rc = do_sns_change_weight(fi, NULL, &v6_list[i]); + if (rc < 0) { + cause = -rc; + /* continue to others */ + } + } + if (cause != 0xff) { + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + } else { + cause = NS_CAUSE_INVAL_NR_IPv4_EP; + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, &cause, NULL, 0, NULL, 0); + return; + } + ns2_tx_sns_ack(gss->sns_nsvc, trans_id, NULL, v4_list, num_v4, v6_list, num_v6); +} + +static void ns2_sns_st_configured(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct tlv_parsed *tp = data; + + switch (event) { + case NS2_SNS_EV_RX_ADD: + ns2_sns_st_configured_add(fi, gss, tp); + break; + case NS2_SNS_EV_RX_DELETE: + ns2_sns_st_configured_delete(fi, gss, tp); + break; + case NS2_SNS_EV_RX_CHANGE_WEIGHT: + ns2_sns_st_configured_change(fi, gss, tp); + break; + case NS2_SNS_EV_REQ_NSVC_ALIVE: + osmo_timer_del(&fi->timer); + break; + } +} + +static void ns2_sns_st_configured_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc *nsvc; + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + /* NS-VC status updates are only parsed in ST_CONFIGURED. + * Do an initial check if there are any nsvc alive atm */ + llist_for_each_entry(nsvc, &nse->nsvc, list) { + if (ns2_vc_is_unblocked(nsvc)) { + gss->alive = true; + osmo_timer_del(&fi->timer); + break; + } + } + + /* remove the initial NSVC if the NSVC isn't part of the configuration */ + if (gss->sns_nsvc->sns_only) + gprs_ns2_free_nsvc(gss->sns_nsvc); + + if (old_state != GPRS_SNS_ST_LOCAL_PROCEDURE) + ns2_prim_status_ind(nse, NULL, 0, GPRS_NS2_AFF_CAUSE_SNS_CONFIGURED); + + if (!llist_empty(&gss->procedures)) { + osmo_fsm_inst_state_chg(gss->nse->bss_sns_fi, GPRS_SNS_ST_LOCAL_PROCEDURE, + gss->nse->nsi->timeout[NS_TOUT_TSNS_PROV], 5); + } +} + +static void ns2_sns_st_local_procedure_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + /* check if resend or not */ + if (!gss->current_procedure) { + /* take next procedure */ + gss->current_procedure = llist_first_entry_or_null(&gss->procedures, + struct ns2_sns_procedure, list); + if (!gss->current_procedure) { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_CONFIGURED, 0, 0); + return; + } + gss->N = 0; + gss->current_procedure->running = true; + gss->current_procedure->trans_id = ++gss->trans_id; + if (gss->trans_id == 0) + gss->trans_id = gss->current_procedure->trans_id = 1; + + } + + /* also takes care of retransmitting */ + switch (gss->current_procedure->procedure) { + case SNS_PROC_ADD: + if (gss->family == AF_INET) + ns2_tx_sns_add(gss->sns_nsvc, gss->current_procedure->trans_id, &gss->current_procedure->ip4, 1, NULL, 0); + else + ns2_tx_sns_add(gss->sns_nsvc, gss->current_procedure->trans_id, NULL, 0, &gss->current_procedure->ip6, 1); + break; + case SNS_PROC_CHANGE_WEIGHT: + if (gss->family == AF_INET) + ns2_tx_sns_change_weight(gss->sns_nsvc, gss->current_procedure->trans_id, &gss->current_procedure->ip4, 1, NULL, 0); + else + ns2_tx_sns_change_weight(gss->sns_nsvc, gss->current_procedure->trans_id, NULL, 0, &gss->current_procedure->ip6, 1); + break; + case SNS_PROC_DEL: + if (gss->family == AF_INET) + ns2_tx_sns_del(gss->sns_nsvc, gss->current_procedure->trans_id, &gss->current_procedure->ip4, 1, NULL, 0); + else + ns2_tx_sns_del(gss->sns_nsvc, gss->current_procedure->trans_id, NULL, 0, &gss->current_procedure->ip6, 1); + break; + default: + break; + } +} + +static void create_nsvc_for_new_sbind(struct ns2_sns_state *gss, struct ns2_sns_bind *sbind) +{ + struct gprs_ns2_nse *nse = gss->nse; + struct gprs_ns2_vc_bind *bind = sbind->bind; + struct gprs_ns2_vc *nsvc; + struct osmo_sockaddr remote = { }; + unsigned int i; + + /* iterate over all remote IPv4 endpoints */ + for (i = 0; i < gss->remote.num_ip4; i++) { + const struct gprs_ns_ie_ip4_elem *ip4 = &gss->remote.ip4[i]; + + remote.u.sin.sin_family = AF_INET; + remote.u.sin.sin_addr.s_addr = ip4->ip_addr; + remote.u.sin.sin_port = ip4->udp_port; + /* we only care about UDP binds */ + if (bind->ll != GPRS_NS2_LL_UDP) + continue; + + nsvc = nsvc_for_bind_and_remote(nse, bind, &remote); + if (!nsvc) { + nsvc = gprs_ns2_ip_connect_inactive(bind, &remote, nse, 0); + if (!nsvc) { + /* TODO: add to a list to send back a NS-STATUS */ + continue; + } + } + + /* update data / signalling weight */ + nsvc->data_weight = ip4->data_weight; + nsvc->sig_weight = ip4->sig_weight; + nsvc->sns_only = false; + } + + /* iterate over all remote IPv4 endpoints */ + for (i = 0; i < gss->remote.num_ip6; i++) { + const struct gprs_ns_ie_ip6_elem *ip6 = &gss->remote.ip6[i]; + + remote.u.sin6.sin6_family = AF_INET6; + remote.u.sin6.sin6_addr = ip6->ip_addr; + remote.u.sin6.sin6_port = ip6->udp_port; + + /* we only care about UDP binds */ + nsvc = nsvc_for_bind_and_remote(nse, bind, &remote); + if (!nsvc) { + nsvc = gprs_ns2_ip_connect_inactive(bind, &remote, nse, 0); + if (!nsvc) { + /* TODO: add to a list to send back a NS-STATUS */ + continue; + } + } + + /* update data / signalling weight */ + nsvc->data_weight = ip6->data_weight; + nsvc->sig_weight = ip6->sig_weight; + nsvc->sns_only = false; + } +} + +static void ns2_sns_st_local_procedure(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns_ie_ip4_elem *ip4, *proc4; + struct gprs_ns_ie_ip6_elem *ip6, *proc6; + struct tlv_parsed *tp = data; + uint8_t trans_id; + uint8_t cause; + + switch (event) { + case NS2_SNS_EV_RX_ADD: + ns2_sns_st_configured_add(fi, gss, tp); + break; + case NS2_SNS_EV_RX_DELETE: + ns2_sns_st_configured_delete(fi, gss, tp); + break; + case NS2_SNS_EV_RX_CHANGE_WEIGHT: + ns2_sns_st_configured_change(fi, gss, tp); + break; + case NS2_SNS_EV_RX_ACK: + /* presence of trans_id is already checked here */ + trans_id = tlvp_val8(tp, NS_IE_TRANS_ID, 0); + if (trans_id != gss->current_procedure->trans_id) { + LOGPFSML(fi, LOGL_INFO, "NSEI=%u Rx SNS ACK with invalid transaction id %d. Valid %d\n", + nse->nsei, trans_id, gss->current_procedure->trans_id); + break; + } + + if (TLVP_PRESENT(tp, NS_IE_CAUSE)) { + /* what happend on error cause? return to size? */ + cause = tlvp_val8(tp, NS_IE_CAUSE, 0); + LOGPFSML(fi, LOGL_ERROR, "NSEI=%u Rx SNS ACK trans %d with cause code %d.\n", + nse->nsei, trans_id, cause); + sns_failed(fi, NULL); + break; + } + + switch (gss->current_procedure->procedure) { + case SNS_PROC_ADD: + switch (gss->family) { + case AF_INET: + add_ip4_elem(gss, &gss->local, &gss->current_procedure->ip4); + break; + case AF_INET6: + add_ip6_elem(gss, &gss->local, &gss->current_procedure->ip6); + break; + } + /* the sbind can be NULL if the bind has been released by del_bind */ + if (gss->current_procedure->sbind) { + create_nsvc_for_new_sbind(gss, gss->current_procedure->sbind); + gprs_ns2_start_alive_all_nsvcs(nse); + } + break; + case SNS_PROC_CHANGE_WEIGHT: + switch (gss->family) { + case AF_INET: + proc4 = &gss->current_procedure->ip4; + for (unsigned int i=0; i<gss->local.num_ip4; i++) { + ip4 = &gss->local.ip4[i]; + if (ip4->ip_addr != proc4->ip_addr || + ip4->udp_port != proc4->udp_port) + continue; + ip4->sig_weight = proc4->sig_weight; + ip4->data_weight = proc4->data_weight; + break; + } + break; + case AF_INET6: + proc6 = &gss->current_procedure->ip6; + for (unsigned int i=0; i<gss->local.num_ip6; i++) { + ip6 = &gss->local.ip6[i]; + if (memcmp(&ip6->ip_addr, &proc6->ip_addr, sizeof(proc6->ip_addr)) || + ip6->udp_port != proc6->udp_port) { + continue; + } + ip6->sig_weight = proc6->sig_weight; + ip6->data_weight = proc6->data_weight; + break; + } + break; + default: + OSMO_ASSERT(0); + } + break; + case SNS_PROC_DEL: + switch (gss->family) { + case AF_INET: + remove_ip4_elem(gss, &gss->local, &gss->current_procedure->ip4); + break; + case AF_INET6: + remove_ip6_elem(gss, &gss->local, &gss->current_procedure->ip6); + break; + } + break; + default: + break; + } + + llist_del(&gss->current_procedure->list); + talloc_free(gss->current_procedure); + gss->current_procedure = NULL; + + if (llist_empty(&gss->procedures)) + osmo_fsm_inst_state_chg(gss->nse->bss_sns_fi, GPRS_SNS_ST_CONFIGURED, + 0, 0); + else + osmo_fsm_inst_state_chg(gss->nse->bss_sns_fi, GPRS_SNS_ST_LOCAL_PROCEDURE, + gss->nse->nsi->timeout[NS_TOUT_TSNS_PROV], 5); + break; + } +} + +static const struct osmo_fsm_state ns2_sns_bss_states[] = { + [GPRS_SNS_ST_UNCONFIGURED] = { + .in_event_mask = 0, /* handled by all_state_action */ + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_SIZE), + .name = "UNCONFIGURED", + .action = ns2_sns_st_bss_unconfigured, + }, + [GPRS_SNS_ST_BSS_SIZE] = { + .in_event_mask = S(NS2_SNS_EV_RX_SIZE_ACK), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_SIZE) | + S(GPRS_SNS_ST_BSS_CONFIG_BSS), + .name = "BSS_SIZE", + .action = ns2_sns_st_bss_size, + .onenter = ns2_sns_st_bss_size_onenter, + }, + [GPRS_SNS_ST_BSS_CONFIG_BSS] = { + .in_event_mask = S(NS2_SNS_EV_RX_CONFIG_ACK), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_CONFIG_BSS) | + S(GPRS_SNS_ST_BSS_CONFIG_SGSN) | + S(GPRS_SNS_ST_BSS_SIZE), + .name = "BSS_CONFIG_BSS", + .action = ns2_sns_st_bss_config_bss, + .onenter = ns2_sns_st_bss_config_bss_onenter, + }, + [GPRS_SNS_ST_BSS_CONFIG_SGSN] = { + .in_event_mask = S(NS2_SNS_EV_RX_CONFIG) | + S(NS2_SNS_EV_RX_CONFIG_END), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_CONFIG_SGSN) | + S(GPRS_SNS_ST_CONFIGURED) | + S(GPRS_SNS_ST_BSS_SIZE), + .name = "BSS_CONFIG_SGSN", + .action = ns2_sns_st_bss_config_sgsn, + .onenter = ns2_sns_st_bss_config_sgsn_onenter, + }, + [GPRS_SNS_ST_CONFIGURED] = { + .in_event_mask = S(NS2_SNS_EV_RX_ADD) | + S(NS2_SNS_EV_RX_DELETE) | + S(NS2_SNS_EV_RX_CHANGE_WEIGHT) | + S(NS2_SNS_EV_REQ_NSVC_ALIVE), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_SIZE) | + S(GPRS_SNS_ST_LOCAL_PROCEDURE), + .name = "CONFIGURED", + .action = ns2_sns_st_configured, + .onenter = ns2_sns_st_configured_onenter, + }, + [GPRS_SNS_ST_LOCAL_PROCEDURE] = { + .in_event_mask = S(NS2_SNS_EV_RX_ADD) | + S(NS2_SNS_EV_RX_DELETE) | + S(NS2_SNS_EV_RX_CHANGE_WEIGHT) | + S(NS2_SNS_EV_RX_ACK) | + S(NS2_SNS_EV_REQ_NSVC_ALIVE), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_BSS_SIZE) | + S(GPRS_SNS_ST_CONFIGURED) | + S(GPRS_SNS_ST_LOCAL_PROCEDURE), + .name = "LOCAL_PROCEDURE", + .action = ns2_sns_st_local_procedure, + .onenter = ns2_sns_st_local_procedure_onenter, + }, + +}; + +static int ns2_sns_fsm_bss_timer_cb(struct osmo_fsm_inst *fi) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_inst *nsi = nse->nsi; + + gss->N++; + switch (fi->T) { + case 1: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_SIZE_RETRIES]) { + sns_failed(fi, "Size retries failed. Selecting next IP-SNS endpoint."); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_SIZE, nsi->timeout[NS_TOUT_TSNS_PROV], 1); + } + break; + case 2: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_CONFIG_RETRIES]) { + sns_failed(fi, "BSS Config retries failed. Selecting next IP-SNS endpoint"); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_CONFIG_BSS, nsi->timeout[NS_TOUT_TSNS_PROV], 2); + } + break; + case 3: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_CONFIG_RETRIES]) { + sns_failed(fi, "SGSN Config retries failed. Selecting next IP-SNS endpoint."); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_CONFIG_SGSN, nsi->timeout[NS_TOUT_TSNS_PROV], 3); + } + break; + case 4: + sns_failed(fi, "Config succeeded but no NS-VC came online. Selecting next IP-SNS endpoint."); + break; + case 5: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_CONFIG_RETRIES]) { + sns_failed(fi, "SNS Procedure retries failed."); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_LOCAL_PROCEDURE, nsi->timeout[NS_TOUT_TSNS_PROV], 5); + } + break; + } + return 0; +} + +static struct gprs_ns_ie_ip4_elem *ns2_get_sbind_ip4_entry(struct ns2_sns_state *gss, + struct ns2_sns_bind *sbind, + struct ns2_sns_elems *endpoints) +{ + const struct osmo_sockaddr *addr; + struct gprs_ns_ie_ip4_elem *ip4; + + if (gss->family != AF_INET) + return NULL; + + addr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + if (addr->u.sa.sa_family != AF_INET) + return NULL; + + for (unsigned int i=0; i<endpoints->num_ip4; i++) { + ip4 = &endpoints->ip4[i]; + if (ip4->ip_addr == addr->u.sin.sin_addr.s_addr && + ip4->udp_port == addr->u.sin.sin_port) + return ip4; + } + + return NULL; +} + +static struct gprs_ns_ie_ip6_elem *ns2_get_sbind_ip6_entry(struct ns2_sns_state *gss, + struct ns2_sns_bind *sbind, + struct ns2_sns_elems *endpoints) +{ + const struct osmo_sockaddr *addr; + struct gprs_ns_ie_ip6_elem *ip6; + + if (gss->family != AF_INET6) + return NULL; + + addr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + if (addr->u.sa.sa_family != AF_INET6) + return NULL; + + for (unsigned int i=0; i<endpoints->num_ip6; i++) { + ip6 = &endpoints->ip6[i]; + if (memcmp(&ip6->ip_addr, &addr->u.sin6.sin6_addr, sizeof(ip6->ip_addr)) || + ip6->udp_port != addr->u.sin6.sin6_port) + return ip6; + } + + return NULL; +} + +/* return != 0 if the resulting weight is invalid. return 1 if sbind doesn't have an entry */ +static int ns2_update_weight_entry(struct ns2_sns_state *gss, struct ns2_sns_bind *sbind, + struct ns2_sns_elems *endpoints) +{ + struct gprs_ns_ie_ip4_elem *ip4; + struct gprs_ns_ie_ip6_elem *ip6; + + switch (gss->family) { + case AF_INET: + ip4 = ns2_get_sbind_ip4_entry(gss, sbind, endpoints); + if (!ip4) + return 1; + ip4->sig_weight = sbind->bind->sns_sig_weight; + ip4->data_weight = sbind->bind->sns_data_weight; + return (ip4_weight_sum_sig(endpoints) != 0 && ip4_weight_sum_data(endpoints) != 0); + break; + case AF_INET6: + ip6 = ns2_get_sbind_ip6_entry(gss, sbind, endpoints); + if (!ip6) + return 1; + ip6->sig_weight = sbind->bind->sns_sig_weight; + ip6->data_weight = sbind->bind->sns_data_weight; + return (ip6_weight_sum_sig(endpoints) != 0 && ip6_weight_sum_data(endpoints) != 0); + break; + default: + OSMO_ASSERT(0); + } +} +static void ns2_add_procedure(struct ns2_sns_state *gss, struct ns2_sns_bind *sbind, + enum sns_procedure procedure_type) +{ + struct ns2_sns_procedure *procedure = NULL; + const struct osmo_sockaddr *saddr; + saddr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + + OSMO_ASSERT(saddr->u.sa.sa_family == gss->family); + + switch (procedure_type) { + case SNS_PROC_ADD: + break; + case SNS_PROC_DEL: + break; + case SNS_PROC_CHANGE_WEIGHT: + llist_for_each_entry(procedure, &gss->procedures, list) { + if (procedure->sbind == sbind && procedure->procedure == procedure_type && + !procedure->running) { + switch(gss->family) { + case AF_INET: + /* merge it with a previous procedure */ + procedure->ip4.ip_addr = sbind->bind->sns_sig_weight; + procedure->ip4.data_weight = sbind->bind->sns_data_weight; + break; + case AF_INET6: + /* merge it with a previous procedure */ + procedure->ip6.sig_weight = sbind->bind->sns_sig_weight; + procedure->ip6.data_weight = sbind->bind->sns_data_weight; + break; + default: + OSMO_ASSERT(0); + } + return; + } + } + break; + default: + return; + } + + procedure = talloc_zero(gss, struct ns2_sns_procedure); + if (!procedure) + return; + + switch (procedure_type) { + case SNS_PROC_ADD: + case SNS_PROC_CHANGE_WEIGHT: + procedure->sbind = sbind; + break; + default: + break; + } + + llist_add_tail(&procedure->list, &gss->procedures); + procedure->procedure = procedure_type; + procedure->sig_weight = sbind->bind->sns_sig_weight; + procedure->data_weight = sbind->bind->sns_data_weight; + + switch(gss->family) { + case AF_INET: + procedure->ip4.ip_addr = saddr->u.sin.sin_addr.s_addr; + procedure->ip4.udp_port = saddr->u.sin.sin_port; + procedure->ip4.sig_weight = sbind->bind->sns_sig_weight; + procedure->ip4.data_weight = sbind->bind->sns_data_weight; + break; + case AF_INET6: + memcpy(&procedure->ip6.ip_addr, &saddr->u.sin6.sin6_addr, sizeof(struct in6_addr)); + procedure->ip6.udp_port = saddr->u.sin.sin_port; + procedure->ip6.sig_weight = sbind->bind->sns_sig_weight; + procedure->ip6.data_weight = sbind->bind->sns_data_weight; + break; + default: + OSMO_ASSERT(0); + } + + if (gss->nse->bss_sns_fi->state == GPRS_SNS_ST_CONFIGURED) { + osmo_fsm_inst_state_chg(gss->nse->bss_sns_fi, GPRS_SNS_ST_LOCAL_PROCEDURE, + gss->nse->nsi->timeout[NS_TOUT_TSNS_PROV], 5); + } +} + +/* add an entrypoint to sns_endpoints */ +static int ns2_sns_add_elements(struct ns2_sns_state *gss, struct ns2_sns_bind *sbind, + struct ns2_sns_elems *elems) +{ + const struct osmo_sockaddr *saddr; + struct gprs_ns_ie_ip4_elem ip4; + struct gprs_ns_ie_ip6_elem ip6; + int rc = -1; + + saddr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + OSMO_ASSERT(saddr->u.sa.sa_family == gss->family); + + switch (gss->family) { + case AF_INET: + ip4.ip_addr = saddr->u.sin.sin_addr.s_addr; + ip4.udp_port= saddr->u.sin.sin_port; + ip4.sig_weight = sbind->bind->sns_sig_weight; + ip4.data_weight = sbind->bind->sns_data_weight; + rc = add_ip4_elem(gss, elems, &ip4); + break; + case AF_INET6: + memcpy(&ip6.ip_addr, &saddr->u.sin6.sin6_addr, sizeof(struct in6_addr)); + ip6.udp_port= saddr->u.sin.sin_port; + ip6.sig_weight = sbind->bind->sns_sig_weight; + ip6.data_weight = sbind->bind->sns_data_weight; + rc = add_ip6_elem(gss, elems, &ip6); + break; + } + + return rc; +} + +/* common allstate-action for both roles */ +static void ns2_sns_st_all_action(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct ns2_sns_bind *sbind; + struct gprs_ns2_vc *nsvc, *nsvc2; + struct ns2_sns_procedure *procedure; + + switch (event) { + case NS2_SNS_EV_REQ_ADD_BIND: + sbind = data; + switch (fi->state) { + case GPRS_SNS_ST_UNCONFIGURED: + if (gss->role == GPRS_SNS_ROLE_BSS) + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, NULL); + break; + case GPRS_SNS_ST_BSS_SIZE: + switch (gss->family) { + case AF_INET: + if (gss->num_max_ip4_remote <= gss->local.num_ip4 || + gss->num_max_ip4_remote * (gss->local.num_ip4 + 1) > gss->num_max_nsvcs) { + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, GPRS_SNS_FLAG_KEEP_SELECT_ENDPOINT_ORDER); + return; + } + break; + case AF_INET6: + if (gss->num_max_ip6_remote <= gss->local.num_ip6 || + gss->num_max_ip6_remote * (gss->local.num_ip6 + 1) > gss->num_max_nsvcs) { + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, GPRS_SNS_FLAG_KEEP_SELECT_ENDPOINT_ORDER); + return; + } + break; + } + ns2_sns_add_elements(gss, sbind, &gss->local); + break; + case GPRS_SNS_ST_BSS_CONFIG_BSS: + case GPRS_SNS_ST_BSS_CONFIG_SGSN: + case GPRS_SNS_ST_CONFIGURED: + switch (gss->family) { + case AF_INET: + if (gss->num_max_ip4_remote <= gss->local.num_ip4) { + LOGPFSML(fi, LOGL_ERROR, + "NSE %d: ignoring bind %s because there are too many endpoints for the SNS.\n", + nse->nsei, sbind->bind->name); + return; + } + if (gss->remote.num_ip4 * (gss->local.num_ip4 + 1) > gss->num_max_nsvcs) { + LOGPFSML(fi, LOGL_ERROR, + "NSE %d: ignoring bind %s because there are too many endpoints for the SNS.\n", + nse->nsei, sbind->bind->name); + return; + } + break; + case AF_INET6: + if (gss->num_max_ip6_remote <= gss->local.num_ip6) { + LOGPFSML(fi, LOGL_ERROR, + "NSE %d: ignoring bind %s because there are too many endpoints for the SNS.\n", + nse->nsei, sbind->bind->name); + return; + } + if (gss->remote.num_ip6 * (gss->local.num_ip6 + 1) > gss->num_max_nsvcs) { + LOGPFSML(fi, LOGL_ERROR, + "NSE %d: ignoring bind %s because there are too many endpoints for the SNS.\n", + nse->nsei, sbind->bind->name); + return; + } + break; + } + ns2_sns_add_elements(gss, sbind, &gss->local_procedure); + ns2_add_procedure(gss, sbind, SNS_PROC_ADD); + break; + } + break; + case NS2_SNS_EV_REQ_DELETE_BIND: + sbind = data; + switch (fi->state) { + case GPRS_SNS_ST_UNCONFIGURED: + break; + case GPRS_SNS_ST_BSS_SIZE: + llist_for_each_entry_safe(nsvc, nsvc2, &nse->nsvc, list) { + if (nsvc->bind == sbind->bind) { + gprs_ns2_free_nsvc(nsvc); + } + } + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, NULL); + break; + case GPRS_SNS_ST_BSS_CONFIG_BSS: + case GPRS_SNS_ST_BSS_CONFIG_SGSN: + case GPRS_SNS_ST_CONFIGURED: + case GPRS_SNS_ST_LOCAL_PROCEDURE: + remove_bind_elem(gss, &gss->local_procedure, sbind); + if (ip46_weight_sum(&gss->local_procedure, true) == 0 || + ip46_weight_sum(&gss->local_procedure, false) == 0) { + LOGPFSML(fi, LOGL_ERROR, "NSE %d: weight has become invalid because of removing bind %s. Resetting the configuration\n", + nse->nsei, sbind->bind->name); + sns_failed(fi, NULL); + break; + } + gss->block_no_nsvc_events = true; + llist_for_each_entry_safe(nsvc, nsvc2, &nse->nsvc, list) { + if (nsvc->bind == sbind->bind) { + gprs_ns2_free_nsvc(nsvc); + } + } + gss->block_no_nsvc_events = false; + if (nse->sum_sig_weight == 0 || !nse->alive || !gss->alive) { + sns_failed(fi, "While deleting a bind the current state became invalid (no signalling weight)"); + break; + } + + /* ensure other procedures doesn't use the sbind */ + llist_for_each_entry(procedure, &gss->procedures, list) { + if (procedure->sbind == sbind) + procedure->sbind = NULL; + } + ns2_add_procedure(gss, sbind, SNS_PROC_DEL); + break; + } + + /* if this is the last bind, the free_nsvc() will trigger a reselection */ + talloc_free(sbind); + break; + case NS2_SNS_EV_REQ_CHANGE_WEIGHT: + sbind = data; + switch (fi->state) { + case GPRS_SNS_ST_UNCONFIGURED: + /* select_endpoint will check if this is a valid configuration */ + if (gss->role == GPRS_SNS_ROLE_BSS) + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, NULL); + break; + case GPRS_SNS_ST_BSS_SIZE: + /* invalid weight? */ + if (!ns2_update_weight_entry(gss, sbind, &gss->local)) + sns_failed(fi, "updating weights results in an invalid configuration."); + break; + default: + if (!ns2_update_weight_entry(gss, sbind, &gss->local_procedure)) { + sns_failed(fi, "updating weights results in an invalid configuration."); + break; + } + ns2_add_procedure(gss, sbind, SNS_PROC_CHANGE_WEIGHT); + break; + } + } +} + +/* validate the bss configuration (sns endpoint and binds) + * - no endpoints -> invalid + * - no binds -> invalid + * - only v4 sns endpoints, only v6 binds -> invalid + * - only v4 sns endpoints, but v4 sig weights == 0 -> invalid ... + */ +static int ns2_sns_bss_valid_configuration(struct ns2_sns_state *gss) +{ + struct ns2_sns_bind *sbind; + struct sns_endpoint *endpoint; + const struct osmo_sockaddr *addr; + int v4_sig = 0, v4_data = 0, v6_sig = 0, v6_data = 0; + bool v4_endpoints = false; + bool v6_endpoints = false; + + if (llist_empty(&gss->sns_endpoints) || llist_empty(&gss->binds)) + return 0; + + llist_for_each_entry(sbind, &gss->binds, list) { + addr = gprs_ns2_ip_bind_sockaddr(sbind->bind); + if (!addr) + continue; + switch (addr->u.sa.sa_family) { + case AF_INET: + v4_sig += sbind->bind->sns_sig_weight; + v4_data += sbind->bind->sns_data_weight; + break; + case AF_INET6: + v6_sig += sbind->bind->sns_sig_weight; + v6_data += sbind->bind->sns_data_weight; + break; + } + } + + llist_for_each_entry(endpoint, &gss->sns_endpoints, list) { + switch (endpoint->saddr.u.sa.sa_family) { + case AF_INET: + v4_endpoints = true; + break; + case AF_INET6: + v6_endpoints = true; + break; + } + } + + return (v4_endpoints && v4_sig && v4_data) || (v6_endpoints && v6_sig && v6_data); +} + +/* allstate-action for BSS role */ +static void ns2_sns_st_all_action_bss(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + + /* reset when receiving NS2_SNS_EV_REQ_NO_NSVC */ + switch (event) { + case NS2_SNS_EV_REQ_NO_NSVC: + /* ignore reselection running */ + if (gss->reselection_running || gss->block_no_nsvc_events) + break; + + sns_failed(fi, "no remaining NSVC, resetting SNS FSM"); + break; + case NS2_SNS_EV_REQ_FREE_NSVCS: + case NS2_SNS_EV_REQ_SELECT_ENDPOINT: + /* TODO: keep the order of binds when data == GPRS_SNS_FLAG_KEEP_SELECT_ENDPOINT_ORDER */ + /* tear down previous state + * gprs_ns2_free_nsvcs() will trigger NO_NSVC, prevent this from triggering a reselection */ + if (gss->reselection_running || gss->block_no_nsvc_events) + break; + + gss->reselection_running = true; + ns2_free_nsvcs(nse); + ns2_clear_elems(&gss->local); + ns2_clear_elems(&gss->remote); + + /* Choose the next sns endpoint. */ + if (!ns2_sns_bss_valid_configuration(gss)) { + gss->initial = NULL; + ns2_prim_status_ind(gss->nse, NULL, 0, GPRS_NS2_AFF_CAUSE_SNS_NO_ENDPOINTS); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 3); + gss->reselection_running = false; + return; + } else if (!gss->initial) { + gss->initial = llist_first_entry(&gss->sns_endpoints, struct sns_endpoint, list); + } else if (gss->initial->list.next == &gss->sns_endpoints) { + /* last entry, continue with first */ + gss->initial = llist_first_entry(&gss->sns_endpoints, struct sns_endpoint, list); + } else { + /* next element is an entry */ + gss->initial = llist_entry(gss->initial->list.next, struct sns_endpoint, list); + } + + gss->family = gss->initial->saddr.u.sa.sa_family; + gss->reselection_running = false; + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_BSS_SIZE, nse->nsi->timeout[NS_TOUT_TSNS_PROV], 1); + break; + default: + ns2_sns_st_all_action(fi, event, data); + break; + } +} + +static struct osmo_fsm gprs_ns2_sns_bss_fsm = { + .name = "GPRS-NS2-SNS-BSS", + .states = ns2_sns_bss_states, + .num_states = ARRAY_SIZE(ns2_sns_bss_states), + .allstate_event_mask = S(NS2_SNS_EV_REQ_NO_NSVC) | + S(NS2_SNS_EV_REQ_FREE_NSVCS) | + S(NS2_SNS_EV_REQ_SELECT_ENDPOINT) | + S(NS2_SNS_EV_REQ_ADD_BIND) | + S(NS2_SNS_EV_REQ_CHANGE_WEIGHT) | + S(NS2_SNS_EV_REQ_DELETE_BIND), + .allstate_action = ns2_sns_st_all_action_bss, + .cleanup = NULL, + .timer_cb = ns2_sns_fsm_bss_timer_cb, + .event_names = gprs_sns_event_names, + .pre_term = NULL, + .log_subsys = DLNS, +}; + +/*! Allocate an IP-SNS FSM for the BSS side. + * \param[in] nse NS Entity in which the FSM runs + * \param[in] id string identifier + * \returns FSM instance on success; NULL on error */ +struct osmo_fsm_inst *ns2_sns_bss_fsm_alloc(struct gprs_ns2_nse *nse, + const char *id) +{ + struct osmo_fsm_inst *fi; + struct ns2_sns_state *gss; + + fi = osmo_fsm_inst_alloc(&gprs_ns2_sns_bss_fsm, nse, NULL, LOGL_DEBUG, id); + if (!fi) + return fi; + + gss = talloc_zero(fi, struct ns2_sns_state); + if (!gss) + goto err; + + fi->priv = gss; + gss->nse = nse; + gss->role = GPRS_SNS_ROLE_BSS; + /* The SGSN doesn't tell the BSS, so we assume there's always sufficient */ + gss->num_max_ip4_remote = 8192; + gss->num_max_ip6_remote = 8192; + INIT_LLIST_HEAD(&gss->sns_endpoints); + INIT_LLIST_HEAD(&gss->binds); + INIT_LLIST_HEAD(&gss->procedures); + + return fi; +err: + osmo_fsm_inst_term(fi, OSMO_FSM_TERM_ERROR, NULL); + return NULL; +} + +/*! main entry point for receiving SNS messages from the network. + * \param[in] nsvc NS-VC on which the message was received + * \param[in] msg message buffer of the IP-SNS message + * \param[in] tp parsed TLV structure of message + * \returns 0 on success; negative on error */ +int ns2_sns_rx(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp) +{ + struct gprs_ns2_nse *nse = nsvc->nse; + struct gprs_ns_hdr *nsh = (struct gprs_ns_hdr *) msg->l2h; + uint16_t nsei = nsvc->nse->nsei; + struct ns2_sns_state *gss; + struct osmo_fsm_inst *fi; + int rc = 0; + + if (!nse->bss_sns_fi) { + LOGNSVC(nsvc, LOGL_NOTICE, "Rx %s for NS Instance that has no SNS!\n", + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type)); + rc = -EINVAL; + goto out; + } + + /* FIXME: how to resolve SNS FSM Instance by NSEI (SGSN)? */ + fi = nse->bss_sns_fi; + gss = (struct ns2_sns_state *) fi->priv; + gss->sns_nsvc = nsvc; + + LOGPFSML(fi, LOGL_DEBUG, "NSEI=%u Rx SNS PDU type %s\n", nsei, + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type)); + + switch (nsh->pdu_type) { + case SNS_PDUT_SIZE: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_SIZE, tp); + break; + case SNS_PDUT_SIZE_ACK: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_SIZE_ACK, tp); + break; + case SNS_PDUT_CONFIG: + if (nsh->data[0] & 0x01) + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_CONFIG_END, tp); + else + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_CONFIG, tp); + break; + case SNS_PDUT_CONFIG_ACK: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_CONFIG_ACK, tp); + break; + case SNS_PDUT_ADD: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_ADD, tp); + break; + case SNS_PDUT_DELETE: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_DELETE, tp); + break; + case SNS_PDUT_CHANGE_WEIGHT: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_CHANGE_WEIGHT, tp); + break; + case SNS_PDUT_ACK: + osmo_fsm_inst_dispatch(fi, NS2_SNS_EV_RX_ACK, tp); + break; + default: + LOGPFSML(fi, LOGL_ERROR, "NSEI=%u Rx unknown SNS PDU type %s\n", nsei, + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type)); + rc = -EINVAL; + } + +out: + msgb_free(msg); + + return rc; +} + +#include <osmocom/vty/vty.h> +#include <osmocom/vty/misc.h> + +static void vty_dump_sns_ip4(struct vty *vty, const char *prefix, const struct gprs_ns_ie_ip4_elem *ip4) +{ + struct in_addr in = { .s_addr = ip4->ip_addr }; + vty_out(vty, "%s %s:%u, Signalling Weight: %u, Data Weight: %u%s", prefix, + inet_ntoa(in), ntohs(ip4->udp_port), ip4->sig_weight, ip4->data_weight, VTY_NEWLINE); +} + +static void vty_dump_sns_ip6(struct vty *vty, const char *prefix, const struct gprs_ns_ie_ip6_elem *ip6) +{ + char ip_addr[INET6_ADDRSTRLEN] = {}; + if (!inet_ntop(AF_INET6, &ip6->ip_addr, ip_addr, (INET6_ADDRSTRLEN))) + strcpy(ip_addr, "Invalid IPv6"); + + vty_out(vty, "%s %s:%u, Signalling Weight: %u, Data Weight: %u%s", prefix, + ip_addr, ntohs(ip6->udp_port), ip6->sig_weight, ip6->data_weight, VTY_NEWLINE); +} + +/*! Dump the IP-SNS state to a vty. + * \param[in] vty VTY to which the state shall be printed + * \param[in] prefix prefix to print at start of each line (typically indenting) + * \param[in] nse NS Entity whose IP-SNS state shall be printed + * \param[in] stats Whether or not statistics shall also be printed */ +void ns2_sns_dump_vty(struct vty *vty, const char *prefix, const struct gprs_ns2_nse *nse, bool stats) +{ + struct ns2_sns_state *gss; + unsigned int i; + + if (!nse->bss_sns_fi) + return; + + vty_out_fsm_inst2(vty, prefix, nse->bss_sns_fi); + gss = (struct ns2_sns_state *) nse->bss_sns_fi->priv; + + vty_out(vty, "%sMaximum number of remote NS-VCs: %zu, IPv4 Endpoints: %zu, IPv6 Endpoints: %zu%s", + prefix, gss->num_max_nsvcs, gss->num_max_ip4_remote, gss->num_max_ip6_remote, VTY_NEWLINE); + + if (gss->local.num_ip4 && gss->remote.num_ip4) { + vty_out(vty, "%sLocal IPv4 Endpoints:%s", prefix, VTY_NEWLINE); + for (i = 0; i < gss->local.num_ip4; i++) + vty_dump_sns_ip4(vty, prefix, &gss->local.ip4[i]); + + vty_out(vty, "%sRemote IPv4 Endpoints:%s", prefix, VTY_NEWLINE); + for (i = 0; i < gss->remote.num_ip4; i++) + vty_dump_sns_ip4(vty, prefix, &gss->remote.ip4[i]); + } + + if (gss->local.num_ip6 && gss->remote.num_ip6) { + vty_out(vty, "%sLocal IPv6 Endpoints:%s", prefix, VTY_NEWLINE); + for (i = 0; i < gss->local.num_ip6; i++) + vty_dump_sns_ip6(vty, prefix, &gss->local.ip6[i]); + + vty_out(vty, "%sRemote IPv6 Endpoints:%s", prefix, VTY_NEWLINE); + for (i = 0; i < gss->remote.num_ip6; i++) + vty_dump_sns_ip6(vty, prefix, &gss->remote.ip6[i]); + } +} + +/*! write IP-SNS to a vty + * \param[in] vty VTY to which the state shall be printed + * \param[in] nse NS Entity whose IP-SNS state shall be printed */ +void ns2_sns_write_vty(struct vty *vty, const struct gprs_ns2_nse *nse) +{ + struct ns2_sns_state *gss; + struct osmo_sockaddr_str addr_str; + struct sns_endpoint *endpoint; + + if (!nse->bss_sns_fi) + return; + + gss = (struct ns2_sns_state *) nse->bss_sns_fi->priv; + llist_for_each_entry(endpoint, &gss->sns_endpoints, list) { + /* It's unlikely that an error happens, but let's better be safe. */ + if (osmo_sockaddr_str_from_sockaddr(&addr_str, &endpoint->saddr.u.sas) != 0) + addr_str = (struct osmo_sockaddr_str) { .ip = "<INVALID>" }; + vty_out(vty, " ip-sns-remote %s %u%s", addr_str.ip, addr_str.port, VTY_NEWLINE); + } +} + +static struct sns_endpoint *ns2_get_sns_endpoint(struct ns2_sns_state *state, + const struct osmo_sockaddr *saddr) +{ + struct sns_endpoint *endpoint; + + llist_for_each_entry(endpoint, &state->sns_endpoints, list) { + if (!osmo_sockaddr_cmp(saddr, &endpoint->saddr)) + return endpoint; + } + + return NULL; +} + +/*! gprs_ns2_sns_add_endpoint + * \param[in] nse + * \param[in] sockaddr + * \return + */ +int gprs_ns2_sns_add_endpoint(struct gprs_ns2_nse *nse, + const struct osmo_sockaddr *saddr) +{ + struct ns2_sns_state *gss; + struct sns_endpoint *endpoint; + bool do_selection = false; + + if (nse->ll != GPRS_NS2_LL_UDP) { + return -EINVAL; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + return -EINVAL; + } + + gss = nse->bss_sns_fi->priv; + + if (ns2_get_sns_endpoint(gss, saddr)) + return -EADDRINUSE; + + endpoint = talloc_zero(nse->bss_sns_fi->priv, struct sns_endpoint); + if (!endpoint) + return -ENOMEM; + + endpoint->saddr = *saddr; + if (llist_empty(&gss->sns_endpoints)) + do_selection = true; + + llist_add_tail(&endpoint->list, &gss->sns_endpoints); + if (do_selection) + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_SELECT_ENDPOINT, NULL); + + return 0; +} + +/*! gprs_ns2_sns_del_endpoint + * \param[in] nse + * \param[in] sockaddr + * \return 0 on success, otherwise < 0 + */ +int gprs_ns2_sns_del_endpoint(struct gprs_ns2_nse *nse, + const struct osmo_sockaddr *saddr) +{ + struct ns2_sns_state *gss; + struct sns_endpoint *endpoint; + + if (nse->ll != GPRS_NS2_LL_UDP) { + return -EINVAL; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + return -EINVAL; + } + + gss = nse->bss_sns_fi->priv; + endpoint = ns2_get_sns_endpoint(gss, saddr); + if (!endpoint) + return -ENOENT; + + /* if this is an unused SNS endpoint it's done */ + if (gss->initial != endpoint) { + llist_del(&endpoint->list); + talloc_free(endpoint); + return 0; + } + + /* gprs_ns2_free_nsvcs() will trigger NS2_SNS_EV_REQ_NO_NSVC on the last NS-VC + * and restart SNS SIZE procedure which selects a new initial */ + LOGNSE(nse, LOGL_INFO, "Current in-use SNS endpoint is being removed." + "Closing all NS-VC and restart SNS-SIZE procedure" + "with a remaining SNS endpoint.\n"); + + /* Continue with the next endpoint in the list. + * Special case if the endpoint is at the start or end of the list */ + if (endpoint->list.prev == &gss->sns_endpoints || + endpoint->list.next == &gss->sns_endpoints) + gss->initial = NULL; + else + gss->initial = llist_entry(endpoint->list.next->prev, + struct sns_endpoint, + list); + + llist_del(&endpoint->list); + gprs_ns2_free_nsvcs(nse); + talloc_free(endpoint); + + return 0; +} + +/*! gprs_ns2_sns_count + * \param[in] nse NS Entity whose IP-SNS endpoints shall be printed + * \return the count of endpoints or < 0 if NSE doesn't contain sns. + */ +int gprs_ns2_sns_count(struct gprs_ns2_nse *nse) +{ + struct ns2_sns_state *gss; + struct sns_endpoint *endpoint; + int count = 0; + + if (nse->ll != GPRS_NS2_LL_UDP) { + return -EINVAL; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + return -EINVAL; + } + + gss = nse->bss_sns_fi->priv; + llist_for_each_entry(endpoint, &gss->sns_endpoints, list) + count++; + + return count; +} + +void ns2_sns_notify_alive(struct gprs_ns2_nse *nse, struct gprs_ns2_vc *nsvc, bool alive) +{ + struct ns2_sns_state *gss; + struct gprs_ns2_vc *tmp; + + if (!nse->bss_sns_fi) + return; + + gss = nse->bss_sns_fi->priv; + if (nse->bss_sns_fi->state != GPRS_SNS_ST_CONFIGURED && nse->bss_sns_fi->state != GPRS_SNS_ST_LOCAL_PROCEDURE) + return; + + if (gss->block_no_nsvc_events) + return; + + if (gss->alive && nse->sum_sig_weight == 0) { + sns_failed(nse->bss_sns_fi, "No signalling NSVC available"); + return; + } + + /* check if this is the current SNS NS-VC */ + if (nsvc == gss->sns_nsvc && !alive) { + /* only replace the SNS NS-VC if there are other alive NS-VC. + * There aren't any other alive NS-VC when the SNS fsm just reached CONFIGURED + * and couldn't confirm yet if the NS-VC comes up */ + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (nsvc == tmp) + continue; + if (ns2_vc_is_unblocked(nsvc)) { + ns2_sns_replace_nsvc(nsvc); + break; + } + } + } + + if (alive == gss->alive) + return; + + if (alive) { + /* we need at least a signalling NSVC before become alive */ + if (nse->sum_sig_weight == 0) + return; + gss->alive = true; + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_NSVC_ALIVE, NULL); + } else { + /* is there at least another alive nsvc? */ + llist_for_each_entry(tmp, &nse->nsvc, list) { + if (ns2_vc_is_unblocked(tmp)) + return; + } + + /* all NS-VC have failed */ + gss->alive = false; + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_NO_NSVC, NULL); + } +} + +int gprs_ns2_sns_add_bind(struct gprs_ns2_nse *nse, + struct gprs_ns2_vc_bind *bind) +{ + struct ns2_sns_state *gss; + struct ns2_sns_bind *tmp; + + OSMO_ASSERT(nse->bss_sns_fi); + gss = nse->bss_sns_fi->priv; + + if (!gprs_ns2_is_ip_bind(bind)) { + return -EINVAL; + } + + if (!llist_empty(&gss->binds)) { + llist_for_each_entry(tmp, &gss->binds, list) { + if (tmp->bind == bind) + return -EALREADY; + } + } + + tmp = talloc_zero(gss, struct ns2_sns_bind); + if (!tmp) + return -ENOMEM; + tmp->bind = bind; + llist_add_tail(&tmp->list, &gss->binds); + + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_ADD_BIND, tmp); + return 0; +} + +/* Remove a bind from the SNS. All assosiated NSVC must be removed. */ +int gprs_ns2_sns_del_bind(struct gprs_ns2_nse *nse, + struct gprs_ns2_vc_bind *bind) +{ + struct ns2_sns_state *gss; + struct ns2_sns_bind *tmp, *tmp2; + bool found = false; + + if (!nse->bss_sns_fi) + return -EINVAL; + + gss = nse->bss_sns_fi->priv; + if (gss->initial_bind && gss->initial_bind->bind == bind) { + if (gss->initial_bind->list.prev == &gss->binds) + gss->initial_bind = NULL; + else + gss->initial_bind = llist_entry(gss->initial_bind->list.prev, struct ns2_sns_bind, list); + } + + llist_for_each_entry_safe(tmp, tmp2, &gss->binds, list) { + if (tmp->bind == bind) { + llist_del(&tmp->list); + found = true; + break; + } + } + + if (!found) + return -ENOENT; + + osmo_fsm_inst_dispatch(nse->bss_sns_fi, NS2_SNS_EV_REQ_DELETE_BIND, tmp); + return 0; +} + +/* Update SNS weights for a bind (local endpoint). + * \param[in] bind the bind which has been updated + */ +void ns2_sns_update_weights(struct gprs_ns2_vc_bind *bind) +{ + struct ns2_sns_bind *sbind; + struct gprs_ns2_nse *nse; + struct ns2_sns_state *gss; + const struct osmo_sockaddr *addr = gprs_ns2_ip_bind_sockaddr(bind); + + llist_for_each_entry(nse, &bind->nsi->nse, list) { + if (!nse->bss_sns_fi) + continue; + + gss = nse->bss_sns_fi->priv; + if (addr->u.sa.sa_family != gss->family) + return; + + llist_for_each_entry(sbind, &gss->binds, list) { + if (sbind->bind == bind) { + osmo_fsm_inst_dispatch(gss->nse->bss_sns_fi, NS2_SNS_EV_REQ_CHANGE_WEIGHT, sbind); + break; + } + } + } +} + + + + +/*********************************************************************** + * SGSN role + ***********************************************************************/ + +/* cleanup all state. If nsvc is given, don't remove this nsvc. (nsvc is given when a SIZE PDU received) */ +static void ns2_clear_sgsn(struct ns2_sns_state *gss, struct gprs_ns2_vc *size_nsvc) +{ + struct gprs_ns2_vc *nsvc, *nsvc2; + + ns2_clear_procedures(gss); + ns2_clear_elems(&gss->local); + ns2_clear_elems(&gss->remote); + gss->block_no_nsvc_events = true; + llist_for_each_entry_safe(nsvc, nsvc2, &gss->nse->nsvc, list) { + /* Ignore the NSVC over which the SIZE PDU got received */ + if (size_nsvc && size_nsvc == nsvc) + continue; + + gprs_ns2_free_nsvc(nsvc); + } + gss->block_no_nsvc_events = false; +} + +static void ns2_sns_st_sgsn_unconfigured_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + + ns2_clear_sgsn(gss, NULL); +} + +static void ns2_sns_st_sgsn_unconfigured(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_SGSN); + /* do nothing; Rx SNS-SIZE handled in ns2_sns_st_all_action_sgsn() */ +} + +/* We're waiting for inbound SNS-CONFIG from the BSS */ +static void ns2_sns_st_sgsn_wait_config(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_inst *nsi = nse->nsi; + uint8_t cause; + int rc; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_SGSN); + + switch (event) { + case NS2_SNS_EV_RX_CONFIG: + case NS2_SNS_EV_RX_CONFIG_END: + rc = ns_sns_append_remote_eps(fi, data); + if (rc < 0) { + cause = -rc; + ns2_tx_sns_config_ack(gss->sns_nsvc, &cause); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0); + return; + } + /* only change state if last CONFIG was received */ + if (event == NS2_SNS_EV_RX_CONFIG_END) { + /* ensure sum of data weight / sig weights is > 0 */ + if (ip46_weight_sum_data(&gss->remote) == 0 || ip46_weight_sum_sig(&gss->remote) == 0) { + cause = NS_CAUSE_INVAL_WEIGH; + ns2_tx_sns_config_ack(gss->sns_nsvc, &cause); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0); + break; + } + ns2_tx_sns_config_ack(gss->sns_nsvc, NULL); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK, nsi->timeout[NS_TOUT_TSNS_PROV], 3); + } else { + /* just send CONFIG-ACK */ + ns2_tx_sns_config_ack(gss->sns_nsvc, NULL); + osmo_timer_schedule(&fi->timer, nse->nsi->timeout[NS_TOUT_TSNS_PROV], 0); + } + break; + } +} + +static void ns2_sns_st_sgsn_wait_config_ack_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_SGSN); + + /* transmit SGSN-oriented SNS-CONFIG */ + ns2_tx_sns_config(gss->sns_nsvc, true, gss->local.ip4, gss->local.num_ip4, + gss->local.ip6, gss->local.num_ip6); +} + +/* We're waiting for SNS-CONFIG-ACK from the BSS (in response to our outbound SNS-CONFIG) */ +static void ns2_sns_st_sgsn_wait_config_ack(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct tlv_parsed *tp = NULL; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_SGSN); + + switch (event) { + case NS2_SNS_EV_RX_CONFIG_ACK: + tp = data; + if (TLVP_VAL_MINLEN(tp, NS_IE_CAUSE, 1)) { + LOGPFSML(fi, LOGL_ERROR, "Rx SNS-CONFIG-ACK with cause %s\n", + gprs_ns2_cause_str(*TLVP_VAL(tp, NS_IE_CAUSE))); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, 0, 0); + break; + } + /* we currently only send one SNS-CONFIG with END FLAG */ + if (true) { + create_missing_nsvcs(fi); + /* start the test procedure on ALL NSVCs! */ + gprs_ns2_start_alive_all_nsvcs(nse); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_CONFIGURED, ns_sns_configured_timeout(fi), 4); + } + break; + } +} + +/* SGSN-side SNS state machine */ +static const struct osmo_fsm_state ns2_sns_sgsn_states[] = { + [GPRS_SNS_ST_UNCONFIGURED] = { + .in_event_mask = 0, /* handled by all_state_action */ + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG), + .name = "UNCONFIGURED", + .action = ns2_sns_st_sgsn_unconfigured, + .onenter = ns2_sns_st_sgsn_unconfigured_onenter, + }, + [GPRS_SNS_ST_SGSN_WAIT_CONFIG] = { + .in_event_mask = S(NS2_SNS_EV_RX_CONFIG) | + S(NS2_SNS_EV_RX_CONFIG_END), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK), + .name = "SGSN_WAIT_CONFIG", + .action = ns2_sns_st_sgsn_wait_config, + }, + [GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK] = { + .in_event_mask = S(NS2_SNS_EV_RX_CONFIG_ACK), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK) | + S(GPRS_SNS_ST_CONFIGURED), + .name = "SGSN_WAIT_CONFIG_ACK", + .action = ns2_sns_st_sgsn_wait_config_ack, + .onenter = ns2_sns_st_sgsn_wait_config_ack_onenter, + }, + [GPRS_SNS_ST_CONFIGURED] = { + .in_event_mask = S(NS2_SNS_EV_RX_ADD) | + S(NS2_SNS_EV_RX_DELETE) | + S(NS2_SNS_EV_RX_CHANGE_WEIGHT) | + S(NS2_SNS_EV_REQ_NSVC_ALIVE), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_SGSN_WAIT_CONFIG) | + S(GPRS_SNS_ST_LOCAL_PROCEDURE), + .name = "CONFIGURED", + /* shared with BSS side; once configured there's no difference */ + .action = ns2_sns_st_configured, + .onenter = ns2_sns_st_configured_onenter, + }, + [GPRS_SNS_ST_LOCAL_PROCEDURE] = { + .in_event_mask = S(NS2_SNS_EV_RX_ADD) | + S(NS2_SNS_EV_RX_DELETE) | + S(NS2_SNS_EV_RX_CHANGE_WEIGHT) | + S(NS2_SNS_EV_RX_ACK) | + S(NS2_SNS_EV_REQ_CHANGE_WEIGHT) | + S(NS2_SNS_EV_REQ_NSVC_ALIVE), + .out_state_mask = S(GPRS_SNS_ST_UNCONFIGURED) | + S(GPRS_SNS_ST_CONFIGURED) | + S(GPRS_SNS_ST_LOCAL_PROCEDURE), + .name = "LOCAL_PROCEDURE", + /* shared with BSS side; once configured there's no difference */ + .action = ns2_sns_st_local_procedure, + .onenter = ns2_sns_st_local_procedure_onenter, + }, +}; + +static int ns2_sns_fsm_sgsn_timer_cb(struct osmo_fsm_inst *fi) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct gprs_ns2_nse *nse = nse_inst_from_fi(fi); + struct gprs_ns2_inst *nsi = nse->nsi; + + gss->N++; + switch (fi->T) { + case 3: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_CONFIG_RETRIES]) { + LOGPFSML(fi, LOGL_ERROR, "NSE %d: SGSN Config retries failed. Giving up.\n", nse->nsei); + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_UNCONFIGURED, nsi->timeout[NS_TOUT_TSNS_PROV], 3); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_SGSN_WAIT_CONFIG_ACK, nsi->timeout[NS_TOUT_TSNS_PROV], 3); + } + break; + case 4: + LOGPFSML(fi, LOGL_ERROR, "NSE %d: Config succeeded but no NS-VC came online.\n", nse->nsei); + break; + case 5: + if (gss->N >= nsi->timeout[NS_TOUT_TSNS_PROCEDURES_RETRIES]) { + sns_failed(fi, "SNS Procedure retries failed."); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_LOCAL_PROCEDURE, nsi->timeout[NS_TOUT_TSNS_PROV], + fi->T); + } + break; + } + + return 0; +} + +/* allstate-action for SGSN role */ +static void ns2_sns_st_all_action_sgsn(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct ns2_sns_state *gss = (struct ns2_sns_state *) fi->priv; + struct tlv_parsed *tp = NULL; + size_t num_local_eps, num_remote_eps; + uint8_t flag; + uint8_t cause; + + OSMO_ASSERT(gss->role == GPRS_SNS_ROLE_SGSN); + + switch (event) { + case NS2_SNS_EV_RX_SIZE: + tp = (struct tlv_parsed *) data; + /* check for mandatory / conditional IEs */ + if (!TLVP_PRES_LEN(tp, NS_IE_RESET_FLAG, 1) || + !TLVP_PRES_LEN(tp, NS_IE_MAX_NR_NSVC, 2)) { + cause = NS_CAUSE_MISSING_ESSENT_IE; + ns2_tx_sns_size_ack(gss->sns_nsvc, &cause); + if (fi->state == GPRS_SNS_ST_UNCONFIGURED) + sns_failed(fi, "Rx Size: Missing essential IE"); + break; + } + if (!TLVP_PRES_LEN(tp, NS_IE_IPv4_EP_NR, 2) && + !TLVP_PRES_LEN(tp, NS_IE_IPv6_EP_NR, 2)) { + cause = NS_CAUSE_MISSING_ESSENT_IE; + ns2_tx_sns_size_ack(gss->sns_nsvc, &cause); + if (fi->state == GPRS_SNS_ST_UNCONFIGURED) + sns_failed(fi, "Rx Size: Missing essential IE"); + break; + } + if (TLVP_PRES_LEN(tp, NS_IE_IPv4_EP_NR, 2)) + gss->num_max_ip4_remote = tlvp_val16be(tp, NS_IE_IPv4_EP_NR); + if (TLVP_PRES_LEN(tp, NS_IE_IPv6_EP_NR, 2)) + gss->num_max_ip6_remote = tlvp_val16be(tp, NS_IE_IPv6_EP_NR); + /* decide if we go for IPv4 or IPv6 */ + if (gss->num_max_ip6_remote && ns2_sns_count_num_local_ep(fi, AF_INET6)) { + gss->family = AF_INET6; + ns2_sns_compute_local_ep_from_binds(fi); + num_local_eps = gss->local.num_ip6; + num_remote_eps = gss->num_max_ip6_remote; + } else if (gss->num_max_ip4_remote && ns2_sns_count_num_local_ep(fi, AF_INET)) { + gss->family = AF_INET; + ns2_sns_compute_local_ep_from_binds(fi); + num_local_eps = gss->local.num_ip4; + num_remote_eps = gss->num_max_ip4_remote; + } else { + if (gss->local.num_ip4 && !gss->num_max_ip4_remote) + cause = NS_CAUSE_INVAL_NR_IPv4_EP; + else + cause = NS_CAUSE_INVAL_NR_IPv6_EP; + ns2_tx_sns_size_ack(gss->sns_nsvc, &cause); + if (fi->state == GPRS_SNS_ST_UNCONFIGURED) + sns_failed(fi, "Rx Size: Invalid Nr of IPv4/IPv6 EPs"); + break; + } + /* ensure number of NS-VCs is sufficient for full mesh */ + gss->num_max_nsvcs = tlvp_val16be(tp, NS_IE_MAX_NR_NSVC); + if (gss->num_max_nsvcs < num_remote_eps * num_local_eps) { + LOGPFSML(fi, LOGL_ERROR, "%zu local and %zu remote EPs, requires %zu NS-VC, " + "but BSS supports only %zu maximum NS-VCs\n", num_local_eps, + num_remote_eps, num_local_eps * num_remote_eps, gss->num_max_nsvcs); + cause = NS_CAUSE_INVAL_NR_NS_VC; + ns2_tx_sns_size_ack(gss->sns_nsvc, &cause); + if (fi->state == GPRS_SNS_ST_UNCONFIGURED) + sns_failed(fi, NULL); + break; + } + /* perform state reset, if requested */ + flag = *TLVP_VAL(tp, NS_IE_RESET_FLAG); + if (flag & 1) { + /* clear all state */ + /* TODO: ensure gss->sns_nsvc is always the NSVC on which we received the SIZE PDU */ + gss->N = 0; + ns2_clear_sgsn(gss, gss->sns_nsvc); + /* keep the NSVC we need for SNS, but unconfigure it */ + gss->sns_nsvc->sig_weight = 0; + gss->sns_nsvc->data_weight = 0; + gss->block_no_nsvc_events = true; + ns2_vc_force_unconfigured(gss->sns_nsvc); + gss->block_no_nsvc_events = false; + ns2_sns_compute_local_ep_from_binds(fi); + } + + if (fi->state == GPRS_SNS_ST_UNCONFIGURED && !(flag & 1)) { + sns_failed(fi, "Rx Size without Reset flag, but NSE is unknown"); + break; + } + + /* send SIZE_ACK */ + ns2_tx_sns_size_ack(gss->sns_nsvc, NULL); + /* only wait for SNS-CONFIG in case of Reset flag */ + if (flag & 1) + osmo_fsm_inst_state_chg(fi, GPRS_SNS_ST_SGSN_WAIT_CONFIG, 0, 0); + break; + case NS2_SNS_EV_REQ_FREE_NSVCS: + sns_failed(fi, "On user request to free all NSVCs"); + break; + default: + ns2_sns_st_all_action(fi, event, data); + break; + } +} + +static struct osmo_fsm gprs_ns2_sns_sgsn_fsm = { + .name = "GPRS-NS2-SNS-SGSN", + .states = ns2_sns_sgsn_states, + .num_states = ARRAY_SIZE(ns2_sns_sgsn_states), + .allstate_event_mask = S(NS2_SNS_EV_RX_SIZE) | + S(NS2_SNS_EV_REQ_NO_NSVC) | + S(NS2_SNS_EV_REQ_FREE_NSVCS) | + S(NS2_SNS_EV_REQ_ADD_BIND) | + S(NS2_SNS_EV_REQ_CHANGE_WEIGHT) | + S(NS2_SNS_EV_REQ_DELETE_BIND), + .allstate_action = ns2_sns_st_all_action_sgsn, + .cleanup = NULL, + .timer_cb = ns2_sns_fsm_sgsn_timer_cb, + .event_names = gprs_sns_event_names, + .pre_term = NULL, + .log_subsys = DLNS, +}; + +/*! Allocate an IP-SNS FSM for the SGSN side. + * \param[in] nse NS Entity in which the FSM runs + * \param[in] id string identifier + * \returns FSM instance on success; NULL on error */ +struct osmo_fsm_inst *ns2_sns_sgsn_fsm_alloc(struct gprs_ns2_nse *nse, const char *id) +{ + struct osmo_fsm_inst *fi; + struct ns2_sns_state *gss; + + fi = osmo_fsm_inst_alloc(&gprs_ns2_sns_sgsn_fsm, nse, NULL, LOGL_DEBUG, id); + if (!fi) + return fi; + + gss = talloc_zero(fi, struct ns2_sns_state); + if (!gss) + goto err; + + fi->priv = gss; + gss->nse = nse; + gss->role = GPRS_SNS_ROLE_SGSN; + INIT_LLIST_HEAD(&gss->sns_endpoints); + INIT_LLIST_HEAD(&gss->binds); + INIT_LLIST_HEAD(&gss->procedures); + + return fi; +err: + osmo_fsm_inst_term(fi, OSMO_FSM_TERM_ERROR, NULL); + return NULL; +} + + + + +/* initialize osmo_ctx on main tread */ +static __attribute__((constructor)) void on_dso_load_ctx(void) +{ + OSMO_ASSERT(osmo_fsm_register(&gprs_ns2_sns_bss_fsm) == 0); + OSMO_ASSERT(osmo_fsm_register(&gprs_ns2_sns_sgsn_fsm) == 0); +} diff --git a/src/gb/gprs_ns2_udp.c b/src/gb/gprs_ns2_udp.c new file mode 100644 index 00000000..1dcc3030 --- /dev/null +++ b/src/gb/gprs_ns2_udp.c @@ -0,0 +1,597 @@ +/*! \file gprs_ns2_udp.c + * NS-over-UDP implementation. + * GPRS Networks Service (NS) messages on the Gb interface. + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> + +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/select.h> +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/core/socket.h> +#include <osmocom/gprs/gprs_ns2.h> + +#include "common_vty.h" +#include "gprs_ns2_internal.h" + + +static void free_bind(struct gprs_ns2_vc_bind *bind); + + +struct gprs_ns2_vc_driver vc_driver_ip = { + .name = "GB UDP IPv4/IPv6", + .free_bind = free_bind, +}; + +struct priv_bind { + struct osmo_io_fd *iofd; + struct osmo_sockaddr addr; + int dscp; + uint8_t priority; +}; + +struct priv_vc { + struct osmo_sockaddr remote; +}; + +/*! clean up all private driver state. Should be only called by gprs_ns2_free_bind() */ +static void free_bind(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + + if (!bind) + return; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + + priv = bind->priv; + + osmo_iofd_free(priv->iofd); + priv->iofd = NULL; + talloc_free(priv); +} + +static void free_vc(struct gprs_ns2_vc *nsvc) +{ + if (!nsvc) + return; + + if (!nsvc->priv) + return; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(nsvc->bind)); + talloc_free(nsvc->priv); + nsvc->priv = NULL; +} + +static void dump_vty(const struct gprs_ns2_vc_bind *bind, + struct vty *vty, bool stats) +{ + struct priv_bind *priv; + struct gprs_ns2_vc *nsvc; + struct osmo_sockaddr_str sockstr = {}; + unsigned long nsvcs = 0; + + if (!bind) + return; + + priv = bind->priv; + if (osmo_sockaddr_str_from_sockaddr(&sockstr, &priv->addr.u.sas)) + strcpy(sockstr.ip, "invalid"); + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + nsvcs++; + } + + vty_out(vty, "UDP bind: %s:%d DSCP: %d Priority: %u%s", sockstr.ip, sockstr.port, + priv->dscp, priv->priority, VTY_NEWLINE); + vty_out(vty, " IP-SNS signalling weight: %u data weight: %u%s", + bind->sns_sig_weight, bind->sns_data_weight, VTY_NEWLINE); + vty_out(vty, " %lu NS-VC:%s", nsvcs, VTY_NEWLINE); + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + ns2_vty_dump_nsvc(vty, nsvc, stats); + } +} + + +/*! Find a NS-VC by its remote socket address. + * \param[in] bind in which to search + * \param[in] rem_addr remote peer socket address to search + * \returns NS-VC matching sockaddr; NULL if none found */ +struct gprs_ns2_vc *gprs_ns2_nsvc_by_sockaddr_bind(struct gprs_ns2_vc_bind *bind, + const struct osmo_sockaddr *rem_addr) +{ + struct gprs_ns2_vc *nsvc; + struct priv_vc *vcpriv; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + + llist_for_each_entry(nsvc, &bind->nsvc, blist) { + vcpriv = nsvc->priv; + if (vcpriv->remote.u.sa.sa_family != rem_addr->u.sa.sa_family) + continue; + if (osmo_sockaddr_cmp(&vcpriv->remote, rem_addr)) + continue; + + return nsvc; + } + + return NULL; +} + +static inline int nsip_sendmsg(struct gprs_ns2_vc_bind *bind, + struct msgb *msg, + const struct osmo_sockaddr *dest) +{ + struct priv_bind *priv = bind->priv; + + return osmo_iofd_sendto_msgb(priv->iofd, msg, 0, dest); +} + +/*! send the msg and free it afterwards. + * \param nsvc NS-VC on which the message shall be sent + * \param msg message to be sent + * \return number of bytes transmitted; negative on error */ +static int nsip_vc_sendmsg(struct gprs_ns2_vc *nsvc, struct msgb *msg) +{ + int rc; + struct gprs_ns2_vc_bind *bind = nsvc->bind; + struct priv_vc *priv = nsvc->priv; + + rc = nsip_sendmsg(bind, msg, &priv->remote); + + return rc; +} + +static struct priv_vc *ns2_driver_alloc_vc(struct gprs_ns2_vc_bind *bind, struct gprs_ns2_vc *nsvc, const struct osmo_sockaddr *remote) +{ + struct priv_vc *priv = talloc_zero(bind, struct priv_vc); + if (!priv) + return NULL; + + nsvc->priv = priv; + priv->remote = *remote; + + return priv; +} + +static void handle_nsip_recvfrom(struct osmo_io_fd *iofd, int error, struct msgb *msg, + const struct osmo_sockaddr *saddr) +{ + int rc = 0; + struct gprs_ns2_vc_bind *bind = osmo_iofd_get_data(iofd); + struct gprs_ns2_vc *nsvc; + + struct msgb *reject; + + msg->l2h = msgb_data(msg); + + /* check if a vc is available */ + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, saddr); + if (!nsvc) { + /* VC not found */ + rc = ns2_create_vc(bind, msg, saddr, "newconnection", &reject, &nsvc); + switch (rc) { + case NS2_CS_FOUND: + break; + case NS2_CS_ERROR: + case NS2_CS_SKIPPED: + rc = 0; + goto out; + case NS2_CS_REJECTED: + /* nsip_sendmsg will free reject */ + rc = nsip_sendmsg(bind, reject, saddr); + goto out; + case NS2_CS_CREATED: + ns2_driver_alloc_vc(bind, nsvc, saddr); + /* only start the fsm for non SNS. SNS will take care of its own */ + if (nsvc->nse->dialect != GPRS_NS2_DIALECT_SNS) + ns2_vc_fsm_start(nsvc); + break; + } + } + + ns2_recv_vc(nsvc, msg); + return; + +out: + msgb_free(msg); +} + +static void handle_nsip_sendto(struct osmo_io_fd *iofd, int res, + struct msgb *msg, + const struct osmo_sockaddr *daddr) +{ + struct gprs_ns2_vc_bind *bind = osmo_iofd_get_data(iofd); + struct gprs_ns2_vc *nsvc; + + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, daddr); + if (!nsvc) + return; + + if (OSMO_LIKELY(res >= 0)) { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT, res); + } else { + RATE_CTR_INC_NS(nsvc, NS_CTR_PKTS_OUT_DROP); + RATE_CTR_ADD_NS(nsvc, NS_CTR_BYTES_OUT_DROP, msgb_length(msg)); + } +} + +/*! Find NS bind for a given socket address + * \param[in] nsi NS instance + * \param[in] sockaddr socket address to search for + * \return + */ +struct gprs_ns2_vc_bind *gprs_ns2_ip_bind_by_sockaddr(struct gprs_ns2_inst *nsi, + const struct osmo_sockaddr *sockaddr) +{ + struct gprs_ns2_vc_bind *bind; + const struct osmo_sockaddr *local; + + OSMO_ASSERT(nsi); + OSMO_ASSERT(sockaddr); + + llist_for_each_entry(bind, &nsi->binding, list) { + if (!gprs_ns2_is_ip_bind(bind)) + continue; + + local = gprs_ns2_ip_bind_sockaddr(bind); + if (!osmo_sockaddr_cmp(sockaddr, local)) + return bind; + } + + return NULL; +} + +/*! Bind to an IPv4/IPv6 address + * \param[in] nsi NS Instance in which to create the NSVC + * \param[in] local the local address to bind to + * \param[in] dscp the DSCP/TOS bits used for transmitted data + * \param[out] result pointer to the created bind or if a bind with the name exists return the bind. + * \return 0 on success; negative on error. -EALREADY returned in case a bind with the name exists */ +int gprs_ns2_ip_bind(struct gprs_ns2_inst *nsi, + const char *name, + const struct osmo_sockaddr *local, + int dscp, + struct gprs_ns2_vc_bind **result) +{ + struct gprs_ns2_vc_bind *bind; + struct priv_bind *priv; + int rc; + + struct osmo_io_ops ioops = { + .sendto_cb = &handle_nsip_sendto, + .recvfrom_cb = &handle_nsip_recvfrom, + }; + + if (local->u.sa.sa_family != AF_INET && local->u.sa.sa_family != AF_INET6) + return -EINVAL; + + if (dscp < 0 || dscp > 63) + return -EINVAL; + + bind = gprs_ns2_ip_bind_by_sockaddr(nsi, local); + if (bind) { + if (result) + *result = bind; + return -EBUSY; + } + + rc = ns2_bind_alloc(nsi, name, &bind); + if (rc < 0) + return rc; + + bind->driver = &vc_driver_ip; + bind->ll = GPRS_NS2_LL_UDP; + /* expect 100 mbit at least. + * TODO: ask the network layer about the speed. But would require + * notification on change. */ + bind->transfer_capability = 100; + bind->send_vc = nsip_vc_sendmsg; + bind->free_vc = free_vc; + bind->dump_vty = dump_vty; + + priv = bind->priv = talloc_zero(bind, struct priv_bind); + if (!priv) { + gprs_ns2_free_bind(bind); + return -ENOMEM; + } + + priv->addr = *local; + priv->dscp = dscp; + + rc = osmo_sock_init_osa(SOCK_DGRAM, IPPROTO_UDP, + local, NULL, + OSMO_SOCK_F_BIND | OSMO_SOCK_F_DSCP(priv->dscp)); + if (rc < 0) { + gprs_ns2_free_bind(bind); + return rc; + } + + priv->iofd = osmo_iofd_setup(bind, rc, "NS bind", OSMO_IO_FD_MODE_RECVFROM_SENDTO, &ioops, bind); + osmo_iofd_register(priv->iofd, rc); + osmo_iofd_set_alloc_info(priv->iofd, 4096, 128); + osmo_iofd_set_txqueue_max_length(priv->iofd, nsi->txqueue_max_length); + + /* IPv4: max fragmented payload can be (13 bit) * 8 byte => 65535. + * IPv6: max payload can be 65535 (RFC 2460). + * UDP header = 8 byte */ + bind->mtu = 65535 - 8; + if (result) + *result = bind; + + return 0; +} + +/*! Create new NS-VC to a given remote address + * \param[in] bind the bind we want to connect + * \param[in] nse NS entity to be used for the new NS-VC + * \param[in] remote remote address to connect to + * \return pointer to newly-allocated and connected NS-VC; NULL on error */ +struct gprs_ns2_vc *ns2_ip_bind_connect(struct gprs_ns2_vc_bind *bind, + struct gprs_ns2_nse *nse, + const struct osmo_sockaddr *remote) +{ + struct gprs_ns2_vc *nsvc; + const struct osmo_sockaddr *local; + struct priv_vc *priv; + enum gprs_ns2_vc_mode vc_mode; + char idbuf[256], tmp[INET6_ADDRSTRLEN + 8]; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + + vc_mode = ns2_dialect_to_vc_mode(nse->dialect); + if ((int) vc_mode == -1) { + LOGNSE(nse, LOGL_ERROR, "Can not derive vc mode from dialect %d. Maybe libosmocore is too old.\n", + nse->dialect); + return NULL; + } + + /* duplicate */ + if (gprs_ns2_nsvc_by_sockaddr_bind(bind, remote)) + return NULL; + + local = gprs_ns2_ip_bind_sockaddr(bind); + osmo_sockaddr_to_str_buf(tmp, sizeof(tmp), local); + snprintf(idbuf, sizeof(idbuf), "NSE%05u-NSVC-%s-%s-%s", nse->nsei, gprs_ns2_lltype_str(nse->ll), + tmp, osmo_sockaddr_to_str(remote)); + osmo_identifier_sanitize_buf(idbuf, NULL, '_'); + + nsvc = ns2_vc_alloc(bind, nse, true, vc_mode, idbuf); + if (!nsvc) + return NULL; + + nsvc->priv = talloc_zero(bind, struct priv_vc); + if (!nsvc->priv) { + gprs_ns2_free_nsvc(nsvc); + return NULL; + } + + priv = nsvc->priv; + priv->remote = *remote; + + return nsvc; +} + +/*! Return the socket address of the local peer of a NS-VC. + * \param[in] nsvc NS-VC whose local peer we want to know + * \return address of the local peer; NULL in case of error */ +const struct osmo_sockaddr *gprs_ns2_ip_vc_local(const struct gprs_ns2_vc *nsvc) +{ + struct priv_bind *priv; + + if (nsvc->bind->driver != &vc_driver_ip) + return NULL; + + priv = nsvc->bind->priv; + return &priv->addr; +} + +/*! Return the socket address of the remote peer of a NS-VC. + * \param[in] nsvc NS-VC whose remote peer we want to know + * \return address of the remote peer; NULL in case of error */ +const struct osmo_sockaddr *gprs_ns2_ip_vc_remote(const struct gprs_ns2_vc *nsvc) +{ + struct priv_vc *priv; + + if (nsvc->bind->driver != &vc_driver_ip) + return NULL; + + priv = nsvc->priv; + return &priv->remote; +} + +/*! Compare the NS-VC with the given parameter + * \param[in] nsvc NS-VC to compare with + * \param[in] local The local address + * \param[in] remote The remote address + * \param[in] nsvci NS-VCI will only be used if the NS-VC in BLOCKRESET mode otherwise NS-VCI isn't applicable. + * \return true if the NS-VC has the same properties as given + */ +bool gprs_ns2_ip_vc_equal(const struct gprs_ns2_vc *nsvc, + const struct osmo_sockaddr *local, + const struct osmo_sockaddr *remote, + uint16_t nsvci) +{ + struct priv_vc *vpriv; + struct priv_bind *bpriv; + + if (nsvc->bind->driver != &vc_driver_ip) + return false; + + vpriv = nsvc->priv; + bpriv = nsvc->bind->priv; + + if (osmo_sockaddr_cmp(local, &bpriv->addr)) + return false; + + if (osmo_sockaddr_cmp(remote, &vpriv->remote)) + return false; + + if (nsvc->mode == GPRS_NS2_VC_MODE_BLOCKRESET) + if (nsvc->nsvci != nsvci) + return false; + + return true; +} + +/*! Return the locally bound socket address of the bind. + * \param[in] bind The bind whose local address we want to know + * \return address of the local bind */ +const struct osmo_sockaddr *gprs_ns2_ip_bind_sockaddr(struct gprs_ns2_vc_bind *bind) +{ + struct priv_bind *priv; + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + + priv = bind->priv; + return &priv->addr; +} + +/*! Is the given bind an IP bind? */ +int gprs_ns2_is_ip_bind(struct gprs_ns2_vc_bind *bind) +{ + return (bind->driver == &vc_driver_ip); +} + +/*! Set the DSCP (TOS) bit value of the given bind. */ +int gprs_ns2_ip_bind_set_dscp(struct gprs_ns2_vc_bind *bind, int dscp) +{ + struct priv_bind *priv; + int rc = 0; + + if (dscp < 0 || dscp > 63) + return -EINVAL; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + priv = bind->priv; + + if (dscp != priv->dscp) { + priv->dscp = dscp; + + rc = osmo_sock_set_dscp(osmo_iofd_get_fd(priv->iofd), dscp); + if (rc < 0) { + LOGBIND(bind, LOGL_ERROR, "Failed to set the DSCP to %u with ret(%d) errno(%d)\n", + dscp, rc, errno); + } + } + + return rc; +} + +/*! Set the socket priority of the given bind. */ +int gprs_ns2_ip_bind_set_priority(struct gprs_ns2_vc_bind *bind, uint8_t priority) +{ + struct priv_bind *priv; + int rc = 0; + + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + priv = bind->priv; + + if (priority != priv->priority) { + priv->priority = priority; + + rc = osmo_sock_set_priority(osmo_iofd_get_fd(priv->iofd), priority); + if (rc < 0) { + LOGBIND(bind, LOGL_ERROR, "Failed to set the priority to %u with ret(%d) errno(%d)\n", + priority, rc, errno); + } + } + + return rc; +} + + +/*! Count UDP binds compatible with remote */ +int ns2_ip_count_bind(struct gprs_ns2_inst *nsi, struct osmo_sockaddr *remote) +{ + struct gprs_ns2_vc_bind *bind; + const struct osmo_sockaddr *sa; + int count = 0; + + llist_for_each_entry(bind, &nsi->binding, list) { + if (!gprs_ns2_is_ip_bind(bind)) + continue; + + sa = gprs_ns2_ip_bind_sockaddr(bind); + if (!sa) + continue; + + if (sa->u.sa.sa_family == remote->u.sa.sa_family) + count++; + } + + return count; +} + +/* return the matching bind by index */ +struct gprs_ns2_vc_bind *ns2_ip_get_bind_by_index(struct gprs_ns2_inst *nsi, + struct osmo_sockaddr *remote, + int index) +{ + struct gprs_ns2_vc_bind *bind; + const struct osmo_sockaddr *sa; + int i = 0; + + llist_for_each_entry(bind, &nsi->binding, list) { + if (!gprs_ns2_is_ip_bind(bind)) + continue; + + sa = gprs_ns2_ip_bind_sockaddr(bind); + if (!sa) + continue; + + if (sa->u.sa.sa_family == remote->u.sa.sa_family) { + if (index == i) + return bind; + i++; + } + } + + return NULL; +} + +void ns2_ip_set_txqueue_max_length(struct gprs_ns2_vc_bind *bind, unsigned int max_length) +{ + struct priv_bind *priv = bind->priv; + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + + osmo_iofd_set_txqueue_max_length(priv->iofd, max_length); +} + +/*! set the signalling and data weight for this bind + * \param[in] bind + * \param[in] signalling the signalling weight + * \param[in] data the data weight + */ +void gprs_ns2_ip_bind_set_sns_weight(struct gprs_ns2_vc_bind *bind, uint8_t signalling, uint8_t data) +{ + OSMO_ASSERT(gprs_ns2_is_ip_bind(bind)); + bind->sns_sig_weight = signalling; + bind->sns_data_weight = data; + ns2_sns_update_weights(bind); +} diff --git a/src/gb/gprs_ns2_vc_fsm.c b/src/gb/gprs_ns2_vc_fsm.c new file mode 100644 index 00000000..9cd83c4f --- /dev/null +++ b/src/gb/gprs_ns2_vc_fsm.c @@ -0,0 +1,992 @@ +/*! \file gprs_ns2_vc_fsm.c + * NS virtual circuit FSM implementation + * 3GPP TS 08.16 version 8.0.1 Release 1999 / ETSI TS 101 299 V8.0.1 (2002-05) + * as well as its successor 3GPP TS 48.016 */ + +/* (C) 2020 sysmocom - s.f.m.c. GmbH + * Author: Alexander Couzens <lynxis@fe80.eu> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +/* The BSS NSE only has one SGSN IP address configured, and it will use the SNS procedures + * to communicated its local IPs/ports as well as all the SGSN side IPs/ports and + * associated weights. In theory, the BSS then uses this to establish a full mesh + * of NSVCs between all BSS-side IPs/ports and SGSN-side IPs/ports */ + +#include <errno.h> + +#include <netinet/in.h> +#include <arpa/inet.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/gsm/prim.h> +#include <osmocom/gsm/tlv.h> +#include <osmocom/gprs/gprs_msgb.h> +#include <osmocom/gprs/protocol/gsm_08_16.h> + +#include "gprs_ns2_internal.h" + +#define S(x) (1 << (x)) + +struct gprs_ns2_vc_priv { + struct gprs_ns2_vc *nsvc; + /* how often the timer was triggered */ + int N; + /* The initiator is responsible to UNBLOCK the VC. The BSS is usually the initiator. + * It can change during runtime. The side which blocks an unblocked side.*/ + bool initiator; + bool initiate_block; + bool initiate_reset; + /* if unitdata is forwarded to the user */ + bool accept_unitdata; + + /* the alive counter is present in all states */ + struct { + struct osmo_timer_list timer; + enum ns2_timeout mode; + int N; + struct timespec timer_started; + } alive; +}; + + +/* The FSM covers both the VC with RESET/BLOCK and without RESET/BLOCK procedure.. + * + * With RESET/BLOCK, the state should follow: + * - UNCONFIGURED -> RESET -> BLOCK -> UNBLOCKED + * + * Without RESET/BLOCK, the state should follow: + * - UNCONFIGURED -> RECOVERY -> UNBLOCKED + * + * The UNBLOCKED and TEST states are used to send ALIVE PDU using the timeout Tns-test and Tns-alive. + * UNBLOCKED -> TEST: on expire of Tns-Test, send Alive PDU. + * TEST -> UNBLOCKED: on receive of Alive_Ack PDU, go into UNBLOCKED. + * + * The RECOVERY state is used as intermediate, because a VC is only valid if it received an Alive ACK when + * not using RESET/BLOCK procedure. + */ + +enum gprs_ns2_vc_state { + GPRS_NS2_ST_UNCONFIGURED, + GPRS_NS2_ST_RESET, + GPRS_NS2_ST_BLOCKED, + GPRS_NS2_ST_UNBLOCKED, /* allows sending NS_UNITDATA */ + + GPRS_NS2_ST_RECOVERING, /* only used when not using RESET/BLOCK procedure */ +}; + +enum gprs_ns2_vc_event { + GPRS_NS2_EV_REQ_START, + + /* received messages */ + GPRS_NS2_EV_RX_RESET, + GPRS_NS2_EV_RX_RESET_ACK, + GPRS_NS2_EV_RX_UNBLOCK, + GPRS_NS2_EV_RX_UNBLOCK_ACK, + GPRS_NS2_EV_RX_BLOCK, + GPRS_NS2_EV_RX_BLOCK_ACK, + GPRS_NS2_EV_RX_ALIVE, + GPRS_NS2_EV_RX_ALIVE_ACK, + GPRS_NS2_EV_RX_STATUS, + + GPRS_NS2_EV_RX_UNITDATA, + + GPRS_NS2_EV_REQ_FORCE_UNCONFIGURED, /* called via vty for tests */ + GPRS_NS2_EV_REQ_OM_RESET, /* vty cmd: reset */ + GPRS_NS2_EV_REQ_OM_BLOCK, /* vty cmd: block */ + GPRS_NS2_EV_REQ_OM_UNBLOCK, /* vty cmd: unblock*/ + GPRS_NS2_EV_RX_BLOCK_FOREIGN, /* received a BLOCK over another NSVC */ +}; + +static const struct value_string ns2_vc_event_names[] = { + { GPRS_NS2_EV_REQ_START, "REQ-START" }, + { GPRS_NS2_EV_RX_RESET, "RX-RESET" }, + { GPRS_NS2_EV_RX_RESET_ACK, "RX-RESET_ACK" }, + { GPRS_NS2_EV_RX_UNBLOCK, "RX-UNBLOCK" }, + { GPRS_NS2_EV_RX_UNBLOCK_ACK, "RX-UNBLOCK_ACK" }, + { GPRS_NS2_EV_RX_BLOCK, "RX-BLOCK" }, + { GPRS_NS2_EV_RX_BLOCK_FOREIGN, "RX-BLOCK_FOREIGN" }, + { GPRS_NS2_EV_RX_BLOCK_ACK, "RX-BLOCK_ACK" }, + { GPRS_NS2_EV_RX_ALIVE, "RX-ALIVE" }, + { GPRS_NS2_EV_RX_ALIVE_ACK, "RX-ALIVE_ACK" }, + { GPRS_NS2_EV_RX_STATUS, "RX-STATUS" }, + { GPRS_NS2_EV_RX_UNITDATA, "RX-UNITDATA" }, + { GPRS_NS2_EV_REQ_FORCE_UNCONFIGURED, "REQ-FORCE_UNCONFIGURED" }, + { GPRS_NS2_EV_REQ_OM_RESET, "REQ-O&M-RESET"}, + { GPRS_NS2_EV_REQ_OM_BLOCK, "REQ-O&M-BLOCK"}, + { GPRS_NS2_EV_REQ_OM_UNBLOCK, "REQ-O&M-UNBLOCK"}, + { 0, NULL } +}; + +static inline struct gprs_ns2_inst *ns_inst_from_fi(struct osmo_fsm_inst *fi) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + return priv->nsvc->nse->nsi; +} + +/* Start the NS-TEST procedure, either with transmitting a tx_alive, + * (start_tx_alive==true) or with starting tns-test */ +static void start_test_procedure(struct osmo_fsm_inst *fi, bool start_tx_alive) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = priv->nsvc->nse->nsi; + unsigned int tout_idx; + + if (osmo_timer_pending(&priv->alive.timer)) { + if (start_tx_alive) { + if (priv->alive.mode == NS_TOUT_TNS_ALIVE) + return; + } else { + if (priv->alive.mode == NS_TOUT_TNS_TEST) + return; + } + } + + priv->alive.N = 0; + + if (start_tx_alive) { + priv->alive.mode = NS_TOUT_TNS_ALIVE; + osmo_clock_gettime(CLOCK_MONOTONIC, &priv->alive.timer_started); + ns2_tx_alive(priv->nsvc); + tout_idx = NS_TOUT_TNS_ALIVE; + } else { + priv->alive.mode = NS_TOUT_TNS_TEST; + tout_idx = NS_TOUT_TNS_TEST; + } + LOGPFSML(fi, LOGL_DEBUG, "Starting Tns-%s of %u seconds\n", + tout_idx == NS_TOUT_TNS_ALIVE ? "alive" : "test", nsi->timeout[tout_idx]); + osmo_timer_schedule(&priv->alive.timer, nsi->timeout[tout_idx], 0); +} + +static void stop_test_procedure(struct gprs_ns2_vc_priv *priv) +{ + osmo_stat_item_set(osmo_stat_item_group_get_item(priv->nsvc->statg, NS_STAT_ALIVE_DELAY), 0); + osmo_timer_del(&priv->alive.timer); +} + +/* how many milliseconds have expired since the last alive timer start? */ +static int alive_timer_elapsed_ms(struct gprs_ns2_vc_priv *priv) +{ + struct timespec now, elapsed; + + if (osmo_clock_gettime(CLOCK_MONOTONIC, &now) != 0) + return 0; + + timespecsub(&now, &priv->alive.timer_started, &elapsed); + return elapsed.tv_sec * 1000 + (elapsed.tv_nsec / 1000000); +} + +/* we just received a NS-ALIVE-ACK; re-schedule after Tns-test */ +static void recv_test_procedure(struct osmo_fsm_inst *fi) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct gprs_ns2_vc *nsvc = priv->nsvc; + + /* ignoring ACKs without sending an ALIVE */ + if (priv->alive.mode != NS_TOUT_TNS_ALIVE) + return; + + priv->alive.mode = NS_TOUT_TNS_TEST; + osmo_timer_schedule(&priv->alive.timer, nsi->timeout[NS_TOUT_TNS_TEST], 0); + osmo_stat_item_set(osmo_stat_item_group_get_item(nsvc->statg, NS_STAT_ALIVE_DELAY), + alive_timer_elapsed_ms(priv)); +} + + +static void alive_timeout_handler(void *data) +{ + struct osmo_fsm_inst *fi = data; + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct gprs_ns2_vc_priv *priv = fi->priv; + + switch (priv->alive.mode) { + case NS_TOUT_TNS_TEST: + priv->alive.mode = NS_TOUT_TNS_ALIVE; + priv->alive.N = 0; + osmo_clock_gettime(CLOCK_MONOTONIC, &priv->alive.timer_started); + ns2_tx_alive(priv->nsvc); + osmo_timer_schedule(&priv->alive.timer, nsi->timeout[NS_TOUT_TNS_ALIVE], 0); + break; + case NS_TOUT_TNS_ALIVE: + RATE_CTR_INC_NS(priv->nsvc, NS_CTR_LOST_ALIVE); + priv->alive.N++; + + if (priv->alive.N <= nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES]) { + /* retransmission */ + ns2_tx_alive(priv->nsvc); + osmo_timer_schedule(&priv->alive.timer, nsi->timeout[NS_TOUT_TNS_ALIVE], 0); + } else { + /* lost connection */ + if (priv->nsvc->mode == GPRS_NS2_VC_MODE_BLOCKRESET) { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], 0); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RECOVERING, nsi->timeout[NS_TOUT_TNS_ALIVE], 0); + } + } + break; + default: + break; + } +} + + +static void ns2_st_unconfigured_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + stop_test_procedure(fi->priv); + ns2_nse_notify_unblocked(priv->nsvc, false); +} + +static void ns2_st_unconfigured(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = priv->nsvc->nse->nsi; + + priv->initiate_reset = priv->initiate_block = priv->initiator; + priv->nsvc->om_blocked = false; + + switch (event) { + case GPRS_NS2_EV_REQ_START: + switch (priv->nsvc->mode) { + case GPRS_NS2_VC_MODE_ALIVE: + if (priv->nsvc->nse->dialect == GPRS_NS2_DIALECT_SNS) { + /* In IP-SNS, the NS-VC are assumed initially alive, until the alive + * procedure should fail at some future point */ + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_UNBLOCKED, 0, 0); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RECOVERING, nsi->timeout[NS_TOUT_TNS_ALIVE], NS_TOUT_TNS_ALIVE); + } + break; + case GPRS_NS2_VC_MODE_BLOCKRESET: + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], NS_TOUT_TNS_RESET); + break; + } + break; + default: + OSMO_ASSERT(0); + } +} + + +static void ns2_st_reset_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + if (old_state != GPRS_NS2_ST_RESET) + priv->N = 0; + + priv->accept_unitdata = false; + if (priv->initiate_reset) + ns2_tx_reset(priv->nsvc, NS_CAUSE_OM_INTERVENTION); + + stop_test_procedure(priv); + ns2_nse_notify_unblocked(priv->nsvc, false); +} + +static void ns2_st_reset(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct gprs_ns2_vc_priv *priv = fi->priv; + + if (priv->initiate_reset) { + switch (event) { + case GPRS_NS2_EV_RX_RESET: + ns2_tx_reset_ack(priv->nsvc); + /* fall-through */ + case GPRS_NS2_EV_RX_RESET_ACK: + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, + nsi->timeout[NS_TOUT_TNS_BLOCK], NS_TOUT_TNS_BLOCK); + break; + } + } else { + /* we are on the receiving end */ + switch (event) { + case GPRS_NS2_EV_RX_RESET: + ns2_tx_reset_ack(priv->nsvc); + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, + 0, 0); + break; + } + } +} + +static void ns2_st_blocked_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + if (old_state != GPRS_NS2_ST_BLOCKED) { + priv->N = 0; + RATE_CTR_INC_NS(priv->nsvc, NS_CTR_BLOCKED); + } + + ns2_nse_notify_unblocked(priv->nsvc, false); + if (priv->nsvc->om_blocked) { + /* we are already blocked after a RESET */ + if (old_state == GPRS_NS2_ST_RESET) { + osmo_timer_del(&fi->timer); + } else { + ns2_tx_block(priv->nsvc, NS_CAUSE_OM_INTERVENTION, NULL); + } + } else if (priv->initiate_block) { + ns2_tx_unblock(priv->nsvc); + } + + start_test_procedure(fi, true); +} + +static void ns2_st_blocked(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + if (priv->nsvc->om_blocked) { + switch (event) { + case GPRS_NS2_EV_RX_BLOCK_ACK: + priv->accept_unitdata = false; + osmo_timer_del(&fi->timer); + break; + case GPRS_NS2_EV_RX_BLOCK: + ns2_tx_block_ack(priv->nsvc, NULL); + /* fall through */ + case GPRS_NS2_EV_RX_BLOCK_FOREIGN: + /* the BLOCK ACK for foreign BLOCK PDUs (rx over another nsvc) will be sent + * from the receiving nsvc */ + priv->accept_unitdata = false; + osmo_timer_del(&fi->timer); + break; + case GPRS_NS2_EV_RX_UNBLOCK: + priv->accept_unitdata = false; + ns2_tx_block(priv->nsvc, NS_CAUSE_OM_INTERVENTION, NULL); + osmo_timer_add(&fi->timer); + break; + } + } else if (priv->initiate_block) { + switch (event) { + case GPRS_NS2_EV_RX_BLOCK_FOREIGN: + /* the block ack will be sent by the rx NSVC */ + break; + case GPRS_NS2_EV_RX_BLOCK: + /* TODO: BLOCK is a UNBLOCK_NACK */ + ns2_tx_block_ack(priv->nsvc, NULL); + break; + case GPRS_NS2_EV_RX_UNBLOCK: + ns2_tx_unblock_ack(priv->nsvc); + /* fall through */ + case GPRS_NS2_EV_RX_UNBLOCK_ACK: + priv->accept_unitdata = true; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_UNBLOCKED, + 0, NS_TOUT_TNS_TEST); + break; + } + } else { + /* we are on the receiving end. The initiator who sent RESET is responsible to UNBLOCK! */ + switch (event) { + case GPRS_NS2_EV_RX_BLOCK_FOREIGN: + /* the block ack will be sent by the rx NSVC */ + break; + case GPRS_NS2_EV_RX_BLOCK: + ns2_tx_block_ack(priv->nsvc, NULL); + break; + case GPRS_NS2_EV_RX_UNBLOCK: + ns2_tx_unblock_ack(priv->nsvc); + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_UNBLOCKED, + 0, 0); + break; + } + } +} + +static void ns2_st_unblocked_on_enter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_vc *nsvc = priv->nsvc; + struct gprs_ns2_nse *nse = nsvc->nse; + + if (old_state != GPRS_NS2_ST_UNBLOCKED) { + RATE_CTR_INC_NS(nsvc, NS_CTR_UNBLOCKED); + osmo_clock_gettime(CLOCK_MONOTONIC, &nsvc->ts_alive_change); + } + + priv->accept_unitdata = true; + ns2_nse_notify_unblocked(nsvc, true); + ns2_prim_status_ind(nse, nsvc, 0, GPRS_NS2_AFF_CAUSE_VC_RECOVERY); + + /* the closest interpretation of the spec would start Tns-test here first, + * and only send a NS-ALIVE after Tns-test has expired (i.e. setting the + * second argument to 'false'. However, being quick in detecting unavailability + * of a NS-VC seems like a good idea */ + start_test_procedure(fi, true); +} + +static void ns2_st_unblocked(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + switch (event) { + case GPRS_NS2_EV_RX_UNBLOCK: + ns2_tx_unblock_ack(priv->nsvc); + break; + case GPRS_NS2_EV_RX_BLOCK: + ns2_tx_block_ack(priv->nsvc, NULL); + /* fall through */ + case GPRS_NS2_EV_RX_BLOCK_FOREIGN: + /* the BLOCK ACK for foreign BLOCK PDUs (rx over another nsvc) will be sent + * from the receiving nsvc */ + priv->initiate_block = false; + priv->accept_unitdata = false; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, + 0, 2); + break; + } +} + +static void ns2_st_alive(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case GPRS_NS2_EV_RX_ALIVE_ACK: + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_UNBLOCKED, 0, 0); + break; + } +} + +static void ns2_st_alive_onenter(struct osmo_fsm_inst *fi, uint32_t old_state) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + + priv->alive.mode = NS_TOUT_TNS_TEST; + osmo_timer_schedule(&priv->alive.timer, nsi->timeout[NS_TOUT_TNS_TEST], 0); + + if (old_state != GPRS_NS2_ST_RECOVERING) + priv->N = 0; + + start_test_procedure(fi, true); + ns2_nse_notify_unblocked(priv->nsvc, false); +} + +static const struct osmo_fsm_state ns2_vc_states[] = { + [GPRS_NS2_ST_UNCONFIGURED] = { + .in_event_mask = S(GPRS_NS2_EV_REQ_START), + .out_state_mask = S(GPRS_NS2_ST_RESET) | + S(GPRS_NS2_ST_RECOVERING) | + S(GPRS_NS2_ST_UNBLOCKED), + .name = "UNCONFIGURED", + .action = ns2_st_unconfigured, + .onenter = ns2_st_unconfigured_onenter, + }, + [GPRS_NS2_ST_RESET] = { + .in_event_mask = S(GPRS_NS2_EV_RX_RESET_ACK) | S(GPRS_NS2_EV_RX_RESET), + .out_state_mask = S(GPRS_NS2_ST_RESET) | + S(GPRS_NS2_ST_BLOCKED) | + S(GPRS_NS2_ST_UNCONFIGURED), + .name = "RESET", + .action = ns2_st_reset, + .onenter = ns2_st_reset_onenter, + }, + [GPRS_NS2_ST_BLOCKED] = { + .in_event_mask = S(GPRS_NS2_EV_RX_BLOCK) | S(GPRS_NS2_EV_RX_BLOCK_ACK) | + S(GPRS_NS2_EV_RX_UNBLOCK) | S(GPRS_NS2_EV_RX_UNBLOCK_ACK) | + S(GPRS_NS2_EV_RX_BLOCK_FOREIGN), + .out_state_mask = S(GPRS_NS2_ST_RESET) | + S(GPRS_NS2_ST_UNBLOCKED) | + S(GPRS_NS2_ST_BLOCKED) | + S(GPRS_NS2_ST_UNCONFIGURED), + .name = "BLOCKED", + .action = ns2_st_blocked, + .onenter = ns2_st_blocked_onenter, + }, + [GPRS_NS2_ST_UNBLOCKED] = { + .in_event_mask = S(GPRS_NS2_EV_RX_BLOCK) | S(GPRS_NS2_EV_RX_UNBLOCK_ACK) | + S(GPRS_NS2_EV_RX_UNBLOCK) | S(GPRS_NS2_EV_RX_BLOCK_FOREIGN), + .out_state_mask = S(GPRS_NS2_ST_RESET) | S(GPRS_NS2_ST_RECOVERING) | + S(GPRS_NS2_ST_BLOCKED) | + S(GPRS_NS2_ST_UNCONFIGURED), + .name = "UNBLOCKED", + .action = ns2_st_unblocked, + .onenter = ns2_st_unblocked_on_enter, + }, + + /* ST_RECOVERING is only used on VC without RESET/BLOCK */ + [GPRS_NS2_ST_RECOVERING] = { + .in_event_mask = S(GPRS_NS2_EV_RX_ALIVE_ACK), + .out_state_mask = S(GPRS_NS2_ST_RECOVERING) | + S(GPRS_NS2_ST_UNBLOCKED) | + S(GPRS_NS2_ST_UNCONFIGURED), + .name = "RECOVERING", + .action = ns2_st_alive, + .onenter = ns2_st_alive_onenter, + }, +}; + +static int ns2_vc_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct gprs_ns2_vc_priv *priv = fi->priv; + + switch (fi->state) { + case GPRS_NS2_ST_RESET: + if (priv->initiate_reset) { + RATE_CTR_INC_NS(priv->nsvc, NS_CTR_LOST_RESET); + priv->N++; + if (priv->N <= nsi->timeout[NS_TOUT_TNS_RESET_RETRIES]) { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], 0); + } else { + priv->N = 0; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], 0); + } + } + break; + case GPRS_NS2_ST_BLOCKED: + if (priv->initiate_block) { + priv->N++; + if (priv->nsvc->om_blocked) { + if (priv->N <= nsi->timeout[NS_TOUT_TNS_BLOCK_RETRIES]) { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, nsi->timeout[NS_TOUT_TNS_BLOCK], 0); + } else { + /* 7.2 stop accepting data when BLOCK PDU not responded */ + priv->accept_unitdata = false; + } + } else { + if (priv->N <= nsi->timeout[NS_TOUT_TNS_BLOCK_RETRIES]) { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, nsi->timeout[NS_TOUT_TNS_BLOCK], 0); + } else { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], 0); + } + } + } + break; + case GPRS_NS2_ST_RECOVERING: + if (priv->initiate_reset) { + priv->N++; + if (priv->N <= nsi->timeout[NS_TOUT_TNS_ALIVE_RETRIES]) { + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RECOVERING, 0, 0); + } else { + priv->N = 0; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RECOVERING, 0, 0); + } + break; + } + break; + } + return 0; +} + +static void ns2_recv_unitdata(struct osmo_fsm_inst *fi, + struct msgb *msg) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct gprs_ns_hdr *nsh = (struct gprs_ns_hdr *) msg->l2h; + struct osmo_gprs_ns2_prim nsp = {}; + uint16_t bvci; + + if (msgb_l2len(msg) < sizeof(*nsh) + 3) { + msgb_free(msg); + return; + } + + /* TODO: 7.1: For an IP sub-network, an NS-UNITDATA PDU + * for a PTP BVC may indicate a request to change the IP endpoint + * and/or a response to a change in the IP endpoint. */ + + /* TODO: nsh->data[0] -> C/R only valid in IP SNS */ + bvci = nsh->data[1] << 8 | nsh->data[2]; + + msg->l3h = &nsh->data[3]; + nsp.bvci = bvci; + nsp.nsei = priv->nsvc->nse->nsei; + + /* 10.3.9 NS SDU Control Bits */ + if (nsh->data[0] & 0x1) + nsp.u.unitdata.change = GPRS_NS2_ENDPOINT_REQUEST_CHANGE; + + osmo_prim_init(&nsp.oph, SAP_NS, GPRS_NS2_PRIM_UNIT_DATA, + PRIM_OP_INDICATION, msg); + nsi->cb(&nsp.oph, nsi->cb_data); +} + +static void ns2_vc_fsm_allstate_action(struct osmo_fsm_inst *fi, + uint32_t event, + void *data) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + struct gprs_ns2_inst *nsi = ns_inst_from_fi(fi); + struct tlv_parsed *tp; + struct msgb *msg = data; + uint8_t cause; + + switch (event) { + case GPRS_NS2_EV_REQ_OM_RESET: + if (priv->nsvc->mode != GPRS_NS2_VC_MODE_BLOCKRESET) + break; + /* move the FSM into reset */ + if (fi->state != GPRS_NS2_ST_RESET) { + priv->initiate_reset = true; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], NS_TOUT_TNS_RESET); + } + break; + case GPRS_NS2_EV_RX_RESET: + if (priv->nsvc->mode != GPRS_NS2_VC_MODE_BLOCKRESET) + break; + + /* move the FSM into reset */ + if (fi->state != GPRS_NS2_ST_RESET) { + priv->initiate_reset = false; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], NS_TOUT_TNS_RESET); + } + /* pass the event down into FSM action */ + ns2_st_reset(fi, event, data); + break; + case GPRS_NS2_EV_RX_ALIVE: + switch (fi->state) { + case GPRS_NS2_ST_UNCONFIGURED: + case GPRS_NS2_ST_RESET: + /* ignore ALIVE */ + break; + default: + ns2_tx_alive_ack(priv->nsvc); + } + break; + case GPRS_NS2_EV_RX_ALIVE_ACK: + /* for VCs without RESET/BLOCK/UNBLOCK, the connections comes after ALIVE_ACK unblocked */ + if (fi->state == GPRS_NS2_ST_RECOVERING) + ns2_st_alive(fi, event, data); + else + recv_test_procedure(fi); + break; + case GPRS_NS2_EV_RX_UNITDATA: + /* UNITDATA has to handle the release of msg. + * If send upwards (gprs_ns2_recv_unitdata) it must NOT free + * the msg, the upper layer has to do it. + * Otherwise the msg must be freed. + */ + + LOG_NS_DATA(priv->nsvc, "Rx", NS_PDUT_UNITDATA, LOGL_INFO, "\n"); + switch (fi->state) { + case GPRS_NS2_ST_BLOCKED: + /* 7.2.1: the BLOCKED_ACK might be lost */ + if (priv->accept_unitdata) { + ns2_recv_unitdata(fi, msg); + return; + } + + ns2_tx_status(priv->nsvc, + NS_CAUSE_NSVC_BLOCKED, + 0, msg, NULL); + break; + /* ALIVE can receive UNITDATA if the ALIVE_ACK is lost */ + case GPRS_NS2_ST_RECOVERING: + case GPRS_NS2_ST_UNBLOCKED: + ns2_recv_unitdata(fi, msg); + return; + } + + msgb_free(msg); + break; + case GPRS_NS2_EV_REQ_FORCE_UNCONFIGURED: + if (fi->state != GPRS_NS2_ST_UNCONFIGURED) { + /* Force the NSVC back to its initial state */ + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_UNCONFIGURED, 0, 0); + return; + } + break; + case GPRS_NS2_EV_REQ_OM_BLOCK: + /* vty cmd: block */ + priv->initiate_block = true; + priv->nsvc->om_blocked = true; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, nsi->timeout[NS_TOUT_TNS_BLOCK], 0); + break; + case GPRS_NS2_EV_REQ_OM_UNBLOCK: + /* vty cmd: unblock*/ + if (!priv->nsvc->om_blocked) + return; + priv->nsvc->om_blocked = false; + if (fi->state == GPRS_NS2_ST_BLOCKED) + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, nsi->timeout[NS_TOUT_TNS_BLOCK], 0); + break; + case GPRS_NS2_EV_RX_STATUS: + tp = data; + cause = tlvp_val8(tp, NS_IE_CAUSE, 0); + switch (cause) { + case NS_CAUSE_NSVC_BLOCKED: + if (fi->state != GPRS_NS2_ST_BLOCKED) { + LOG_NS_SIGNAL(priv->nsvc, "Rx", NS_PDUT_STATUS, LOGL_ERROR, ": remote side reported blocked state.\n"); + priv->initiate_block = false; + priv->accept_unitdata = false; + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_BLOCKED, nsi->timeout[NS_TOUT_TNS_BLOCK], 0); + } + break; + case NS_CAUSE_NSVC_UNKNOWN: + if (fi->state != GPRS_NS2_ST_RESET && fi->state != GPRS_NS2_ST_UNCONFIGURED) { + LOG_NS_SIGNAL(priv->nsvc, "Rx", NS_PDUT_STATUS, LOGL_ERROR, ": remote side reported unknown nsvc.\n"); + osmo_fsm_inst_state_chg(fi, GPRS_NS2_ST_RESET, nsi->timeout[NS_TOUT_TNS_RESET], 0); + } + break; + } + + break; + } +} + +static void ns2_vc_fsm_clean(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause) +{ + struct gprs_ns2_vc_priv *priv = fi->priv; + + osmo_timer_del(&priv->alive.timer); +} + +static struct osmo_fsm ns2_vc_fsm = { + .name = "GPRS-NS2-VC", + .states = ns2_vc_states, + .num_states = ARRAY_SIZE(ns2_vc_states), + .allstate_event_mask = S(GPRS_NS2_EV_RX_UNITDATA) | + S(GPRS_NS2_EV_RX_RESET) | + S(GPRS_NS2_EV_RX_ALIVE) | + S(GPRS_NS2_EV_RX_ALIVE_ACK) | + S(GPRS_NS2_EV_RX_STATUS) | + S(GPRS_NS2_EV_REQ_FORCE_UNCONFIGURED) | + S(GPRS_NS2_EV_REQ_OM_RESET) | + S(GPRS_NS2_EV_REQ_OM_BLOCK) | + S(GPRS_NS2_EV_REQ_OM_UNBLOCK), + .allstate_action = ns2_vc_fsm_allstate_action, + .cleanup = ns2_vc_fsm_clean, + .timer_cb = ns2_vc_fsm_timer_cb, + .event_names = ns2_vc_event_names, + .pre_term = NULL, + .log_subsys = DLNS, +}; + +/*! + * \brief gprs_ns2_vc_fsm_alloc + * \param ctx + * \param vc + * \param id a char representation of the virtual curcuit + * \param initiator initiator is the site which starts the connection. Usually the BSS. + * \return NULL on error, otherwise the fsm + */ +struct osmo_fsm_inst *ns2_vc_fsm_alloc(struct gprs_ns2_vc *nsvc, + const char *id, bool initiator) +{ + struct osmo_fsm_inst *fi; + struct gprs_ns2_vc_priv *priv; + + fi = osmo_fsm_inst_alloc(&ns2_vc_fsm, nsvc, NULL, LOGL_DEBUG, id); + if (!fi) + return fi; + + nsvc->fi = fi; + priv = fi->priv = talloc_zero(fi, struct gprs_ns2_vc_priv); + priv->nsvc = nsvc; + priv->initiator = initiator; + + osmo_timer_setup(&priv->alive.timer, alive_timeout_handler, fi); + + return fi; +} + +/*! Start a NS-VC FSM. + * \param nsvc the virtual circuit + * \return 0 on success; negative on error */ +int ns2_vc_fsm_start(struct gprs_ns2_vc *nsvc) +{ + /* allows to call this function even for started nsvc by gprs_ns2_start_alive_all_nsvcs */ + if (nsvc->fi->state == GPRS_NS2_ST_UNCONFIGURED) + return osmo_fsm_inst_dispatch(nsvc->fi, GPRS_NS2_EV_REQ_START, NULL); + return 0; +} + +/*! Reset a NS-VC FSM. + * \param nsvc the virtual circuit + * \return 0 on success; negative on error */ +int ns2_vc_force_unconfigured(struct gprs_ns2_vc *nsvc) +{ + return osmo_fsm_inst_dispatch(nsvc->fi, GPRS_NS2_EV_REQ_FORCE_UNCONFIGURED, NULL); +} + +/*! Block a NS-VC. + * \param nsvc the virtual circuit + * \return 0 on success; negative on error */ +int ns2_vc_block(struct gprs_ns2_vc *nsvc) +{ + struct gprs_ns2_vc_priv *priv = nsvc->fi->priv; + if (priv->nsvc->om_blocked) + return -EALREADY; + + return osmo_fsm_inst_dispatch(nsvc->fi, GPRS_NS2_EV_REQ_OM_BLOCK, NULL); +} + +/*! Unblock a NS-VC. + * \param nsvc the virtual circuit + * \return 0 on success; negative on error */ +int ns2_vc_unblock(struct gprs_ns2_vc *nsvc) +{ + struct gprs_ns2_vc_priv *priv = nsvc->fi->priv; + if (!priv->nsvc->om_blocked) + return -EALREADY; + + return osmo_fsm_inst_dispatch(nsvc->fi, GPRS_NS2_EV_REQ_OM_UNBLOCK, NULL); +} + +/*! Reset a NS-VC. + * \param nsvc the virtual circuit + * \return 0 on success; negative on error */ +int ns2_vc_reset(struct gprs_ns2_vc *nsvc) +{ + return osmo_fsm_inst_dispatch(nsvc->fi, GPRS_NS2_EV_REQ_OM_RESET, NULL); +} + +/*! entry point for messages from the driver/VL + * \param nsvc virtual circuit on which the message was received + * \param msg message that was received + * \param tp parsed TLVs of the received message + * \return 0 on success; negative on error */ +int ns2_vc_rx(struct gprs_ns2_vc *nsvc, struct msgb *msg, struct tlv_parsed *tp) +{ + struct gprs_ns_hdr *nsh = (struct gprs_ns_hdr *) msg->l2h; + struct gprs_ns2_vc *target_nsvc = nsvc; + struct osmo_fsm_inst *fi = nsvc->fi; + int rc = 0; + uint8_t cause; + uint16_t nsei, nsvci; + + /* TODO: 7.2: on UNBLOCK/BLOCK: check if NS-VCI is correct, + * if not answer STATUS with "NS-VC unknown" */ + /* TODO: handle BLOCK/UNBLOCK/ALIVE with different VCI */ + + if (ns2_validate(nsvc, nsh->pdu_type, msg, tp, &cause)) { + /* don't answer on a STATUS with a STATUS */ + if (nsh->pdu_type != NS_PDUT_STATUS) { + rc = ns2_tx_status(nsvc, cause, 0, msg, NULL); + goto out; + } + } + + if (TLVP_PRESENT(tp, NS_IE_NSEI)) { + nsei = tlvp_val16be(tp, NS_IE_NSEI); + if (nsei != nsvc->nse->nsei) { + /* 48.016 § 7.3.1 send, RESET_ACK to wrong NSVCI + ignore */ + if (nsh->pdu_type == NS_PDUT_RESET) + ns2_tx_reset_ack(nsvc); + + LOG_NS_SIGNAL(nsvc, "Rx", nsh->pdu_type, LOGL_ERROR, " with wrong NSEI (exp: %05u, got %05u). Ignoring PDU.\n", nsvc->nse->nsei, nsei); + goto out; + } + } + + if (nsvc->nsvci_is_valid && TLVP_PRESENT(tp, NS_IE_VCI)) { + nsvci = tlvp_val16be(tp, NS_IE_VCI); + if (nsvci != nsvc->nsvci) { + /* 48.016 § 7.3.1 send RESET_ACK to wrong NSVCI + ignore */ + if (nsh->pdu_type == NS_PDUT_RESET) { + ns2_tx_reset_ack(nsvc); + LOG_NS_SIGNAL(nsvc, "Rx", nsh->pdu_type, LOGL_ERROR, " with wrong NSVCI (exp: %05u, got %05u). Ignoring PDU.\n", nsvc->nsvci, nsvci); + goto out; + } else if (nsh->pdu_type == NS_PDUT_BLOCK || nsh->pdu_type == NS_PDUT_STATUS) { + /* this is a PDU received over a NSVC and reports a status/block for another NSVC */ + target_nsvc = gprs_ns2_nsvc_by_nsvci(nsvc->nse->nsi, nsvci); + if (!target_nsvc) { + LOGPFSML(fi, LOGL_ERROR, "Received a %s PDU for unknown NSVC (NSVCI %d)\n", + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type), nsvci); + if (nsh->pdu_type == NS_PDUT_BLOCK) + ns2_tx_status(nsvc, NS_CAUSE_NSVC_UNKNOWN, 0, msg, &nsvci); + goto out; + } + + if (target_nsvc->nse != nsvc->nse) { + LOGPFSML(fi, LOGL_ERROR, "Received a %s PDU for a NSVC (NSVCI %d) but it belongs to a different NSE!\n", + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type), nsvci); + goto out; + } + + /* the status/block will be passed to the nsvc/target nsvc in the switch */ + } else { + LOG_NS_SIGNAL(nsvc, "Rx", nsh->pdu_type, LOGL_ERROR, " with wrong NSVCI=%05u. Ignoring PDU.\n", nsvci); + goto out; + } + } + } + + switch (nsh->pdu_type) { + case NS_PDUT_RESET: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_RESET, tp); + break; + case NS_PDUT_RESET_ACK: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_RESET_ACK, tp); + break; + case NS_PDUT_BLOCK: + if (target_nsvc != nsvc) { + osmo_fsm_inst_dispatch(target_nsvc->fi, GPRS_NS2_EV_RX_BLOCK_FOREIGN, tp); + ns2_tx_block_ack(nsvc, &nsvci); + } else { + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_BLOCK, tp); + } + break; + case NS_PDUT_BLOCK_ACK: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_BLOCK_ACK, tp); + break; + case NS_PDUT_UNBLOCK: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_UNBLOCK, tp); + break; + case NS_PDUT_UNBLOCK_ACK: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_UNBLOCK_ACK, tp); + break; + case NS_PDUT_ALIVE: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_ALIVE, tp); + break; + case NS_PDUT_ALIVE_ACK: + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_ALIVE_ACK, tp); + break; + case NS_PDUT_UNITDATA: + /* UNITDATA have to free msg because it might send the msg layer upwards */ + osmo_fsm_inst_dispatch(fi, GPRS_NS2_EV_RX_UNITDATA, msg); + return 0; + case NS_PDUT_STATUS: + osmo_fsm_inst_dispatch(target_nsvc->fi, GPRS_NS2_EV_RX_STATUS, tp); + break; + default: + LOGPFSML(fi, LOGL_ERROR, "NSEI=%u Rx unknown NS PDU type %s\n", nsvc->nse->nsei, + get_value_string(gprs_ns_pdu_strings, nsh->pdu_type)); + rc = -EINVAL; + break; + } + +out: + msgb_free(msg); + + return rc; +} + +/*! is the given NS-VC unblocked? */ +int ns2_vc_is_unblocked(struct gprs_ns2_vc *nsvc) +{ + return (nsvc->fi->state == GPRS_NS2_ST_UNBLOCKED); +} + +/* initialize osmo_ctx on main tread */ +static __attribute__((constructor)) void on_dso_load_ctx(void) +{ + OSMO_ASSERT(osmo_fsm_register(&ns2_vc_fsm) == 0); +} diff --git a/src/gb/gprs_ns2_vty.c b/src/gb/gprs_ns2_vty.c new file mode 100644 index 00000000..32de49d4 --- /dev/null +++ b/src/gb/gprs_ns2_vty.c @@ -0,0 +1,2351 @@ +/*! \file gprs_ns2_vty.c + * VTY interface for our GPRS Networks Service (NS) implementation. */ + +/* (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Alexander Couzens <lynxis@fe80.eu> + * (C) 2021 by Harald Welte <laforge@osmocom.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <stdlib.h> +#include <unistd.h> +#include <errno.h> +#include <stdint.h> + +#include <arpa/inet.h> +#include <net/if.h> + +#include <osmocom/core/byteswap.h> +#include <osmocom/core/fsm.h> +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/osmo_io.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/select.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/core/socket.h> +#include <osmocom/gprs/frame_relay.h> +#include <osmocom/gprs/gprs_ns2.h> +#include <osmocom/gsm/tlv.h> +#include <osmocom/vty/command.h> +#include <osmocom/vty/logging.h> +#include <osmocom/vty/misc.h> +#include <osmocom/vty/telnet_interface.h> +#include <osmocom/vty/vty.h> + +#include "gprs_ns2_internal.h" + +#define SHOW_NS_STR "Display information about the NS protocol\n" +#define NSVCI_STR "NS Virtual Connection ID (NS-VCI)\n" +#define DLCI_STR "Data Link connection identifier\n" + +static struct gprs_ns2_inst *vty_nsi = NULL; +static struct osmo_fr_network *vty_fr_network = NULL; +static struct llist_head binds; +static struct llist_head nses; +static struct llist_head ip_sns_default_binds; + +struct vty_bind { + struct llist_head list; + const char *name; + enum gprs_ns2_ll ll; + int dscp; + uint8_t priority; + bool accept_ipaccess; + bool accept_sns; + uint8_t ip_sns_sig_weight; + uint8_t ip_sns_data_weight; +}; + +struct vty_nse { + struct llist_head list; + uint16_t nsei; + /* list of binds which are valid for this nse. Only IP-SNS uses this + * to allow `no listen ..` in the bind context. So "half" created binds are valid for + * IP-SNS. This allows changing the bind ip without modifying all NSEs afterwards */ + struct llist_head binds; +}; + +/* used by IP-SNS to connect multiple vty_nse_bind to a vty_nse */ +struct vty_nse_bind { + struct llist_head list; + struct vty_bind *vbind; +}; + +/* TODO: this should into osmo timer */ +const struct value_string gprs_ns_timer_strs[] = { + { NS_TOUT_TNS_BLOCK, TNS_BLOCK_STR }, + { NS_TOUT_TNS_BLOCK_RETRIES, TNS_BLOCK_RETRIES_STR }, + { NS_TOUT_TNS_RESET, TNS_RESET_STR }, + { NS_TOUT_TNS_RESET_RETRIES, TNS_RESET_RETRIES_STR }, + { NS_TOUT_TNS_TEST, TNS_TEST_STR }, + { NS_TOUT_TNS_ALIVE, TNS_ALIVE_STR }, + { NS_TOUT_TNS_ALIVE_RETRIES, TNS_ALIVE_RETRIES_STR }, + { NS_TOUT_TSNS_PROV, TSNS_PROV_STR }, + { NS_TOUT_TSNS_SIZE_RETRIES, TSNS_SIZE_RETRIES_STR }, + { NS_TOUT_TSNS_CONFIG_RETRIES, TSNS_CONFIG_RETRIES_STR }, + { NS_TOUT_TSNS_PROCEDURES_RETRIES, TSNS_PROCEDURES_RETRIES_STR }, + { 0, NULL } +}; + +const struct value_string vty_fr_role_names[] = { + { FR_ROLE_USER_EQUIPMENT, "fr" }, + { FR_ROLE_NETWORK_EQUIPMENT, "frnet" }, + { 0, NULL } +}; + +const struct value_string vty_ll_names[] = { + { GPRS_NS2_LL_FR, "fr" }, + { GPRS_NS2_LL_FR_GRE, "frgre" }, + { GPRS_NS2_LL_UDP, "udp" }, + { 0, NULL } +}; + +static struct vty_bind *vty_bind_by_name(const char *name) +{ + struct vty_bind *vbind; + llist_for_each_entry(vbind, &binds, list) { + if (!strcmp(vbind->name, name)) + return vbind; + } + return NULL; +} + +static struct vty_bind *vty_bind_alloc(const char *name) +{ + struct vty_bind *vbind = talloc_zero(vty_nsi, struct vty_bind); + if (!vbind) + return NULL; + + vbind->name = talloc_strdup(vty_nsi, name); + if (!vbind->name) { + talloc_free(vbind); + return NULL; + } + + vbind->ip_sns_sig_weight = 1; + vbind->ip_sns_data_weight = 1; + llist_add_tail(&vbind->list, &binds); + return vbind; +} + +static void vty_bind_free(struct vty_bind *vbind) +{ + if (!vbind) + return; + + llist_del(&vbind->list); + talloc_free(vbind); +} + +static struct vty_nse *vty_nse_by_nsei(uint16_t nsei) +{ + struct vty_nse *vnse; + llist_for_each_entry(vnse, &nses, list) { + if (vnse->nsei == nsei) + return vnse; + } + return NULL; +} + +static struct vty_nse *vty_nse_alloc(uint16_t nsei) +{ + struct vty_nse *vnse = talloc_zero(vty_nsi, struct vty_nse); + if (!vnse) + return NULL; + + vnse->nsei = nsei; + INIT_LLIST_HEAD(&vnse->binds); + llist_add_tail(&vnse->list, &nses); + return vnse; +} + +static void vty_nse_free(struct vty_nse *vnse) +{ + if (!vnse) + return; + + llist_del(&vnse->list); + /* all vbind of the nse will be freed by talloc */ + talloc_free(vnse); +} + +static int vty_nse_add_vbind(struct vty_nse *vnse, struct vty_bind *vbind) +{ + struct vty_nse_bind *vnse_bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) + return -EINVAL; + + llist_for_each_entry(vnse_bind, &vnse->binds, list) { + if (vnse_bind->vbind == vbind) + return -EALREADY; + } + + vnse_bind = talloc(vnse, struct vty_nse_bind); + if (!vnse_bind) + return -ENOMEM; + vnse_bind->vbind = vbind; + + llist_add_tail(&vnse_bind->list, &vnse->binds); + return 0; +} + +static int vty_nse_remove_vbind(struct vty_nse *vnse, struct vty_bind *vbind) +{ + struct vty_nse_bind *vnse_bind, *tmp; + if (vbind->ll != GPRS_NS2_LL_UDP) + return -EINVAL; + + llist_for_each_entry_safe(vnse_bind, tmp, &vnse->binds, list) { + if (vnse_bind->vbind == vbind) { + llist_del(&vnse_bind->list); + talloc_free(vnse_bind); + return 0; + } + } + + return -ENOENT; +} + +/* check if the NSE still has SNS configuration */ +static bool vty_nse_check_sns(struct gprs_ns2_nse *nse) { + struct vty_nse *vnse = vty_nse_by_nsei(nse->nsei); + + int count = gprs_ns2_sns_count(nse); + if (count > 0) { + /* there are other sns endpoints */ + return true; + } + + if (!vnse) + return false; + + if (llist_empty(&vnse->binds)) + return false; + + return true; +} + +static struct cmd_node ns_node = { + L_NS_NODE, + "%s(config-ns)# ", + 1, +}; + +DEFUN(cfg_ns, cfg_ns_cmd, + "ns", + "Configure the GPRS Network Service") +{ + vty->node = L_NS_NODE; + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_timer, cfg_ns_timer_cmd, + "timer " NS_TIMERS " <0-65535>", + "Network Service Timer\n" + NS_TIMERS_HELP "Timer Value\n") +{ + int idx = get_string_value(gprs_ns_timer_strs, argv[0]); + int val = atoi(argv[1]); + + if (idx < 0 || idx >= ARRAY_SIZE(vty_nsi->timeout)) + return CMD_WARNING; + + vty_nsi->timeout[idx] = val; + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_nsei, cfg_ns_nsei_cmd, + "nse <0-65535> [ip-sns-role-sgsn]", + "Persistent NS Entity\n" + "NS Entity ID (NSEI)\n" + "Create NSE in SGSN role (default: BSS)\n" + ) +{ + struct gprs_ns2_nse *nse; + struct vty_nse *vnse; + uint16_t nsei = atoi(argv[0]); + bool sgsn_role = false; + bool free_vnse = false; + if (argc > 1 && !strcmp(argv[1], "ip-sns-role-sgsn")) + sgsn_role = true; + + vnse = vty_nse_by_nsei(nsei); + if (!vnse) { + vnse = vty_nse_alloc(nsei); + if (!vnse) { + vty_out(vty, "Failed to create vty NSE!%s", VTY_NEWLINE); + return CMD_ERR_INCOMPLETE; + } + free_vnse = true; + } + + nse = gprs_ns2_nse_by_nsei(vty_nsi, nsei); + if (!nse) { + nse = gprs_ns2_create_nse2(vty_nsi, nsei, GPRS_NS2_LL_UNDEF, GPRS_NS2_DIALECT_UNDEF, + sgsn_role); + if (!nse) { + vty_out(vty, "Failed to create NSE!%s", VTY_NEWLINE); + goto err; + } + nse->persistent = true; + } + + if (!nse->persistent) { + /* TODO: should the dynamic NSE removed? */ + vty_out(vty, "A dynamic NSE with the specified NSEI already exists%s", VTY_NEWLINE); + goto err; + } + + vty->node = L_NS_NSE_NODE; + vty->index = nse; + + return CMD_SUCCESS; + +err: + if (free_vnse) + talloc_free(vnse); + + return CMD_ERR_INCOMPLETE; +} + +DEFUN(cfg_no_ns_nsei, cfg_no_ns_nsei_cmd, + "no nse <0-65535>", + NO_STR + "Delete a Persistent NS Entity\n" + "NS Entity ID (NSEI)\n" + ) +{ + struct gprs_ns2_nse *nse; + struct vty_nse *vnse; + uint16_t nsei = atoi(argv[0]); + + nse = gprs_ns2_nse_by_nsei(vty_nsi, nsei); + if (!nse) { + vty_out(vty, "Can not find NS Entity %s%s", argv[0], VTY_NEWLINE); + return CMD_ERR_NOTHING_TODO; + } + + if (!nse->persistent) { + vty_out(vty, "Ignoring non-persistent NS Entity%s", VTY_NEWLINE); + return CMD_WARNING; + } + + vty_out(vty, "Deleting NS Entity %u%s", nse->nsei, VTY_NEWLINE); + gprs_ns2_free_nse(nse); + + vnse = vty_nse_by_nsei(nsei); + vty_nse_free(vnse); + + return CMD_SUCCESS; +} + +/* TODO: add fr/gre */ +DEFUN(cfg_ns_bind, cfg_ns_bind_cmd, + "bind (fr|udp) ID", + "Configure local Bind\n" + "Frame Relay\n" "UDP/IP\n" + "Unique identifier for this bind (to reference from NS-VCs, NSEs, ...)\n" + ) +{ + const char *nstype = argv[0]; + const char *name = argv[1]; + struct vty_bind *vbind; + enum gprs_ns2_ll ll; + int rc; + + rc = get_string_value(vty_ll_names, nstype); + if (rc < 0) + return CMD_WARNING; + ll = (enum gprs_ns2_ll) rc; + + if (!osmo_identifier_valid(name)) { + vty_out(vty, "Invalid ID. The ID should be only alphanumeric.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + vbind = vty_bind_by_name(name); + if (vbind) { + if (vbind->ll != ll) { + vty_out(vty, "A bind with the specified ID already exists with a different type (fr|frgre|udp)!%s", + VTY_NEWLINE); + return CMD_WARNING; + } + } else { + vbind = vty_bind_alloc(name); + if (!vbind) { + vty_out(vty, "Can not create bind - out of memory%s", VTY_NEWLINE); + return CMD_WARNING; + } + vbind->ll = ll; + } + + vty->index = vbind; + vty->node = L_NS_BIND_NODE; + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind, cfg_no_ns_bind_cmd, + "no bind ID", + NO_STR + "Delete a bind\n" + "Unique identifier for this bind\n" + ) +{ + struct vty_bind *vbind; + struct gprs_ns2_vc_bind *bind; + const char *name = argv[0]; + + vbind = vty_bind_by_name(name); + if (!vbind) { + vty_out(vty, "bind %s does not exist!%s", name, VTY_NEWLINE); + return CMD_WARNING; + } + vty_bind_free(vbind); + bind = gprs_ns2_bind_by_name(vty_nsi, name); + if (bind) + gprs_ns2_free_bind(bind); + return CMD_SUCCESS; +} + + +static void config_write_vbind(struct vty *vty, struct vty_bind *vbind) +{ + struct gprs_ns2_vc_bind *bind; + const struct osmo_sockaddr *addr; + struct osmo_sockaddr_str addr_str; + const char *netif, *frrole_str, *llstr; + enum osmo_fr_role frrole; + + llstr = get_value_string_or_null(vty_ll_names, vbind->ll); + if (!llstr) + return; + vty_out(vty, " bind %s %s%s", llstr, vbind->name, VTY_NEWLINE); + + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + switch (vbind->ll) { + case GPRS_NS2_LL_FR: + if (bind) { + netif = gprs_ns2_fr_bind_netif(bind); + if (!netif) + return; + frrole = gprs_ns2_fr_bind_role(bind); + if ((int) frrole == -1) + return; + frrole_str = get_value_string_or_null(vty_fr_role_names, frrole); + if (netif && frrole_str) + vty_out(vty, " fr %s %s%s", netif, frrole_str, VTY_NEWLINE); + } + break; + case GPRS_NS2_LL_UDP: + if (bind) { + addr = gprs_ns2_ip_bind_sockaddr(bind); + if (!osmo_sockaddr_str_from_sockaddr(&addr_str, &addr->u.sas)) { + vty_out(vty, " listen %s %u%s", addr_str.ip, addr_str.port, + VTY_NEWLINE); + } + } + if (vbind->accept_ipaccess) + vty_out(vty, " accept-ipaccess%s", VTY_NEWLINE); + if (vbind->accept_sns) + vty_out(vty, " accept-dynamic-ip-sns%s", VTY_NEWLINE); + if (vbind->dscp) + vty_out(vty, " dscp %u%s", vbind->dscp, VTY_NEWLINE); + if (vbind->priority) + vty_out(vty, " socket-priority %u%s", vbind->priority, VTY_NEWLINE); + vty_out(vty, " ip-sns signalling-weight %u data-weight %u%s", + vbind->ip_sns_sig_weight, vbind->ip_sns_data_weight, VTY_NEWLINE); + break; + default: + return; + } +} + +static void config_write_nsvc(struct vty *vty, const struct gprs_ns2_vc *nsvc) +{ + const char *netif; + uint16_t dlci; + const struct osmo_sockaddr *addr; + struct osmo_sockaddr_str addr_str; + + switch (nsvc->nse->ll) { + case GPRS_NS2_LL_UNDEF: + break; + case GPRS_NS2_LL_UDP: + switch (nsvc->nse->dialect) { + case GPRS_NS2_DIALECT_IPACCESS: + addr = gprs_ns2_ip_vc_remote(nsvc); + if (!addr) + break; + if (osmo_sockaddr_str_from_sockaddr(&addr_str, &addr->u.sas)) + break; + vty_out(vty, " nsvc ipa %s %s %u nsvci %u%s", + nsvc->bind->name, addr_str.ip, addr_str.port, + nsvc->nsvci, VTY_NEWLINE); + break; + case GPRS_NS2_DIALECT_STATIC_ALIVE: + addr = gprs_ns2_ip_vc_remote(nsvc); + if (!addr) + break; + if (osmo_sockaddr_str_from_sockaddr(&addr_str, &addr->u.sas)) + break; + vty_out(vty, " nsvc udp %s %s %u%s", + nsvc->bind->name, addr_str.ip, addr_str.port, VTY_NEWLINE); + break; + default: + break; + } + break; + case GPRS_NS2_LL_FR: + netif = gprs_ns2_fr_bind_netif(nsvc->bind); + if (!netif) + break; + dlci = gprs_ns2_fr_nsvc_dlci(nsvc); + if (!dlci) + break; + OSMO_ASSERT(nsvc->nsvci_is_valid); + vty_out(vty, " nsvc fr %s dlci %u nsvci %u%s", + netif, dlci, nsvc->nsvci, VTY_NEWLINE); + break; + case GPRS_NS2_LL_FR_GRE: + break; + } +} + +static void _config_write_ns_nse(struct vty *vty, struct gprs_ns2_nse *nse) +{ + struct gprs_ns2_vc *nsvc; + struct vty_nse *vnse = vty_nse_by_nsei(nse->nsei); + struct vty_nse_bind *vbind; + + OSMO_ASSERT(vnse); + + vty_out(vty, " nse %u%s%s", nse->nsei, + nse->ip_sns_role_sgsn ? " ip-sns-role-sgsn" : "", VTY_NEWLINE); + switch (nse->dialect) { + case GPRS_NS2_DIALECT_SNS: + ns2_sns_write_vty(vty, nse); + llist_for_each_entry(vbind, &vnse->binds, list) { + vty_out(vty, " ip-sns-bind %s%s", vbind->vbind->name, VTY_NEWLINE); + } + break; + default: + llist_for_each_entry(nsvc, &nse->nsvc, list) { + config_write_nsvc(vty, nsvc); + } + break; + } +} + +static int config_write_ns_nse(struct vty *vty) +{ + struct gprs_ns2_nse *nse; + + llist_for_each_entry(nse, &vty_nsi->nse, list) { + if (!nse->persistent) + continue; + + _config_write_ns_nse(vty, nse); + } + + return 0; +} + +static int config_write_ns_bind(struct vty *vty) +{ + struct vty_bind *vbind; + + llist_for_each_entry(vbind, &binds, list) { + config_write_vbind(vty, vbind); + } + + return 0; +} + +static int config_write_ns(struct vty *vty) +{ + struct vty_nse_bind *vbind; + unsigned int i; + int ret; + + vty_out(vty, "ns%s", VTY_NEWLINE); + + for (i = 0; i < ARRAY_SIZE(vty_nsi->timeout); i++) + vty_out(vty, " timer %s %u%s", + get_value_string(gprs_ns_timer_strs, i), + vty_nsi->timeout[i], VTY_NEWLINE); + + if (vty_nsi->txqueue_max_length != NS_DEFAULT_TXQUEUE_MAX_LENGTH) + vty_out(vty, " txqueue-max-length %u%s", vty_nsi->txqueue_max_length, VTY_NEWLINE); + + ret = config_write_ns_bind(vty); + if (ret) + return ret; + + llist_for_each_entry(vbind, &ip_sns_default_binds, list) { + vty_out(vty, " ip-sns-default bind %s%s", vbind->vbind->name, VTY_NEWLINE); + } + + ret = config_write_ns_nse(vty); + if (ret) + return ret; + + return 0; +} + + +static struct cmd_node ns_bind_node = { + L_NS_BIND_NODE, + "%s(config-ns-bind)# ", + 1, +}; + +DEFUN(cfg_ns_bind_listen, cfg_ns_bind_listen_cmd, + "listen " VTY_IPV46_CMD " <1-65535>", + "Configure local IP + Port of this bind\n" + "Local IPv4 Address\n" "Local IPv6 Address\n" + "Local UDP Port\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + int rc; + const char *addr_str = argv[0]; + unsigned int port = atoi(argv[1]); + struct osmo_sockaddr_str sockaddr_str; + struct osmo_sockaddr sockaddr; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "listen can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_from_str(&sockaddr_str, addr_str, port)) { + vty_out(vty, "Can not parse the Address %s %s%s", argv[0], argv[1], VTY_NEWLINE); + return CMD_WARNING; + } + osmo_sockaddr_str_to_sockaddr(&sockaddr_str, &sockaddr.u.sas); + if (gprs_ns2_ip_bind_by_sockaddr(vty_nsi, &sockaddr)) { + vty_out(vty, "A bind with the specified address already exists!%s", VTY_NEWLINE); + return CMD_WARNING; + } + + rc = gprs_ns2_ip_bind(vty_nsi, vbind->name, &sockaddr, vbind->dscp, &bind); + if (rc != 0) { + vty_out(vty, "Failed to create the bind (rc %d)!%s", rc, VTY_NEWLINE); + return CMD_WARNING; + } + + bind->accept_ipaccess = vbind->accept_ipaccess; + bind->accept_sns = vbind->accept_sns; + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind_listen, cfg_no_ns_bind_listen_cmd, + "no listen", + NO_STR + "Delete a IP/Port assignment\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "no listen can be only used with UDP bind%s", VTY_NEWLINE); + return CMD_WARNING; + } + + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (!bind) + return CMD_ERR_NOTHING_TODO; + + OSMO_ASSERT(bind->ll == GPRS_NS2_LL_UDP); + gprs_ns2_free_bind(bind); + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_dscp, cfg_ns_bind_dscp_cmd, + "dscp <0-63>", + "Set DSCP/TOS on the UDP socket\n" "DSCP Value\n") +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + uint16_t dscp = atoi(argv[0]); + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "dscp can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->dscp = dscp; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + gprs_ns2_ip_bind_set_dscp(bind, dscp); + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind_dscp, cfg_no_ns_bind_dscp_cmd, + "no dscp", + "Set DSCP/TOS on the UDP socket\n" "DSCP Value\n") +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + uint16_t dscp = 0; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "dscp can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->dscp = dscp; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + gprs_ns2_ip_bind_set_dscp(bind, dscp); + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_priority, cfg_ns_bind_priority_cmd, + "socket-priority <0-255>", + "Set socket priority on the UDP socket\n" "Priority Value (>6 requires CAP_NET_ADMIN)\n") +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + uint8_t prio = atoi(argv[0]); + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "dscp can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->priority = prio; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + gprs_ns2_ip_bind_set_priority(bind, prio); + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_ipaccess, cfg_ns_bind_ipaccess_cmd, + "accept-ipaccess", + "Allow to create dynamic NS Entity by NS Reset PDU on UDP (ip.access style)\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "accept-ipaccess can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->accept_ipaccess = true; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + bind->accept_ipaccess = true; + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind_ipaccess, cfg_no_ns_bind_ipaccess_cmd, + "no accept-ipaccess", + NO_STR + "Reject NS Reset PDU on UDP (ip.access style)\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "no accept-ipaccess can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->accept_ipaccess = false; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + bind->accept_ipaccess = false; + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_accept_sns, cfg_ns_bind_accept_sns_cmd, + "accept-dynamic-ip-sns", + "Allow to create dynamic NS Entities by IP-SNS PDUs\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "accept-dynamic-ip-sns can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->accept_sns = true; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + bind->accept_sns = true; + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind_accept_sns, cfg_no_ns_bind_accept_sns_cmd, + "no accept-dynamic-ip-sns", + NO_STR + "Disable dynamic creation of NS Entities by IP-SNS PDUs\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "no accept-dynamic-ip-sns can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->accept_sns = false; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + bind->accept_sns = false; + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_ip_sns_weight, cfg_ns_bind_ip_sns_weight_cmd, + "ip-sns signalling-weight <0-254> data-weight <0-254>", + "IP SNS\n" + "signalling weight used by IP-SNS dynamic configuration\n" + "signalling weight used by IP-SNS dynamic configuration\n" + "data weight used by IP-SNS dynamic configuration\n" + "data weight used by IP-SNS dynamic configuration\n") +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + + int signalling = atoi(argv[0]); + int data = atoi(argv[1]); + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "ip-sns signalling-weight <0-254> data-weight <0-254> can be only used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + vbind->ip_sns_data_weight = data; + vbind->ip_sns_sig_weight = signalling; + bind = gprs_ns2_bind_by_name(vty_nsi, vbind->name); + if (bind) + gprs_ns2_ip_bind_set_sns_weight(bind, signalling, data); + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_bind_fr, cfg_ns_bind_fr_cmd, + "fr NETIF (fr|frnet)", + "frame relay\n" + IFNAME_STR + "fr (user) is used by BSS or SGSN attached to UNI of a FR network\n" + "frnet (network) is used by SGSN if BSS is directly attached\n" + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + const char *netif = argv[0]; + const char *role = argv[1]; + + int rc = 0; + enum osmo_fr_role frrole; + + if (vbind->ll != GPRS_NS2_LL_FR) { + vty_out(vty, "fr can be only used with frame relay bind%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (!strcmp(role, "fr")) + frrole = FR_ROLE_USER_EQUIPMENT; + else if (!strcmp(role, "frnet")) + frrole = FR_ROLE_NETWORK_EQUIPMENT; + else + return CMD_WARNING; + + bind = gprs_ns2_fr_bind_by_netif(vty_nsi, netif); + if (bind) { + vty_out(vty, "Interface %s already used.%s", netif, VTY_NEWLINE); + return CMD_WARNING; + } + + rc = gprs_ns2_fr_bind(vty_nsi, vbind->name, netif, vty_fr_network, frrole, &bind); + if (rc < 0) { + LOGP(DLNS, LOGL_ERROR, "Failed to bind interface %s on fr. Err: %d\n", netif, rc); + return CMD_WARNING; + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_bind_fr, cfg_no_ns_bind_fr_cmd, + "no fr NETIF", + NO_STR + "Delete a frame relay link\n" + "Delete a frame relay link\n" + IFNAME_STR + ) +{ + struct vty_bind *vbind = vty->index; + struct gprs_ns2_vc_bind *bind; + const char *netif = argv[0]; + + if (vbind->ll != GPRS_NS2_LL_FR) { + vty_out(vty, "fr can be only used with frame relay bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + bind = gprs_ns2_fr_bind_by_netif(vty_nsi, netif); + if (!bind) { + vty_out(vty, "Interface not found.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (strcmp(bind->name, vbind->name)) { + vty_out(vty, "The specified interface is not bound to this bind.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_bind(bind); + return CMD_SUCCESS; +} + + +static struct cmd_node ns_nse_node = { + L_NS_NSE_NODE, + "%s(config-ns-nse)# ", + 1, +}; + +DEFUN(cfg_ns_nse_nsvc_fr, cfg_ns_nse_nsvc_fr_cmd, + "nsvc fr NETIF dlci <16-1007> nsvci <0-65535>", + "NS Virtual Connection\n" + "frame relay\n" + "frame relay interface. Must be registered via fr vty\n" + NSVCI_STR + NSVCI_STR + DLCI_STR + DLCI_STR + ) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + const char *netif = argv[0]; + uint16_t dlci = atoi(argv[1]); + uint16_t nsvci = atoi(argv[2]); + bool dialect_modified = false; + bool ll_modified = false; + + if (nse->ll != GPRS_NS2_LL_FR && nse->ll != GPRS_NS2_LL_UNDEF) { + vty_out(vty, "Can not mix NS-VC with different link layer%s", VTY_NEWLINE); + goto err; + } + + if (nse->dialect != GPRS_NS2_DIALECT_STATIC_RESETBLOCK && nse->dialect != GPRS_NS2_DIALECT_UNDEF) { + vty_out(vty, "Can not mix NS-VC with different dialects%s", VTY_NEWLINE); + goto err; + } + + if (nse->ll == GPRS_NS2_LL_UNDEF) { + nse->ll = GPRS_NS2_LL_FR; + ll_modified = true; + } + + if (nse->dialect == GPRS_NS2_DIALECT_UNDEF) { + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_STATIC_RESETBLOCK); + dialect_modified = true; + } + + + bind = gprs_ns2_fr_bind_by_netif(vty_nsi, netif); + if (!bind) { + vty_out(vty, "Can not find fr interface \"%s\". Please configure it via fr vty.%s", + netif, VTY_NEWLINE); + goto err; + } + + if (gprs_ns2_fr_nsvc_by_dlci(bind, dlci)) { + vty_out(vty, "A NS-VC with the specified DLCI already exist!%s", VTY_NEWLINE); + goto err; + } + + if (gprs_ns2_nsvc_by_nsvci(vty_nsi, nsvci)) { + vty_out(vty, "A NS-VC with the specified NS-VCI already exist!%s", VTY_NEWLINE); + goto err; + } + + nsvc = gprs_ns2_fr_connect(bind, nse, nsvci, dlci); + if (!nsvc) { + /* Could not create NS-VC, connect failed */ + vty_out(vty, "Failed to create the NS-VC%s", VTY_NEWLINE); + goto err; + } + nsvc->persistent = true; + return CMD_SUCCESS; + +err: + if (ll_modified) + nse->ll = GPRS_NS2_LL_UNDEF; + if (dialect_modified) + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + + return CMD_WARNING; +} + +DEFUN(cfg_no_ns_nse_nsvc_fr_dlci, cfg_no_ns_nse_nsvc_fr_dlci_cmd, + "no nsvc fr NETIF dlci <16-1007>", + NO_STR + "Delete frame relay NS-VC\n" + "frame relay\n" + "frame relay interface. Must be registered via fr vty\n" + DLCI_STR + DLCI_STR + ) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + const char *netif = argv[0]; + uint16_t dlci = atoi(argv[1]); + + if (nse->ll != GPRS_NS2_LL_FR) { + vty_out(vty, "This NSE doesn't support frame relay.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + bind = gprs_ns2_fr_bind_by_netif(vty_nsi, netif); + if (!bind) { + vty_out(vty, "Can not find fr interface \"%s\"%s", + netif, VTY_NEWLINE); + return CMD_ERR_NOTHING_TODO; + } + + nsvc = gprs_ns2_fr_nsvc_by_dlci(bind, dlci); + if (!nsvc) { + vty_out(vty, "Can not find a NS-VC on fr interface %s with dlci %u%s", + netif, dlci, VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse != nsvc->nse) { + vty_out(vty, "The specified NS-VC is not a part of the NSE %u!%s" + "To remove this NS-VC go to the vty node 'nse %u'%s", + nse->nsei, VTY_NEWLINE, + nsvc->nse->nsei, VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_nsvc(nsvc); + if (llist_empty(&nse->nsvc)) { + nse->ll = GPRS_NS2_LL_UNDEF; + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_nse_nsvci, cfg_no_ns_nse_nsvci_cmd, + "no nsvc nsvci <0-65535>", + NO_STR + "Delete NSVC\n" + NSVCI_STR + NSVCI_STR + ) +{ + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + uint16_t nsvci = atoi(argv[0]); + + switch (nse->dialect) { + case GPRS_NS2_DIALECT_SNS: + case GPRS_NS2_DIALECT_STATIC_ALIVE: + vty_out(vty, "NSE doesn't support NSVCI.%s", VTY_NEWLINE); + return CMD_WARNING; + case GPRS_NS2_DIALECT_UNDEF: + vty_out(vty, "No NSVCs configured%s", VTY_NEWLINE); + return CMD_WARNING; + case GPRS_NS2_DIALECT_IPACCESS: + case GPRS_NS2_DIALECT_STATIC_RESETBLOCK: + break; + } + + nsvc = gprs_ns2_nsvc_by_nsvci(vty_nsi, nsvci); + if (!nsvc) { + vty_out(vty, "Can not find NS-VC with NS-VCI %u%s", nsvci, VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse != nsvc->nse) { + vty_out(vty, "NS-VC with NS-VCI %u is not part of this NSE!%s", + nsvci, VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_nsvc(nsvc); + if (llist_empty(&nse->nsvc)) { + nse->ll = GPRS_NS2_LL_UNDEF; + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + } + + return CMD_SUCCESS; +} + +static int ns_nse_nsvc_udp_cmds(struct vty *vty, const char *bind_name, const char *remote_char, uint16_t port, + uint16_t sig_weight, uint16_t data_weight) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + bool dialect_modified = false; + bool ll_modified = false; + + struct osmo_sockaddr_str remote_str; + struct osmo_sockaddr remote; + + if (nse->ll == GPRS_NS2_LL_UNDEF) { + nse->ll = GPRS_NS2_LL_UDP; + ll_modified = true; + } + + if (nse->dialect == GPRS_NS2_DIALECT_UNDEF) { + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_STATIC_ALIVE); + dialect_modified = true; + } + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Can not mix NS-VC with different link layer%s", VTY_NEWLINE); + goto err; + } + + if (nse->dialect != GPRS_NS2_DIALECT_STATIC_ALIVE) { + vty_out(vty, "Can not mix NS-VC with different dialects%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_from_str(&remote_str, remote_char, port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + bind = gprs_ns2_bind_by_name(vty_nsi, bind_name); + if (!bind) { + vty_out(vty, "Can not find bind with name %s%s", + bind_name, VTY_NEWLINE); + goto err; + } + + if (bind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Bind %s is not an UDP bind.%s", + bind_name, VTY_NEWLINE); + goto err; + } + + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, &remote); + if (nsvc) { + if (nsvc->nse == nse) + vty_out(vty, "Specified NSVC is already present in this NSE.%s", VTY_NEWLINE); + else + vty_out(vty, "Specified NSVC is already present in another NSE%05u.%s", nsvc->nse->nsei, VTY_NEWLINE); + goto err; + } + + nsvc = gprs_ns2_ip_connect(bind, &remote, nse, 0); + if (!nsvc) { + vty_out(vty, "Can not create NS-VC.%s", VTY_NEWLINE); + goto err; + } + nsvc->sig_weight = sig_weight; + nsvc->data_weight = data_weight; + nsvc->persistent = true; + + return CMD_SUCCESS; + +err: + if (ll_modified) + nse->ll = GPRS_NS2_LL_UNDEF; + if (dialect_modified) + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + return CMD_WARNING; +} + +DEFUN(cfg_ns_nse_nsvc_udp, cfg_ns_nse_nsvc_udp_cmd, + "nsvc udp BIND " VTY_IPV46_CMD " <1-65535>", + "NS Virtual Connection\n" + "NS over UDP\n" + "A unique bind identifier created by ns bind\n" + "Remote IPv4 Address\n" "Remote IPv6 Address\n" + "Remote UDP Port\n") +{ + const char *bind_name = argv[0]; + const char *remote = argv[1]; + uint16_t port = atoi(argv[2]); + uint16_t sig_weight = 1; + uint16_t data_weight = 1; + + return ns_nse_nsvc_udp_cmds(vty, bind_name, remote, port, sig_weight, data_weight); +} + +DEFUN(cfg_ns_nse_nsvc_udp_weights, cfg_ns_nse_nsvc_udp_weights_cmd, + "nsvc udp BIND " VTY_IPV46_CMD " <1-65535> signalling-weight <0-254> data-weight <0-254>", + "NS Virtual Connection\n" + "NS over UDP\n" + "A unique bind identifier created by ns bind\n" + "Remote IPv4 Address\n" "Remote IPv6 Address\n" + "Remote UDP Port\n" + "Signalling weight of the NSVC (default = 1)\n" + "Signalling weight of the NSVC (default = 1)\n" + "Data weight of the NSVC (default = 1)\n" + "Data weight of the NSVC (default = 1)\n" + ) +{ + const char *bind_name = argv[0]; + const char *remote = argv[1]; + uint16_t port = atoi(argv[2]); + uint16_t sig_weight = atoi(argv[3]); + uint16_t data_weight = atoi(argv[4]); + + return ns_nse_nsvc_udp_cmds(vty, bind_name, remote, port, sig_weight, data_weight); +} + +DEFUN(cfg_no_ns_nse_nsvc_udp, cfg_no_ns_nse_nsvc_udp_cmd, + "no nsvc udp BIND " VTY_IPV46_CMD " <1-65535>", + NO_STR + "Delete a NS Virtual Connection\n" + "NS over UDP\n" + "A unique bind identifier created by ns bind\n" + "Remote IPv4 Address\n" "Remote IPv6 Address\n" + "Remote UDP Port\n" + ) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + const char *bind_name = argv[0]; + struct osmo_sockaddr_str remote_str; + struct osmo_sockaddr remote; + uint16_t port = atoi(argv[2]); + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "This NSE doesn't support UDP.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse->dialect != GPRS_NS2_DIALECT_STATIC_ALIVE) { + vty_out(vty, "This NSE doesn't support UDP with dialect static alive.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + bind = gprs_ns2_bind_by_name(vty_nsi, bind_name); + if (!bind) { + vty_out(vty, "Can not find bind with name %s%s", + bind_name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (bind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Bind %s is not an UDP bind.%s", + bind_name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_from_str(&remote_str, argv[1], port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, &remote); + if (!nsvc) { + vty_out(vty, "Can not find NS-VC with remote %s:%u%s", + remote_str.ip, remote_str.port, VTY_NEWLINE); + return CMD_WARNING; + } + + if (!nsvc->persistent) { + vty_out(vty, "NS-VC with remote %s:%u is a dynamic NS-VC. Not configured by vty.%s", + remote_str.ip, remote_str.port, VTY_NEWLINE); + return CMD_WARNING; + } + + if (nsvc->nse != nse) { + vty_out(vty, "NS-VC is not part of this NSE!%s", VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_nsvc(nsvc); + if (llist_empty(&nse->nsvc)) { + nse->ll = GPRS_NS2_LL_UNDEF; + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_nse_nsvc_ipa, cfg_ns_nse_nsvc_ipa_cmd, + "nsvc ipa BIND " VTY_IPV46_CMD " <1-65535> nsvci <0-65535>" , + "NS Virtual Connection\n" + "NS over UDP ip.access style (uses RESET/BLOCK)\n" + "A unique bind identifier created by ns bind\n" + "Remote IPv4 Address\n" "Remote IPv6 Address\n" + "Remote UDP Port\n" + NSVCI_STR + NSVCI_STR + ) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + bool dialect_modified = false; + bool ll_modified = false; + + const char *bind_name = argv[0]; + struct osmo_sockaddr_str remote_str; + struct osmo_sockaddr remote; + uint16_t port = atoi(argv[2]); + uint16_t nsvci = atoi(argv[3]); + + if (nse->ll == GPRS_NS2_LL_UNDEF) { + nse->ll = GPRS_NS2_LL_UDP; + ll_modified = true; + } + + if (nse->dialect == GPRS_NS2_DIALECT_UNDEF) { + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_IPACCESS); + dialect_modified = true; + } + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Can not mix NS-VC with different link layer%s", VTY_NEWLINE); + goto err; + } + + if (nse->dialect != GPRS_NS2_DIALECT_IPACCESS) { + vty_out(vty, "Can not mix NS-VC with different dialects%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_from_str(&remote_str, argv[1], port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + bind = gprs_ns2_bind_by_name(vty_nsi, bind_name); + if (!bind) { + vty_out(vty, "Can not find bind with name %s%s", + bind_name, VTY_NEWLINE); + goto err; + } + + if (bind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Bind %s is not an UDP bind.%s", + bind_name, VTY_NEWLINE); + goto err; + } + + nsvc = gprs_ns2_ip_connect(bind, &remote, nse, nsvci); + if (!nsvc) { + vty_out(vty, "Can not create NS-VC.%s", VTY_NEWLINE); + goto err; + } + nsvc->persistent = true; + + return CMD_SUCCESS; + +err: + if (ll_modified) + nse->ll = GPRS_NS2_LL_UNDEF; + if (dialect_modified) + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + return CMD_WARNING; +} + +DEFUN(cfg_no_ns_nse_nsvc_ipa, cfg_no_ns_nse_nsvc_ipa_cmd, + "no nsvc ipa BIND " VTY_IPV46_CMD " <1-65535> nsvci <0-65535>", + NO_STR + "Delete a NS Virtual Connection\n" + "NS over UDP\n" + "A unique bind identifier created by ns bind\n" + "Remote IPv4 Address\n" "Remote IPv6 Address\n" + "Remote UDP Port\n" + NSVCI_STR + NSVCI_STR + ) +{ + struct gprs_ns2_vc_bind *bind; + struct gprs_ns2_vc *nsvc; + struct gprs_ns2_nse *nse = vty->index; + const char *bind_name = argv[0]; + struct osmo_sockaddr_str remote_str; + struct osmo_sockaddr remote; + uint16_t port = atoi(argv[2]); + uint16_t nsvci = atoi(argv[3]); + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "This NSE doesn't support UDP.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse->dialect != GPRS_NS2_DIALECT_IPACCESS) { + vty_out(vty, "This NSE doesn't support UDP with dialect ipaccess.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + bind = gprs_ns2_bind_by_name(vty_nsi, bind_name); + if (!bind) { + vty_out(vty, "Can not find bind with name %s%s", + bind_name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (bind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Bind %s is not an UDP bind.%s", + bind_name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_from_str(&remote_str, argv[1], port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + nsvc = gprs_ns2_nsvc_by_sockaddr_bind(bind, &remote); + if (!nsvc) { + vty_out(vty, "Can not find NS-VC with remote %s:%u%s", + remote_str.ip, remote_str.port, VTY_NEWLINE); + return CMD_WARNING; + } + + if (!nsvc->persistent) { + vty_out(vty, "NS-VC with remote %s:%u is a dynamic NS-VC. Not configured by vty.%s", + remote_str.ip, remote_str.port, VTY_NEWLINE); + return CMD_WARNING; + } + + if (nsvc->nse != nse) { + vty_out(vty, "NS-VC is not part of this NSE!%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (!nsvc->nsvci_is_valid) { + vty_out(vty, "NS-VC doesn't have a nsvci!%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (nsvc->nsvci != nsvci) { + vty_out(vty, "NS-VC has a different nsvci (%u)!%s", + nsvc->nsvci, VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_nsvc(nsvc); + if (llist_empty(&nse->nsvc)) { + nse->ll = GPRS_NS2_LL_UNDEF; + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_nse_ip_sns_remote, cfg_ns_nse_ip_sns_remote_cmd, + "ip-sns-remote " VTY_IPV46_CMD " <1-65535>", + "SNS Initial Endpoint\n" + "SGSN IPv4 Address\n" "SGSN IPv6 Address\n" + "SGSN UDP Port\n" + ) +{ + struct gprs_ns2_nse *nse = vty->index; + bool dialect_modified = false; + bool ll_modified = false; + int rc; + + /* argv[0] */ + struct osmo_sockaddr_str remote_str; + struct osmo_sockaddr remote; + uint16_t port = atoi(argv[1]); + + if (nse->ll == GPRS_NS2_LL_UNDEF) { + nse->ll = GPRS_NS2_LL_UDP; + ll_modified = true; + } + + if (nse->dialect == GPRS_NS2_DIALECT_UNDEF) { + if (ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_SNS) < 0) + goto err; + dialect_modified = true; + } + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Can not mix NS-VC with different link layer%s", VTY_NEWLINE); + goto err; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + vty_out(vty, "Can not mix NS-VC with different dialects%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_from_str(&remote_str, argv[0], port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + goto err; + } + + rc = gprs_ns2_sns_add_endpoint(nse, &remote); + switch (rc) { + case 0: + return CMD_SUCCESS; + case -EADDRINUSE: + vty_out(vty, "Specified SNS endpoint already part of the NSE.%s", VTY_NEWLINE); + return CMD_WARNING; + default: + vty_out(vty, "Can not add specified SNS endpoint.%s", VTY_NEWLINE); + return CMD_WARNING; + } + +err: + if (ll_modified) + nse->ll = GPRS_NS2_LL_UNDEF; + if (dialect_modified) + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + return CMD_WARNING; +} + +DEFUN(cfg_no_ns_nse_ip_sns_remote, cfg_no_ns_nse_ip_sns_remote_cmd, + "no ip-sns-remote " VTY_IPV46_CMD " <1-65535>", + NO_STR + "Delete a SNS Initial Endpoint\n" + "SGSN IPv4 Address\n" "SGSN IPv6 Address\n" + "SGSN UDP Port\n" + ) +{ + struct gprs_ns2_nse *nse = vty->index; + struct osmo_sockaddr_str remote_str; /* argv[0] */ + struct osmo_sockaddr remote; + uint16_t port = atoi(argv[1]); + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "This NSE doesn't support UDP.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + vty_out(vty, "This NSE doesn't support UDP with dialect ip-sns.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_from_str(&remote_str, argv[0], port)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (osmo_sockaddr_str_to_sockaddr(&remote_str, &remote.u.sas)) { + vty_out(vty, "Can not parse IPv4/IPv6 or port.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (gprs_ns2_sns_del_endpoint(nse, &remote)) { + vty_out(vty, "Can not remove specified SNS endpoint.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (vty_nse_check_sns(nse)) { + /* there is still sns configuration valid */ + return CMD_SUCCESS; + } else { + /* clean up nse to allow other nsvc commands */ + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + nse->ll = GPRS_NS2_LL_UNDEF; + } + + return CMD_SUCCESS; +} + +/* add all IP-SNS default binds to the given NSE */ +int ns2_sns_add_sns_default_binds(struct gprs_ns2_nse *nse) +{ + struct vty_nse_bind *vnse_bind; + int count = 0; + + OSMO_ASSERT(nse->ll == GPRS_NS2_LL_UDP); + OSMO_ASSERT(nse->dialect == GPRS_NS2_DIALECT_SNS); + + llist_for_each_entry(vnse_bind, &ip_sns_default_binds, list) { + struct gprs_ns2_vc_bind *bind = gprs_ns2_bind_by_name(vty_nsi, vnse_bind->vbind->name); + /* the bind might not yet created because "listen" is missing. */ + if (!bind) + continue; + gprs_ns2_sns_add_bind(nse, bind); + count++; + } + return count; +} + +DEFUN(cfg_ns_ip_sns_default_bind, cfg_ns_ip_sns_default_bind_cmd, + "ip-sns-default bind ID", + "Defaults for dynamically created NSEs created by IP-SNS in SGSN role\n" + "IP SNS binds\n" + "Name of NS udp bind whose IP endpoint will be used as IP-SNS local endpoint. Can be given multiple times.\n") +{ + struct vty_bind *vbind; + struct vty_nse_bind *vnse_bind; + const char *name = argv[0]; + + vbind = vty_bind_by_name(name); + if (!vbind) { + vty_out(vty, "Can not find the given bind '%s'%s", name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "ip-sns-default bind can only be used with UDP bind%s", VTY_NEWLINE); + return CMD_WARNING; + } + + llist_for_each_entry(vnse_bind, &ip_sns_default_binds, list) { + if (vnse_bind->vbind == vbind) + return CMD_SUCCESS; + } + + vnse_bind = talloc(vty_nsi, struct vty_nse_bind); + if (!vnse_bind) + return CMD_WARNING; + vnse_bind->vbind = vbind; + + llist_add_tail(&vnse_bind->list, &ip_sns_default_binds); + + return CMD_SUCCESS; +} + +DEFUN(cfg_no_ns_ip_sns_default_bind, cfg_no_ns_ip_sns_default_bind_cmd, + "no ip-sns-default bind ID", + NO_STR "Defaults for dynamically created NSEs created by IP-SNS in SGSN role\n" + "IP SNS binds\n" + "Name of NS udp bind whose IP endpoint will be removed as IP-SNS local endpoint.\n") +{ + struct vty_bind *vbind; + struct vty_nse_bind *vnse_bind; + const char *name = argv[0]; + + vbind = vty_bind_by_name(name); + if (!vbind) { + vty_out(vty, "Can not find the given bind '%s'%s", name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "ip-sns-default bind can only be used with UDP bind%s", VTY_NEWLINE); + return CMD_WARNING; + } + + llist_for_each_entry(vnse_bind, &ip_sns_default_binds, list) { + if (vnse_bind->vbind == vbind) { + llist_del(&vnse_bind->list); + talloc_free(vnse_bind); + return CMD_SUCCESS; + } + } + + vty_out(vty, "Bind '%s' was not an ip-sns-default bind%s", name, VTY_NEWLINE); + return CMD_WARNING; +} + +DEFUN(cfg_ns_txqueue_max_length, cfg_ns_txqueue_max_length_cmd, + "txqueue-max-length <1-4096>", + "Set the maximum length of the txqueue (for UDP)\n" + "Maximum length of the txqueue\n") +{ + struct gprs_ns2_vc_bind *bind; + uint32_t max_length = atoi(argv[0]); + vty_nsi->txqueue_max_length = max_length; + + + llist_for_each_entry(bind, &vty_nsi->binding, list) { + if (!gprs_ns2_is_ip_bind(bind)) + continue; + + ns2_ip_set_txqueue_max_length(bind, max_length); + } + + + return CMD_SUCCESS; +} + +DEFUN(cfg_ns_nse_ip_sns_bind, cfg_ns_nse_ip_sns_bind_cmd, + "ip-sns-bind BINDID", + "IP SNS binds\n" + "Name of NS udp bind whose IP endpoint will be used as IP-SNS local endpoint. Can be given multiple times.\n") +{ + struct gprs_ns2_nse *nse = vty->index; + struct gprs_ns2_vc_bind *bind; + struct vty_bind *vbind; + struct vty_nse *vnse; + const char *name = argv[0]; + bool ll_modified = false; + bool dialect_modified = false; + int rc; + + if (nse->ll == GPRS_NS2_LL_UNDEF) { + nse->ll = GPRS_NS2_LL_UDP; + ll_modified = true; + } + + if (nse->dialect == GPRS_NS2_DIALECT_UNDEF) { + if (ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_SNS) < 0) + goto err; + dialect_modified = true; + } + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "Can not mix NS-VC with different link layer%s", VTY_NEWLINE); + goto err; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + vty_out(vty, "Can not mix NS-VC with different dialects%s", VTY_NEWLINE); + goto err; + } + + vbind = vty_bind_by_name(name); + if (!vbind) { + vty_out(vty, "Can not find the given bind '%s'%s", name, VTY_NEWLINE); + goto err; + } + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "ip-sns-bind can only be used with UDP bind%s", + VTY_NEWLINE); + goto err; + } + + /* the vnse has been created together when creating the nse node. The parent node should check this already! */ + vnse = vty_nse_by_nsei(nse->nsei); + OSMO_ASSERT(vnse); + + rc = vty_nse_add_vbind(vnse, vbind); + switch (rc) { + case 0: + break; + case -EALREADY: + vty_out(vty, "Failed to add ip-sns-bind %s already present%s", name, VTY_NEWLINE); + goto err; + case -ENOMEM: + vty_out(vty, "Failed to add ip-sns-bind %s out of memory%s", name, VTY_NEWLINE); + goto err; + default: + vty_out(vty, "Failed to add ip-sns-bind %s! %d%s", name, rc, VTY_NEWLINE); + goto err; + } + + /* the bind might not yet created because "listen" is missing. */ + bind = gprs_ns2_bind_by_name(vty_nsi, name); + if (!bind) + return CMD_SUCCESS; + + rc = gprs_ns2_sns_add_bind(nse, bind); + switch (rc) { + case 0: + break; + case -EALREADY: + vty_out(vty, "Failed to add ip-sns-bind %s already present%s", name, VTY_NEWLINE); + goto err; + case -ENOMEM: + vty_out(vty, "Failed to add ip-sns-bind %s out of memory%s", name, VTY_NEWLINE); + goto err; + default: + vty_out(vty, "Failed to add ip-sns-bind %s! %d%s", name, rc, VTY_NEWLINE); + goto err; + } + + return CMD_SUCCESS; +err: + if (ll_modified) + nse->ll = GPRS_NS2_LL_UNDEF; + if (dialect_modified) + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + + return CMD_WARNING; +} + +DEFUN(cfg_no_ns_nse_ip_sns_bind, cfg_no_ns_nse_ip_sns_bind_cmd, + "no ip-sns-bind BINDID", + NO_STR + "IP SNS binds\n" + "Name of NS udp bind whose IP endpoint will not be used as IP-SNS local endpoint\n") +{ + struct gprs_ns2_nse *nse = vty->index; + struct gprs_ns2_vc_bind *bind; + struct vty_bind *vbind; + struct vty_nse *vnse; + const char *name = argv[0]; + int rc; + + if (nse->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "This NSE doesn't support UDP.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + vty_out(vty, "This NSE doesn't support UDP with dialect ip-sns.%s", VTY_NEWLINE); + return CMD_WARNING; + } + + vbind = vty_bind_by_name(name); + if (!vbind) { + vty_out(vty, "Can not find the given bind '%s'%s", name, VTY_NEWLINE); + return CMD_WARNING; + } + + if (vbind->ll != GPRS_NS2_LL_UDP) { + vty_out(vty, "no ip-sns-bind can only be used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + /* the vnse has been created together when creating the nse node. The parent node should check this already! */ + vnse = vty_nse_by_nsei(nse->nsei); + OSMO_ASSERT(vnse); + + rc = vty_nse_remove_vbind(vnse, vbind); + switch(rc) { + case 0: + break; + case -ENOENT: + vty_out(vty, "Bind %s is not part of this NSE%s", name, VTY_NEWLINE); + return CMD_WARNING; + case -EINVAL: + vty_out(vty, "no ip-sns-bind can only be used with UDP bind%s", + VTY_NEWLINE); + return CMD_WARNING; + default: + return CMD_WARNING; + } + + /* the bind might not exists yet */ + bind = gprs_ns2_bind_by_name(vty_nsi, name); + if (bind) + gprs_ns2_sns_del_bind(nse, bind); + + if (!vty_nse_check_sns(nse)) { + /* clean up nse to allow other nsvc commands */ + ns2_nse_set_dialect(nse, GPRS_NS2_DIALECT_UNDEF); + nse->ll = GPRS_NS2_LL_UNDEF; + } + + return CMD_SUCCESS; +} + +/* non-config commands */ +void ns2_vty_dump_nsvc(struct vty *vty, struct gprs_ns2_vc *nsvc, bool stats) +{ + if (nsvc->nsvci_is_valid) + vty_out(vty, " NSVCI %05u: %s %s %s %s %ssince ", nsvc->nsvci, + osmo_fsm_inst_state_name(nsvc->fi), + nsvc->persistent ? "PERSIST" : "DYNAMIC", + gprs_ns2_ll_str(nsvc), + ns2_vc_is_unblocked(nsvc) ? "ALIVE" : "DEAD", + nsvc->om_blocked ? "(blocked by O&M/vty) " : + !ns2_vc_is_unblocked(nsvc) ? "(cause: remote) " : ""); + else + vty_out(vty, " %s %s sig_weight=%u data_weight=%u %s %s %ssince ", + osmo_fsm_inst_state_name(nsvc->fi), + nsvc->persistent ? "PERSIST" : "DYNAMIC", + nsvc->sig_weight, nsvc->data_weight, + gprs_ns2_ll_str(nsvc), + ns2_vc_is_unblocked(nsvc) ? "ALIVE" : "DEAD", + !ns2_vc_is_unblocked(nsvc) ? "(cause: remote) " : ""); + + vty_out_uptime(vty, &nsvc->ts_alive_change); + vty_out_newline(vty); + + if (stats) { + vty_out_rate_ctr_group(vty, " ", nsvc->ctrg); + vty_out_stat_item_group(vty, " ", nsvc->statg); + } +} + +static void dump_nse(struct vty *vty, const struct gprs_ns2_nse *nse, bool stats, bool persistent_only) +{ + struct gprs_ns2_vc *nsvc; + unsigned int nsvcs = 0; + + if (persistent_only && !nse->persistent) + return; + + vty_out(vty, "NSEI %05u: %s, %s since ", nse->nsei, gprs_ns2_lltype_str(nse->ll), + nse->alive ? "ALIVE" : "DEAD"); + vty_out_uptime(vty, &nse->ts_alive_change); + vty_out_newline(vty); + + ns2_sns_dump_vty(vty, " ", nse, stats); + llist_for_each_entry(nsvc, &nse->nsvc, list) { + nsvcs++; + } + vty_out(vty, " %u NS-VC:%s", nsvcs, VTY_NEWLINE); + llist_for_each_entry(nsvc, &nse->nsvc, list) + ns2_vty_dump_nsvc(vty, nsvc, stats); +} + +static void dump_bind(struct vty *vty, const struct gprs_ns2_vc_bind *bind, bool stats) +{ + if (bind->dump_vty) + bind->dump_vty(bind, vty, stats); + + if (stats) { + vty_out_stat_item_group(vty, " ", bind->statg); + } +} + +static void dump_ns_bind(struct vty *vty, const struct gprs_ns2_inst *nsi, bool stats) +{ + struct gprs_ns2_vc_bind *bind; + + llist_for_each_entry(bind, &nsi->binding, list) { + dump_bind(vty, bind, stats); + } +} + + +static void dump_ns_entities(struct vty *vty, const struct gprs_ns2_inst *nsi, bool stats, bool persistent_only) +{ + struct gprs_ns2_nse *nse; + + llist_for_each_entry(nse, &nsi->nse, list) { + dump_nse(vty, nse, stats, persistent_only); + } +} + +/* Backwards compatibility, among other things for the TestVTYGbproxy which expects + * 'show ns' to output something about binds */ +DEFUN_HIDDEN(show_ns, show_ns_cmd, "show ns", + SHOW_STR SHOW_NS_STR) +{ + dump_ns_entities(vty, vty_nsi, false, false); + dump_ns_bind(vty, vty_nsi, false); + if (vty_fr_network && llist_count(&vty_fr_network->links)) + osmo_fr_network_dump_vty(vty, vty_fr_network); + return CMD_SUCCESS; +} + + +DEFUN(show_ns_binds, show_ns_binds_cmd, "show ns binds [stats]", + SHOW_STR SHOW_NS_STR + "Display information about the NS protocol binds\n" + "Include statistic\n") +{ + bool stats = false; + if (argc > 0) + stats = true; + + dump_ns_bind(vty, vty_nsi, stats); + return CMD_SUCCESS; +} + +DEFUN(show_ns_entities, show_ns_entities_cmd, "show ns entities [stats]", + SHOW_STR SHOW_NS_STR + "Display information about the NS protocol entities (NSEs)\n" + "Include statistics\n") +{ + bool stats = false; + if (argc > 0) + stats = true; + + dump_ns_entities(vty, vty_nsi, stats, false); + return CMD_SUCCESS; +} + +DEFUN(show_ns_pers, show_ns_pers_cmd, "show ns persistent", + SHOW_STR SHOW_NS_STR + "Show only persistent NS\n") +{ + dump_ns_entities(vty, vty_nsi, true, true); + return CMD_SUCCESS; +} + +DEFUN(show_nse, show_nse_cmd, "show ns (nsei|nsvc) <0-65535> [stats]", + SHOW_STR SHOW_NS_STR + "Select one NSE by its NSE Identifier\n" + "Select one NSE by its NS-VC Identifier\n" + "The Identifier of selected type\n" + "Include Statistics\n") +{ + struct gprs_ns2_inst *nsi = vty_nsi; + struct gprs_ns2_nse *nse; + struct gprs_ns2_vc *nsvc; + uint16_t id = atoi(argv[1]); + bool show_stats = false; + + if (argc >= 3) + show_stats = true; + + if (!strcmp(argv[0], "nsei")) { + nse = gprs_ns2_nse_by_nsei(nsi, id); + if (!nse) { + return CMD_WARNING; + } + + dump_nse(vty, nse, show_stats, false); + } else { + nsvc = gprs_ns2_nsvc_by_nsvci(nsi, id); + + if (!nsvc) { + vty_out(vty, "No such NS Entity%s", VTY_NEWLINE); + return CMD_WARNING; + } + + ns2_vty_dump_nsvc(vty, nsvc, show_stats); + } + + return CMD_SUCCESS; +} + +static int nsvc_force_unconf_cb(struct gprs_ns2_vc *nsvc, void *ctx) +{ + ns2_vc_force_unconfigured(nsvc); + ns2_vc_fsm_start(nsvc); + return 0; +} + +DEFUN_HIDDEN(nsvc_force_unconf, nsvc_force_unconf_cmd, + "nsvc nsei <0-65535> force-unconfigured", + "NS Virtual Connection\n" + "The NSEI\n" + "Reset the NSVCs back to initial state\n" + ) +{ + struct gprs_ns2_inst *nsi = vty_nsi; + struct gprs_ns2_nse *nse; + + uint16_t id = atoi(argv[0]); + + nse = gprs_ns2_nse_by_nsei(nsi, id); + if (!nse) { + vty_out(vty, "Could not find NSE for NSEI %u%s", id, VTY_NEWLINE); + return CMD_WARNING; + } + + if (!nse->persistent) { + gprs_ns2_free_nse(nse); + } else if (nse->dialect == GPRS_NS2_DIALECT_SNS) { + gprs_ns2_free_nsvcs(nse); + } else { + /* Perform the operation for all nsvc */ + gprs_ns2_nse_foreach_nsvc(nse, nsvc_force_unconf_cb, NULL); + } + + return CMD_SUCCESS; +} + +DEFUN(nse_restart_sns, nse_restart_sns_cmd, + "nse <0-65535> restart-sns", + "NSE specific commands\n" + "NS Entity ID (NSEI)\n" + "Restart SNS procedure\n") +{ + struct gprs_ns2_inst *nsi = vty_nsi; + struct gprs_ns2_nse *nse; + + uint16_t id = atoi(argv[0]); + nse = gprs_ns2_nse_by_nsei(nsi, id); + if (!nse) { + vty_out(vty, "Could not find NSE for NSEI %u%s", id, VTY_NEWLINE); + return CMD_WARNING; + } + + if (nse->dialect != GPRS_NS2_DIALECT_SNS) { + vty_out(vty, "Given NSEI %u doesn't use IP-SNS%s", id, VTY_NEWLINE); + return CMD_WARNING; + } + + gprs_ns2_free_nsvcs(nse); + return CMD_SUCCESS; +} + +DEFUN(nsvc_block, nsvc_block_cmd, + "nsvc <0-65535> (block|unblock|reset)", + "NS Virtual Connection\n" + NSVCI_STR + "Block a NSVC. As cause code O&M intervention will be used.\n" + "Unblock a NSVC. As cause code O&M intervention will be used.\n" + "Reset a NSVC. As cause code O&M intervention will be used.\n") +{ + struct gprs_ns2_inst *nsi = vty_nsi; + struct gprs_ns2_vc *nsvc; + int rc; + + uint16_t id = atoi(argv[0]); + + nsvc = gprs_ns2_nsvc_by_nsvci(nsi, id); + if (!nsvc) { + vty_out(vty, "Could not find NSVCI %05u%s", id, VTY_NEWLINE); + return CMD_WARNING; + } + + if (!strcmp(argv[1], "block")) { + rc = ns2_vc_block(nsvc); + switch (rc) { + case 0: + vty_out(vty, "The NS-VC %05u will be blocked.%s", id, VTY_NEWLINE); + return CMD_SUCCESS; + case -EALREADY: + vty_out(vty, "The NS-VC %05u is already blocked.%s", id, VTY_NEWLINE); + return CMD_ERR_NOTHING_TODO; + default: + vty_out(vty, "An unknown error %d happend on NS-VC %05u.%s", rc, id, VTY_NEWLINE); + return CMD_WARNING; + } + } else if (!strcmp(argv[1], "unblock")) { + rc = ns2_vc_unblock(nsvc); + switch (rc) { + case 0: + vty_out(vty, "The NS-VC %05u will be unblocked.%s", id, VTY_NEWLINE); + return CMD_SUCCESS; + case -EALREADY: + vty_out(vty, "The NS-VC %05u is already unblocked.%s", id, VTY_NEWLINE); + return CMD_ERR_NOTHING_TODO; + default: + vty_out(vty, "An unknown error %d happend on NS-VC %05u.%s", rc, id, VTY_NEWLINE); + return CMD_WARNING; + } + } else { + ns2_vc_reset(nsvc); + vty_out(vty, "The NS-VC %05u has been resetted.%s", id, VTY_NEWLINE); + } + + return CMD_SUCCESS; +} + +static void log_set_nse_filter(struct log_target *target, + struct gprs_ns2_nse *nse) +{ + if (nse) { + target->filter_map |= (1 << LOG_FLT_GB_NSE); + target->filter_data[LOG_FLT_GB_NSE] = nse; + } else if (target->filter_data[LOG_FLT_GB_NSE]) { + target->filter_map = ~(1 << LOG_FLT_GB_NSE); + target->filter_data[LOG_FLT_GB_NSE] = NULL; + } +} + +static void log_set_nsvc_filter(struct log_target *target, + struct gprs_ns2_vc *nsvc) +{ + if (nsvc) { + target->filter_map |= (1 << LOG_FLT_GB_NSVC); + target->filter_data[LOG_FLT_GB_NSVC] = nsvc; + } else if (target->filter_data[LOG_FLT_GB_NSVC]) { + target->filter_map = ~(1 << LOG_FLT_GB_NSVC); + target->filter_data[LOG_FLT_GB_NSVC] = NULL; + } +} + +DEFUN(logging_fltr_nse, + logging_fltr_nse_cmd, + "logging filter nse nsei <0-65535>", + LOGGING_STR FILTER_STR + "Filter based on NS Entity\n" + "Identify NSE by NSEI\n" + "Numeric identifier\n") +{ + struct log_target *tgt; + struct gprs_ns2_nse *nse; + uint16_t id = atoi(argv[0]); + + log_tgt_mutex_lock(); + tgt = osmo_log_vty2tgt(vty); + if (!tgt) { + log_tgt_mutex_unlock(); + return CMD_WARNING; + } + + nse = gprs_ns2_nse_by_nsei(vty_nsi, id); + if (!nse) { + vty_out(vty, "No NSE by that identifier%s", VTY_NEWLINE); + log_tgt_mutex_unlock(); + return CMD_WARNING; + } + + log_set_nse_filter(tgt, nse); + log_tgt_mutex_unlock(); + return CMD_SUCCESS; +} + +/* TODO: add filter for single connection by description */ +DEFUN(logging_fltr_nsvc, + logging_fltr_nsvc_cmd, + "logging filter nsvc nsvci <0-65535>", + LOGGING_STR FILTER_STR + "Filter based on NS Virtual Connection\n" + "Identify NS-VC by NSVCI\n" + "Numeric identifier\n") +{ + struct log_target *tgt; + struct gprs_ns2_vc *nsvc; + uint16_t id = atoi(argv[0]); + + log_tgt_mutex_lock(); + tgt = osmo_log_vty2tgt(vty); + if (!tgt) { + log_tgt_mutex_unlock(); + return CMD_WARNING; + } + + nsvc = gprs_ns2_nsvc_by_nsvci(vty_nsi, id); + if (!nsvc) { + vty_out(vty, "No NS-VC by that identifier%s", VTY_NEWLINE); + log_tgt_mutex_unlock(); + return CMD_WARNING; + } + + log_set_nsvc_filter(tgt, nsvc); + log_tgt_mutex_unlock(); + return CMD_SUCCESS; +} + +/*! initialized a reduced vty interface which excludes the configuration nodes besides timeouts. + * This can be used by the PCU which can be only configured by the BTS/BSC and not by the vty. + * \param[in] nsi NS instance on which we operate + * \return 0 on success. + */ +int gprs_ns2_vty_init_reduced(struct gprs_ns2_inst *nsi) +{ + vty_nsi = nsi; + INIT_LLIST_HEAD(&binds); + INIT_LLIST_HEAD(&nses); + INIT_LLIST_HEAD(&ip_sns_default_binds); + + vty_fr_network = osmo_fr_network_alloc(nsi); + if (!vty_fr_network) + return -ENOMEM; + + install_lib_element_ve(&show_ns_cmd); + install_lib_element_ve(&show_ns_binds_cmd); + install_lib_element_ve(&show_ns_entities_cmd); + install_lib_element_ve(&show_ns_pers_cmd); + install_lib_element_ve(&show_nse_cmd); + install_lib_element_ve(&logging_fltr_nse_cmd); + install_lib_element_ve(&logging_fltr_nsvc_cmd); + + install_lib_element(ENABLE_NODE, &nsvc_force_unconf_cmd); + install_lib_element(ENABLE_NODE, &nsvc_block_cmd); + install_lib_element(ENABLE_NODE, &nse_restart_sns_cmd); + + install_lib_element(CFG_LOG_NODE, &logging_fltr_nse_cmd); + install_lib_element(CFG_LOG_NODE, &logging_fltr_nsvc_cmd); + + install_lib_element(CONFIG_NODE, &cfg_ns_cmd); + + install_node(&ns_node, config_write_ns); + /* TODO: convert into osmo timer */ + install_lib_element(L_NS_NODE, &cfg_ns_timer_cmd); + + return 0; +} + +int gprs_ns2_vty_init(struct gprs_ns2_inst *nsi) +{ + int rc = gprs_ns2_vty_init_reduced(nsi); + if (rc) + return rc; + + install_lib_element(L_NS_NODE, &cfg_ns_nsei_cmd); + install_lib_element(L_NS_NODE, &cfg_no_ns_nsei_cmd); + install_lib_element(L_NS_NODE, &cfg_ns_bind_cmd); + install_lib_element(L_NS_NODE, &cfg_no_ns_bind_cmd); + + install_lib_element(L_NS_NODE, &cfg_ns_ip_sns_default_bind_cmd); + install_lib_element(L_NS_NODE, &cfg_no_ns_ip_sns_default_bind_cmd); + + install_lib_element(L_NS_NODE, &cfg_ns_txqueue_max_length_cmd); + + install_node(&ns_bind_node, NULL); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_listen_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_no_ns_bind_listen_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_dscp_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_no_ns_bind_dscp_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_priority_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_ip_sns_weight_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_ipaccess_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_no_ns_bind_ipaccess_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_fr_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_no_ns_bind_fr_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_ns_bind_accept_sns_cmd); + install_lib_element(L_NS_BIND_NODE, &cfg_no_ns_bind_accept_sns_cmd); + + install_node(&ns_nse_node, NULL); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_nsvc_fr_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_nsvci_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_nsvc_fr_dlci_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_nsvc_udp_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_nsvc_udp_weights_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_nsvc_udp_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_nsvc_ipa_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_nsvc_ipa_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_ip_sns_remote_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_ip_sns_remote_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_ns_nse_ip_sns_bind_cmd); + install_lib_element(L_NS_NSE_NODE, &cfg_no_ns_nse_ip_sns_bind_cmd); + + return 0; +} diff --git a/src/gb/gprs_ns_vty.c b/src/gb/gprs_ns_vty.c index 9cffb71d..f27bf6a8 100644 --- a/src/gb/gprs_ns_vty.c +++ b/src/gb/gprs_ns_vty.c @@ -90,6 +90,32 @@ static int config_write_ns(struct vty *vty) vty_out(vty, "ns%s", VTY_NEWLINE); + /* global configuration must be written first, as some of it may be + * relevant when creating the NSE/NSVC later below */ + + if (vty_nsi->nsip.local_ip) { + ia.s_addr = osmo_htonl(vty_nsi->nsip.local_ip); + vty_out(vty, " encapsulation udp local-ip %s%s", + inet_ntoa(ia), VTY_NEWLINE); + } + if (vty_nsi->nsip.local_port) + vty_out(vty, " encapsulation udp local-port %u%s", + vty_nsi->nsip.local_port, VTY_NEWLINE); + if (vty_nsi->nsip.dscp) + vty_out(vty, " encapsulation udp dscp %d%s", + vty_nsi->nsip.dscp, VTY_NEWLINE); + + vty_out(vty, " encapsulation udp use-reset-block-unblock %s%s", + vty_nsi->nsip.use_reset_block_unblock ? "enabled" : "disabled", VTY_NEWLINE); + + vty_out(vty, " encapsulation framerelay-gre enabled %u%s", + vty_nsi->frgre.enabled ? 1 : 0, VTY_NEWLINE); + if (vty_nsi->frgre.local_ip) { + ia.s_addr = osmo_htonl(vty_nsi->frgre.local_ip); + vty_out(vty, " encapsulation framerelay-gre local-ip %s%s", + inet_ntoa(ia), VTY_NEWLINE); + } + llist_for_each_entry(nsvc, &vty_nsi->gprs_nsvcs, list) { if (!nsvc->persistent) continue; @@ -130,26 +156,6 @@ static int config_write_ns(struct vty *vty) get_value_string(gprs_ns_timer_strs, i), vty_nsi->timeout[i], VTY_NEWLINE); - if (vty_nsi->nsip.local_ip) { - ia.s_addr = osmo_htonl(vty_nsi->nsip.local_ip); - vty_out(vty, " encapsulation udp local-ip %s%s", - inet_ntoa(ia), VTY_NEWLINE); - } - if (vty_nsi->nsip.local_port) - vty_out(vty, " encapsulation udp local-port %u%s", - vty_nsi->nsip.local_port, VTY_NEWLINE); - if (vty_nsi->nsip.dscp) - vty_out(vty, " encapsulation udp dscp %d%s", - vty_nsi->nsip.dscp, VTY_NEWLINE); - - vty_out(vty, " encapsulation framerelay-gre enabled %u%s", - vty_nsi->frgre.enabled ? 1 : 0, VTY_NEWLINE); - if (vty_nsi->frgre.local_ip) { - ia.s_addr = osmo_htonl(vty_nsi->frgre.local_ip); - vty_out(vty, " encapsulation framerelay-gre local-ip %s%s", - inet_ntoa(ia), VTY_NEWLINE); - } - return CMD_SUCCESS; } @@ -286,7 +292,7 @@ DEFUN(cfg_nse_nsvc, cfg_nse_nsvci_cmd, nsvc = gprs_nsvc_by_nsei(vty_nsi, nsei); if (!nsvc) { - nsvc = gprs_nsvc_create(vty_nsi, nsvci); + nsvc = gprs_nsvc_create2(vty_nsi, nsvci, 1, 1); nsvc->nsei = nsei; } nsvc->nsvci = nsvci; @@ -312,6 +318,7 @@ DEFUN(cfg_nse_remoteip, cfg_nse_remoteip_cmd, vty_out(vty, "No such NSE (%u)%s", nsei, VTY_NEWLINE); return CMD_WARNING; } + nsvc->ip.bts_addr.sin_family = AF_INET; inet_aton(argv[1], &nsvc->ip.bts_addr.sin_addr); return CMD_SUCCESS; @@ -500,6 +507,21 @@ DEFUN(cfg_nsip_dscp, cfg_nsip_dscp_cmd, return CMD_SUCCESS; } +DEFUN(cfg_nsip_res_block_unblock, cfg_nsip_res_block_unblock_cmd, + "encapsulation udp use-reset-block-unblock (enabled|disabled)", + ENCAPS_STR "NS over UDP Encapsulation\n" + "Use NS-{RESET,BLOCK,UNBLOCK} procedures in violation of 3GPP TS 48.016\n" + "Enable NS-{RESET,BLOCK,UNBLOCK}\n" + "Disable NS-{RESET,BLOCK,UNBLOCK}\n") +{ + if (!strcmp(argv[0], "enabled")) + vty_nsi->nsip.use_reset_block_unblock = true; + else + vty_nsi->nsip.use_reset_block_unblock = false; + + return CMD_SUCCESS; +} + DEFUN(cfg_frgre_local_ip, cfg_frgre_local_ip_cmd, "encapsulation framerelay-gre local-ip A.B.C.D", ENCAPS_STR "NS over Frame Relay over GRE Encapsulation\n" @@ -622,31 +644,32 @@ int gprs_ns_vty_init(struct gprs_ns_inst *nsi) return 0; vty_elements_installed = true; - install_element_ve(&show_ns_cmd); - install_element_ve(&show_ns_stats_cmd); - install_element_ve(&show_ns_pers_cmd); - install_element_ve(&show_nse_cmd); - install_element_ve(&logging_fltr_nsvc_cmd); + install_lib_element_ve(&show_ns_cmd); + install_lib_element_ve(&show_ns_stats_cmd); + install_lib_element_ve(&show_ns_pers_cmd); + install_lib_element_ve(&show_nse_cmd); + install_lib_element_ve(&logging_fltr_nsvc_cmd); - install_element(CFG_LOG_NODE, &logging_fltr_nsvc_cmd); + install_lib_element(CFG_LOG_NODE, &logging_fltr_nsvc_cmd); - install_element(CONFIG_NODE, &cfg_ns_cmd); + install_lib_element(CONFIG_NODE, &cfg_ns_cmd); install_node(&ns_node, config_write_ns); - install_element(L_NS_NODE, &cfg_nse_nsvci_cmd); - install_element(L_NS_NODE, &cfg_nse_remoteip_cmd); - install_element(L_NS_NODE, &cfg_nse_remoteport_cmd); - install_element(L_NS_NODE, &cfg_nse_fr_dlci_cmd); - install_element(L_NS_NODE, &cfg_nse_encaps_cmd); - install_element(L_NS_NODE, &cfg_nse_remoterole_cmd); - install_element(L_NS_NODE, &cfg_no_nse_cmd); - install_element(L_NS_NODE, &cfg_ns_timer_cmd); - install_element(L_NS_NODE, &cfg_nsip_local_ip_cmd); - install_element(L_NS_NODE, &cfg_nsip_local_port_cmd); - install_element(L_NS_NODE, &cfg_nsip_dscp_cmd); - install_element(L_NS_NODE, &cfg_frgre_enable_cmd); - install_element(L_NS_NODE, &cfg_frgre_local_ip_cmd); - - install_element(ENABLE_NODE, &nsvc_nsei_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_nsvci_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_remoteip_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_remoteport_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_fr_dlci_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_encaps_cmd); + install_lib_element(L_NS_NODE, &cfg_nse_remoterole_cmd); + install_lib_element(L_NS_NODE, &cfg_no_nse_cmd); + install_lib_element(L_NS_NODE, &cfg_ns_timer_cmd); + install_lib_element(L_NS_NODE, &cfg_nsip_local_ip_cmd); + install_lib_element(L_NS_NODE, &cfg_nsip_local_port_cmd); + install_lib_element(L_NS_NODE, &cfg_nsip_dscp_cmd); + install_lib_element(L_NS_NODE, &cfg_nsip_res_block_unblock_cmd); + install_lib_element(L_NS_NODE, &cfg_frgre_enable_cmd); + install_lib_element(L_NS_NODE, &cfg_frgre_local_ip_cmd); + + install_lib_element(ENABLE_NODE, &nsvc_nsei_cmd); return 0; } diff --git a/src/gb/libosmogb.map b/src/gb/libosmogb.map index ad139c1c..e5a5c8fd 100644 --- a/src/gb/libosmogb.map +++ b/src/gb/libosmogb.map @@ -2,7 +2,25 @@ LIBOSMOGB_1.0 { global: bssgp_cause_str; bssgp_create_cell_id; +bssgp_create_rim_ri; +bssgp_dec_app_err_cont_nacc; +bssgp_dec_ran_inf_ack_rim_cont; +bssgp_dec_ran_inf_err_rim_cont; +bssgp_dec_ran_inf_req_app_cont_nacc; +bssgp_dec_ran_inf_req_rim_cont; +bssgp_dec_ran_inf_app_cont_nacc; +bssgp_dec_ran_inf_app_err_rim_cont; +bssgp_dec_ran_inf_rim_cont; bssgp_pdu_str; +bssgp_enc_app_err_cont_nacc; +bssgp_enc_ran_inf_ack_rim_cont; +bssgp_enc_ran_inf_err_rim_cont; +bssgp_enc_ran_inf_req_app_cont_nacc; +bssgp_enc_ran_inf_req_rim_cont; +bssgp_enc_ran_inf_app_cont_nacc; +bssgp_enc_ran_inf_app_err_rim_cont; +bssgp_enc_ran_inf_rim_cont; +bssgp_encode_rim_pdu; bssgp_fc_in; bssgp_fc_init; bssgp_fc_ms_init; @@ -12,9 +30,20 @@ bssgp_msgb_alloc; bssgp_msgb_copy; bssgp_msgb_tlli_put; bssgp_msgb_ra_put; +bssgp_nacc_cause_strs; bssgp_parse_cell_id; +bssgp_parse_rim_pdu; +bssgp_parse_rim_ri; +bssgp_parse_rim_ra; +bssgp_ran_inf_app_id_strs; +bssgp_rim_routing_info_discr_strs; +bssgp_rim_ri_name_buf; +bssgp_rim_ri_name; +bssgp_set_bssgp_callback; bssgp_tx_bvc_block; bssgp_tx_bvc_reset; +bssgp_tx_bvc_reset2; +bssgp_tx_bvc_reset_nsei_bvci; bssgp_tx_bvc_unblock; bssgp_tx_fc_bvc; bssgp_tx_fc_ms; @@ -27,6 +56,8 @@ bssgp_tx_radio_status_tmsi; bssgp_tx_resume; bssgp_tx_resume_ack; bssgp_tx_resume_nack; +bssgp_tx_rim; +bssgp_tx_rim_encoded; bssgp_tx_simple_bvci; bssgp_tx_status; bssgp_tx_suspend; @@ -42,6 +73,45 @@ bssgp_tx_paging; bssgp_vty_init; bssgp_nsi; +bssgp2_nsi_tx_ptp; +bssgp2_nsi_tx_sig; +bssgp2_dec_fc_bvc; +bssgp2_dec_fc_ms; +bssgp2_enc_bvc_block; +bssgp2_enc_bvc_block_ack; +bssgp2_enc_bvc_unblock; +bssgp2_enc_bvc_unblock_ack; +bssgp2_enc_bvc_reset; +bssgp2_enc_bvc_reset_ack; +bssgp2_enc_fc_bvc; +bssgp2_enc_fc_bvc_ack; +bssgp2_enc_fc_ms; +bssgp2_enc_fc_ms_ack; +bssgp2_enc_flush_ll; +bssgp2_enc_status; + +bssgp_bvc_fsm_alloc_sig_bss; +bssgp_bvc_fsm_alloc_ptp_bss; +bssgp_bvc_fsm_alloc_sig_sgsn; +bssgp_bvc_fsm_alloc_ptp_sgsn; +bssgp_bvc_fsm_set_ops; +bssgp_bvc_fsm_is_unblocked; +bssgp_bvc_fsm_get_block_cause; +bssgp_bvc_fsm_get_features_advertised; +bssgp_bvc_fsm_get_features_received; +bssgp_bvc_fsm_get_features_negotiated; +bssgp_bvc_fsm_set_max_pdu_len; +bssgp_bvc_fsm_get_max_pdu_len; + +osmo_fr_network_alloc; +osmo_fr_network_free; +osmo_fr_link_alloc; +osmo_fr_link_free; +osmo_fr_dlc_alloc; +osmo_fr_rx; +osmo_fr_tx_dlc; +osmo_fr_role_names; + gprs_ns_signal_ns_names; gprs_ns_pdu_strings; gprs_ns_cause_str; @@ -70,7 +140,66 @@ gprs_ns_ll_copy; gprs_ns_ll_clear; gprs_ns_msgb_alloc; -gprs_nsvc_create; +gprs_ns2_aff_cause_prim_strs; +gprs_ns2_bind_by_name; +gprs_ns2_cause_strs; +gprs_ns2_create_nse; +gprs_ns2_create_nse2; +gprs_ns2_find_vc_by_sockaddr; +gprs_ns2_free; +gprs_ns2_free_bind; +gprs_ns2_free_binds; +gprs_ns2_free_nse; +gprs_ns2_free_nses; +gprs_ns2_free_nsvc; +gprs_ns2_free_nsvcs; +gprs_ns2_frgre_bind; +gprs_ns2_fr_bind; +gprs_ns2_fr_bind_netif; +gprs_ns2_fr_bind_by_netif; +gprs_ns2_fr_connect; +gprs_ns2_fr_nsvc_by_dlci; +gprs_ns2_fr_nsvc_dlci; +gprs_ns2_is_fr_bind; +gprs_ns2_find_vc_by_dlci; +gprs_ns2_instantiate; +gprs_ns2_ip_bind; +gprs_ns2_ip_bind_by_sockaddr; +gprs_ns2_ip_bind_set_dscp; +gprs_ns2_ip_bind_set_priority; +gprs_ns2_ip_bind_set_sns_weight; +gprs_ns2_ip_bind_sockaddr; +gprs_ns2_ip_connect; +gprs_ns2_ip_connect2; +gprs_ns2_ip_connect_inactive; +gprs_ns2_ip_vc_local; +gprs_ns2_ip_vc_remote; +gprs_ns2_ip_vc_equal; +gprs_ns2_is_frgre_bind; +gprs_ns2_is_ip_bind; +gprs_ns2_ll_str; +gprs_ns2_ll_str_buf; +gprs_ns2_ll_str_c; +gprs_ns2_lltype_strs; +gprs_ns2_nse_by_nsei; +gprs_ns2_nse_foreach_nsvc; +gprs_ns2_nse_nsei; +gprs_ns2_nse_sns_remote; +gprs_ns2_nsvc_by_nsvci; +gprs_ns2_nsvc_by_sockaddr; +gprs_ns2_nsvc_state_name; +gprs_ns2_prim_strs; +gprs_ns2_recv_prim; +gprs_ns2_reset_persistent_nsvcs; +gprs_ns2_start_alive_all_nsvcs; +gprs_ns2_sns_add_bind; +gprs_ns2_sns_add_endpoint; +gprs_ns2_sns_del_bind; +gprs_ns2_sns_del_endpoint; +gprs_ns2_vty_init; +gprs_ns2_vty_init_reduced; + +gprs_nsvc_create2; gprs_nsvc_delete; gprs_nsvc_reset; gprs_nsvc_by_nsvci; @@ -84,5 +213,7 @@ bssgp_bvc_ctx_free; btsctx_by_bvci_nsei; btsctx_by_raid_cid; +osmo_pdef_bssgp; + local: *; }; diff --git a/src/gsm/Makefile.am b/src/gsm/Makefile.am index 0aa0de37..e6b687d2 100644 --- a/src/gsm/Makefile.am +++ b/src/gsm/Makefile.am @@ -1,10 +1,10 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=13:0:0 +LIBVERSION=20:0:0 -AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include $(TALLOC_CFLAGS) -AM_CFLAGS = -Wall ${GCC_FVISIBILITY_HIDDEN} +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) if ENABLE_PSEUDOTALLOC AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc @@ -15,6 +15,8 @@ noinst_HEADERS = milenage/aes.h milenage/aes_i.h milenage/aes_wrap.h \ milenage/common.h milenage/crypto.h milenage/includes.h \ milenage/milenage.h +noinst_HEADERS += tuak/KeccakP-1600-3gpp.h tuak/tuak.h + noinst_LTLIBRARIES = libgsmint.la lib_LTLIBRARIES = libosmogsm.la @@ -25,30 +27,41 @@ libgsmint_la_SOURCES = a5.c rxlev_stat.c tlv_parser.c comp128.c comp128v23.c \ gsm48_ie.c gsm0808.c sysinfo.c \ gprs_cipher_core.c gprs_rlc.c gsm0480.c abis_nm.c gsm0502.c \ gsm0411_utils.c gsm0411_smc.c gsm0411_smr.c gsm0414.c \ - lapd_core.c lapdm.c kasumi.c gsm29205.c gsm_04_08_gprs.c \ - auth_core.c auth_comp128v1.c auth_comp128v23.c \ + lapdm.c kasumi.c gsm29205.c gsm_04_08_gprs.c \ + auth_core.c auth_comp128v1.c auth_comp128v23.c auth_xor.c auth_xor_2g.c \ auth_milenage.c milenage/aes-encblock.c gea.c \ milenage/aes-internal.c milenage/aes-internal-enc.c \ milenage/milenage.c gan.c ipa.c gsm0341.c apn.c \ - gsup.c gsup_sms.c gprs_gea.c gsm0503_conv.c oap.c gsm0808_utils.c \ - gsm23003.c mncc.c bts_features.c oap_client.c \ - gsm29118.c gsm48_rest_octets.c cbsp.c gsm48049.c + tuak/KeccakP-1600-3gpp.c tuak/tuak.c auth_tuak.c \ + gprs_gea.c gsm0503_conv.c oap.c gsm0808_utils.c \ + gsm23003.c gsm23236.c mncc.c bts_features.c oap_client.c \ + gsm29118.c gsm48_rest_octets.c cbsp.c gsm48049.c \ + gad.c bsslap.c bssmap_le.c kdf.c iuup.c gsm44021.c gsm44068.c rlp.c +if !EMBEDDED +libgsmint_la_SOURCES += gsup.c gsup_sms.c +endif # !EMBEDDED + libgsmint_la_LDFLAGS = -no-undefined -libgsmint_la_LIBADD = $(top_builddir)/src/libosmocore.la +libgsmint_la_LIBADD = $(top_builddir)/src/core/libosmocore.la $(top_builddir)/src/isdn/libosmoisdn.la libosmogsm_la_SOURCES = libosmogsm_la_LDFLAGS = $(LTLDFLAGS_OSMOGSM) -version-info $(LIBVERSION) -no-undefined libosmogsm_la_LIBADD = libgsmint.la $(TALLOC_LIBS) if ENABLE_GNUTLS -AM_CPPFLAGS += $(LIBGNUTLS_CFLAGS) +AM_CFLAGS += $(LIBGNUTLS_CFLAGS) libosmogsm_la_LIBADD += $(LIBGNUTLS_LIBS) +else +noinst_HEADERS += kdf/sha1.h kdf/sha256.h kdf/common.h kdf/sha1_i.h kdf/sha256_i.h +libgsmint_la_SOURCES += kdf/sha256.c kdf/sha256-internal.c \ + kdf/sha1.c kdf/sha1-internal.c endif EXTRA_DIST = libosmogsm.map +EXTRA_libosmogsm_la_DEPENDENCIES = libosmogsm.map # Convolutional codes generation gsm0503_conv.c: $(top_srcdir)/utils/conv_gen.py $(top_srcdir)/utils/conv_codes_gsm.py - $(AM_V_GEN)python $(top_srcdir)/utils/conv_gen.py gen_codes gsm + $(AM_V_GEN)python3 $(top_srcdir)/utils/conv_gen.py gen_codes gsm CLEANFILES = gsm0503_conv.c diff --git a/src/gsm/a5.c b/src/gsm/a5.c index 223d3ad8..d223cb76 100644 --- a/src/gsm/a5.c +++ b/src/gsm/a5.c @@ -14,10 +14,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ /*! \addtogroup a5 diff --git a/src/gsm/abis_nm.c b/src/gsm/abis_nm.c index 3fb8f0f5..c4060e01 100644 --- a/src/gsm/abis_nm.c +++ b/src/gsm/abis_nm.c @@ -589,6 +589,7 @@ const struct tlv_definition abis_nm_att_tlvdef = { /*! org.osmocom GSM A-bis OML TLV parser definition */ const struct tlv_definition abis_nm_osmo_att_tlvdef = { .def = { + [NM_ATT_OSMO_NS_LINK_CFG] = { TLV_TYPE_TL16V }, [NM_ATT_OSMO_REDUCEPOWER] = { TLV_TYPE_TV }, }, }; @@ -609,6 +610,10 @@ const struct value_string abis_nm_obj_class_names[] = { { NM_OC_RADIO_CARRIER, "RADIO-CARRIER" }, { NM_OC_BASEB_TRANSC, "BASEBAND-TRANSCEIVER" }, { NM_OC_CHANNEL, "CHANNEL" }, + { NM_OC_IPAC_E1_TRUNK, "IPAC-E1-TRUNK" }, + { NM_OC_IPAC_E1_PORT, "IPAC-E1-PORT" }, + { NM_OC_IPAC_E1_CHAN, "IPAC-E1-CHAN" }, + { NM_OC_IPAC_CLK_MODULE,"IPAC-CLK-MODULE" }, { NM_OC_BS11_ADJC, "ADJC" }, { NM_OC_BS11_HANDOVER, "HANDOVER" }, { NM_OC_BS11_PWR_CTRL, "POWER-CONTROL" }, @@ -698,10 +703,104 @@ static const enum abis_nm_chan_comb chcomb4pchan[] = { [GSM_PCHAN_UNKNOWN] = 0xff, [GSM_PCHAN_CCCH_SDCCH4_CBCH] = NM_CHANC_BCCH_CBCH, [GSM_PCHAN_SDCCH8_SACCH8C_CBCH] = NM_CHANC_SDCCH_CBCH, - [GSM_PCHAN_TCH_F_TCH_H_PDCH] = NM_CHANC_OSMO_TCHFull_TCHHalf_PDCH, + [GSM_PCHAN_OSMO_DYN] = NM_CHANC_OSMO_DYN, /* FIXME: bounds check */ }; +const struct value_string abis_nm_ipacc_freq_band_desc[] = { + { NM_IPAC_F_FREQ_BAND_PGSM, "P-GSM" }, + { NM_IPAC_F_FREQ_BAND_EGSM, "E-GSM" }, + { NM_IPAC_F_FREQ_BAND_RGSM, "R-GSM" }, + { NM_IPAC_F_FREQ_BAND_DCS, "DCS-1800" }, + { NM_IPAC_F_FREQ_BAND_PCS, "PCS-1900" }, + { NM_IPAC_F_FREQ_BAND_850, "850" }, + { NM_IPAC_F_FREQ_BAND_480, "480" }, + { NM_IPAC_F_FREQ_BAND_450, "450" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_ciph_algo_desc[] = { + { NM_IPAC_F_CIPH_ALGO_A51, "A5/1" }, + { NM_IPAC_F_CIPH_ALGO_A52, "A5/2" }, + { NM_IPAC_F_CIPH_ALGO_A53, "A5/3" }, + { NM_IPAC_F_CIPH_ALGO_A54, "A5/4" }, + { NM_IPAC_F_CIPH_ALGO_A55, "A5/5" }, + { NM_IPAC_F_CIPH_ALGO_A56, "A5/6" }, + { NM_IPAC_F_CIPH_ALGO_A57, "A5/7" }, + { NM_IPAC_F_CIPH_ALGO_A58, "A5/8" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_chant_desc[] = { + { NM_IPAC_F_CHANT_TCHF, "TCH/F" }, + { NM_IPAC_F_CHANT_TCHH, "TCH/H" }, + { NM_IPAC_F_CHANT_SDCCH8, "SDCCH/8" }, + { NM_IPAC_F_CHANT_BCCH, "BCCH" }, + { NM_IPAC_F_CHANT_BCCH_SDCCH4, "BCCH+SDCCH/4" }, + { NM_IPAC_F_CHANT_BCH, "BCH" }, + { NM_IPAC_F_CHANT_BCCH_SDCCH4_CBCH, "BCCH+SDCCH/4+CBCH" }, + { NM_IPAC_F_CHANT_SDCCH8_CBCH, "SDCCH/8+CBCH" }, + { NM_IPAC_F_CHANT_PDCHF, "PDCH/F" }, + { NM_IPAC_F_CHANT_TCHF_PDCHF, "TCH/F or PDCH/F" }, + { NM_IPAC_F_CHANT_TCHH_PDCHH, "TCH/H+PDCH/H" }, + { NM_IPAC_F_CHANT_TCHF_TCHH, "TCH/F or TCH/H" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_chanm_desc[] = { + { NM_IPAC_F_CHANM_SPEECH_FS, "FR1/FS" }, + { NM_IPAC_F_CHANM_SPEECH_EFS, "FR2/EFS" }, + { NM_IPAC_F_CHANM_SPEECH_AFS, "FR3/AFS" }, + { NM_IPAC_F_CHANM_SPEECH_HS, "HR1/HS" }, + { NM_IPAC_F_CHANM_SPEECH_AHS, "HR3/AHS" }, + { NM_IPAC_F_CHANM_CSD_NT_4k8, "CSD NT 4.8 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_NT_9k6, "CSD NT 9.6 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_NT_14k4, "CSD NT 14.4 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_1200_75, "CSD T 1200/75 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_600, "CSD T 600 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_1k2, "CSD T 1k2 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_2k4, "CSD T 2.4 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_4k8, "CSD T 4.8 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_9k6, "CSD T 9.6 kbit/s" }, + { NM_IPAC_F_CHANM_CSD_T_14k4, "CSD T 14.4 kbit/s" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_gprs_coding_desc[] = { + { NM_IPAC_F_GPRS_CODING_CS1, "CS1" }, + { NM_IPAC_F_GPRS_CODING_CS2, "CS2" }, + { NM_IPAC_F_GPRS_CODING_CS3, "CS3" }, + { NM_IPAC_F_GPRS_CODING_CS4, "CS4" }, + { NM_IPAC_F_GPRS_CODING_MCS1, "MCS1" }, + { NM_IPAC_F_GPRS_CODING_MCS2, "MCS2" }, + { NM_IPAC_F_GPRS_CODING_MCS3, "MCS3" }, + { NM_IPAC_F_GPRS_CODING_MCS4, "MCS4" }, + { NM_IPAC_F_GPRS_CODING_MCS5, "MCS5" }, + { NM_IPAC_F_GPRS_CODING_MCS6, "MCS6" }, + { NM_IPAC_F_GPRS_CODING_MCS7, "MCS7" }, + { NM_IPAC_F_GPRS_CODING_MCS8, "MCS8" }, + { NM_IPAC_F_GPRS_CODING_MCS9, "MCS9" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_rtp_feat_desc[] = { + { NM_IPAC_F_RTP_FEAT_COMPR_CONTROL, "Compression Control" }, + { NM_IPAC_F_RTP_FEAT_IR_8k, "Intermediate Rate 8 kbit/s" }, + { NM_IPAC_F_RTP_FEAT_IR_16k, "Intermediate Rate 16 kbit/s" }, + { NM_IPAC_F_RTP_FEAT_IR_32k, "Intermediate Rate 32 kbit/s" }, + { NM_IPAC_F_RTP_FEAT_IR_64k, "Intermediate Rate 64 kbit/s" }, + { NM_IPAC_F_RTP_FEAT_MULTIPLEX_RTP, "RTP Multiplex" }, + { NM_IPAC_F_RTP_FEAT_MULTIPLEX_SRTP, "SRTP Multiplex" }, + { 0, NULL } +}; + +const struct value_string abis_nm_ipacc_rsl_feat_desc[] = { + { NM_IPAC_F_RSL_FEAT_PHYSICAL_CONTEXT, "Physical Context" }, + { NM_IPAC_F_RSL_FEAT_DYN_PDCH_ACT, "Dynamic PDCH Activation" }, + { NM_IPAC_F_RSL_FEAT_RTP_PT2, "RTP Payload Type 2" }, + { 0, NULL } +}; + /*! Pack 3GPP TS 12.21 § 8.8.2 Failure Event Report into msgb */ struct msgb *abis_nm_fail_evt_rep(enum abis_nm_event_type t, enum abis_nm_severity s, diff --git a/src/gsm/apn.c b/src/gsm/apn.c index a7074eab..4800702c 100644 --- a/src/gsm/apn.c +++ b/src/gsm/apn.c @@ -24,8 +24,8 @@ #include <stdint.h> #include <stdlib.h> #include <string.h> -#include <talloc.h> +#include <osmocom/core/talloc.h> #include <osmocom/gsm/apn.h> #define APN_OI_GPRS_FMT "mnc%03u.mcc%03u.gprs" diff --git a/src/gsm/auth_comp128v1.c b/src/gsm/auth_comp128v1.c index 493ebfd0..ded3f8a6 100644 --- a/src/gsm/auth_comp128v1.c +++ b/src/gsm/auth_comp128v1.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <osmocom/crypt/auth.h> @@ -31,9 +27,10 @@ */ static int c128v1_gen_vec(struct osmo_auth_vector *vec, - struct osmo_sub_auth_data *aud, + struct osmo_sub_auth_data2 *aud, const uint8_t *_rand) { + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_COMP128v1); comp128v1(aud->u.gsm.ki, _rand, vec->sres, vec->kc); vec->auth_types = OSMO_AUTH_TYPE_GSM; diff --git a/src/gsm/auth_comp128v23.c b/src/gsm/auth_comp128v23.c index 279d2b74..f942bc02 100644 --- a/src/gsm/auth_comp128v23.c +++ b/src/gsm/auth_comp128v23.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <osmocom/crypt/auth.h> @@ -33,9 +29,10 @@ */ static int c128v2_gen_vec(struct osmo_auth_vector *vec, - struct osmo_sub_auth_data *aud, + struct osmo_sub_auth_data2 *aud, const uint8_t *_rand) { + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_COMP128v2); comp128v2(aud->u.gsm.ki, _rand, vec->sres, vec->kc); vec->auth_types = OSMO_AUTH_TYPE_GSM; @@ -50,9 +47,10 @@ static struct osmo_auth_impl c128v2_alg = { }; static int c128v3_gen_vec(struct osmo_auth_vector *vec, - struct osmo_sub_auth_data *aud, + struct osmo_sub_auth_data2 *aud, const uint8_t *_rand) { + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_COMP128v3); comp128v3(aud->u.gsm.ki, _rand, vec->sres, vec->kc); vec->auth_types = OSMO_AUTH_TYPE_GSM; diff --git a/src/gsm/auth_core.c b/src/gsm/auth_core.c index 9e750a01..6972cb77 100644 --- a/src/gsm/auth_core.c +++ b/src/gsm/auth_core.c @@ -1,4 +1,4 @@ -/* (C) 2010-2012 by Harald Welte <laforge@gnumonks.org> +/* (C) 2010-2023 by Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include "config.h" @@ -40,6 +36,35 @@ static LLIST_HEAD(osmo_auths); +/* generate auth_data2 from auth_data (for legacy API/ABI compatibility */ +static int auth_data2auth_data2(struct osmo_sub_auth_data2 *out, const struct osmo_sub_auth_data *in) +{ + out->type = in->type; + out->algo = in->algo; + switch (in->type) { + case OSMO_AUTH_TYPE_NONE: + return 0; + case OSMO_AUTH_TYPE_GSM: + memcpy(out->u.gsm.ki, in->u.gsm.ki, sizeof(out->u.gsm.ki)); + break; + case OSMO_AUTH_TYPE_UMTS: + memcpy(out->u.umts.opc, in->u.umts.opc, sizeof(in->u.umts.opc)); + out->u.umts.opc_len = sizeof(in->u.umts.opc); + memcpy(out->u.umts.k, in->u.umts.k, sizeof(in->u.umts.k)); + out->u.umts.k_len = sizeof(in->u.umts.k); + memcpy(out->u.umts.amf, in->u.umts.amf, sizeof(out->u.umts.amf)); + out->u.umts.sqn = in->u.umts.sqn; + out->u.umts.opc_is_op = in->u.umts.opc_is_op; + out->u.umts.ind_bitlen = in->u.umts.ind_bitlen; + out->u.umts.ind = in->u.umts.ind; + out->u.umts.sqn_ms = in->u.umts.sqn_ms; + break; + default: + return -EINVAL; + } + return 0; +} + static struct osmo_auth_impl *selected_auths[_OSMO_AUTH_ALG_NUM]; /*! Register an authentication algorithm implementation with the core @@ -142,6 +167,42 @@ int osmo_auth_3g_from_2g(struct osmo_auth_vector *vec) } /*! Generate authentication vector + * \param[out] vec Generated authentication vector. See below! + * \param[in] aud Subscriber-specific key material + * \param[in] _rand Random challenge to be used + * \returns 0 on success, negative error on failure + * + * This function performs the core cryptographic function of the AUC, + * computing authentication triples/quintuples based on the permanent + * subscriber data and a random value. The result is what is forwarded + * by the AUC via HLR and VLR to the MSC which will then be able to + * invoke authentication with the MS. + * + * Contrary to the older osmo_auth_gen_vec(), the caller must specify + * the desired RES length in the vec->res_len field prior to calling + * this function. The requested length must match the capabilities of + * the chosen algorithm (e.g. 4/8 for MILENAGE). + */ +int osmo_auth_gen_vec2(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *_rand) +{ + struct osmo_auth_impl *impl = selected_auths[aud->algo]; + int rc; + + if (!impl) + return -ENOENT; + + rc = impl->gen_vec(vec, aud, _rand); + if (rc < 0) + return rc; + + memcpy(vec->rand, _rand, sizeof(vec->rand)); + + return 0; +} + +/*! Generate authentication vector * \param[out] vec Generated authentication vector * \param[in] aud Subscriber-specific key material * \param[in] _rand Random challenge to be used @@ -157,13 +218,58 @@ int osmo_auth_gen_vec(struct osmo_auth_vector *vec, struct osmo_sub_auth_data *aud, const uint8_t *_rand) { + struct osmo_sub_auth_data2 aud2; + int rc; + + if (aud->type == OSMO_AUTH_TYPE_UMTS) { + /* old API callers are not expected to initialize this struct field, + * and always expect an 8-byte RES value */ + vec->res_len = 8; + } + + rc = auth_data2auth_data2(&aud2, aud); + if (rc < 0) + return rc; + + rc = osmo_auth_gen_vec2(vec, &aud2, _rand); + if (aud->type == OSMO_AUTH_TYPE_UMTS) + aud->u.umts.sqn = aud2.u.umts.sqn; + + return rc; +} + +/*! Generate authentication vector and re-sync sequence + * \param[out] vec Generated authentication vector. See below! + * \param[in] aud Subscriber-specific key material + * \param[in] auts AUTS value sent by the SIM/MS + * \param[in] rand_auts RAND value sent by the SIM/MS + * \param[in] _rand Random challenge to be used to generate vector + * \returns 0 on success, negative error on failure + * + * This function performs a special variant of the core cryptographic + * function of the AUC: computing authentication triples/quintuples + * based on the permanent subscriber data, a random value as well as the + * AUTS and RAND values returned by the SIM/MS. This special variant is + * needed if the sequence numbers between MS and AUC have for some + * reason become different. + * + * Contrary to the older osmo_auth_gen_vec_auts(), the caller must specify + * the desired RES length in the vec->res_len field prior to calling + * this function. The requested length must match the capabilities of + * the chosen algorithm (e.g. 4/8 for MILENAGE). + */ +int osmo_auth_gen_vec_auts2(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *auts, const uint8_t *rand_auts, + const uint8_t *_rand) +{ struct osmo_auth_impl *impl = selected_auths[aud->algo]; int rc; - if (!impl) + if (!impl || !impl->gen_vec_auts) return -ENOENT; - rc = impl->gen_vec(vec, aud, _rand); + rc = impl->gen_vec_auts(vec, aud, auts, rand_auts, _rand); if (rc < 0) return rc; @@ -192,19 +298,26 @@ int osmo_auth_gen_vec_auts(struct osmo_auth_vector *vec, const uint8_t *auts, const uint8_t *rand_auts, const uint8_t *_rand) { - struct osmo_auth_impl *impl = selected_auths[aud->algo]; + struct osmo_sub_auth_data2 aud2; int rc; - if (!impl || !impl->gen_vec_auts) - return -ENOENT; + if (aud->type == OSMO_AUTH_TYPE_UMTS) { + /* old API callers are not expected to initialize this struct field, + * and always expect an 8-byte RES value */ + vec->res_len = 8; + } - rc = impl->gen_vec_auts(vec, aud, auts, rand_auts, _rand); + rc = auth_data2auth_data2(&aud2, aud); if (rc < 0) return rc; - memcpy(vec->rand, _rand, sizeof(vec->rand)); + rc = osmo_auth_gen_vec_auts2(vec, &aud2, auts, rand_auts, _rand); + if (aud->type == OSMO_AUTH_TYPE_UMTS) { + aud->u.umts.sqn = aud2.u.umts.sqn; + aud->u.umts.sqn_ms = aud2.u.umts.sqn_ms; + } - return 0; + return rc; } static const struct value_string auth_alg_vals[] = { @@ -212,8 +325,10 @@ static const struct value_string auth_alg_vals[] = { { OSMO_AUTH_ALG_COMP128v1, "COMP128v1" }, { OSMO_AUTH_ALG_COMP128v2, "COMP128v2" }, { OSMO_AUTH_ALG_COMP128v3, "COMP128v3" }, - { OSMO_AUTH_ALG_XOR, "XOR" }, + { OSMO_AUTH_ALG_XOR_3G, "XOR-3G" }, { OSMO_AUTH_ALG_MILENAGE, "MILENAGE" }, + { OSMO_AUTH_ALG_XOR_2G, "XOR-2G" }, + { OSMO_AUTH_ALG_TUAK, "TUAK" }, { 0, NULL } }; @@ -249,4 +364,33 @@ void osmo_auth_c3(uint8_t kc[], const uint8_t ck[], const uint8_t ik[]) kc[i] = ck[i] ^ ck[i + 8] ^ ik[i] ^ ik[i + 8]; } +/*! Derive GSM SRES from UMTS [X]RES (auth function c2 from 3GPP TS 33.103 Section 6.8.1.2 + * \param[out] sres GSM SRES value, 4 byte target buffer + * \param[in] res UMTS XRES, 4..16 bytes input buffer + * \param[in] res_len length of res parameter (in bytes) + * \param[in] sres_deriv_func SRES derivation function (1 or 2, see 3GPP TS 55.205 Section 4 + */ +void osmo_auth_c2(uint8_t sres[4], const uint8_t *res, size_t res_len, uint8_t sres_deriv_func) +{ + uint8_t xres[16]; + + OSMO_ASSERT(sres_deriv_func == 1 || sres_deriv_func == 2); + OSMO_ASSERT(res_len <= sizeof(xres)); + + memcpy(xres, res, res_len); + + /* zero-pad the end, if XRES is < 16 bytes */ + if (res_len < sizeof(xres)) + memset(xres+res_len, 0, sizeof(xres)-res_len); + + if (sres_deriv_func == 1) { + /* SRES derivation function #1 */ + for (unsigned int i = 0; i < 4; i++) + sres[i] = xres[i] ^ xres[4+i] ^ xres[8+i] ^ xres[12+i]; + } else { + /* SRES derivation function #2 */ + memcpy(sres, xres, 4); + } +} + /*! @} */ diff --git a/src/gsm/auth_milenage.c b/src/gsm/auth_milenage.c index 95891000..2bd05a6d 100644 --- a/src/gsm/auth_milenage.c +++ b/src/gsm/auth_milenage.c @@ -17,12 +17,9 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ +#include <errno.h> #include <osmocom/crypt/auth.h> #include <osmocom/core/bits.h> #include "milenage/common.h" @@ -32,7 +29,7 @@ * @{ */ -static const uint8_t *gen_opc_if_needed(const struct osmo_sub_auth_data *aud, uint8_t *gen_opc) +static const uint8_t *gen_opc_if_needed(const struct osmo_sub_auth_data2 *aud, uint8_t *gen_opc) { int rc; @@ -47,7 +44,7 @@ static const uint8_t *gen_opc_if_needed(const struct osmo_sub_auth_data *aud, ui } static int milenage_gen_vec(struct osmo_auth_vector *vec, - struct osmo_sub_auth_data *aud, + struct osmo_sub_auth_data2 *aud, const uint8_t *_rand) { size_t res_len = sizeof(vec->res); @@ -57,7 +54,15 @@ static int milenage_gen_vec(struct osmo_auth_vector *vec, uint8_t sqn[6]; uint64_t ind_mask; uint64_t seq_1; - int rc; + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_MILENAGE); + + if (aud->u.umts.k_len != 16) + return -EINVAL; + if (aud->u.umts.opc_len != 16) + return -EINVAL; + if (vec->res_len != 4 && vec->res_len != 8) + return -EINVAL; opc = gen_opc_if_needed(aud, gen_opc); if (!opc) @@ -131,10 +136,9 @@ static int milenage_gen_vec(struct osmo_auth_vector *vec, milenage_generate(opc, aud->u.umts.amf, aud->u.umts.k, sqn, _rand, vec->autn, vec->ik, vec->ck, vec->res, &res_len); - vec->res_len = res_len; - rc = gsm_milenage(opc, aud->u.umts.k, _rand, vec->sres, vec->kc); - if (rc < 0) - return rc; + + osmo_auth_c3(vec->kc, vec->ck, vec->ik); + osmo_auth_c2(vec->sres, vec->res, vec->res_len, 1); vec->auth_types = OSMO_AUTH_TYPE_UMTS | OSMO_AUTH_TYPE_GSM; @@ -145,7 +149,7 @@ static int milenage_gen_vec(struct osmo_auth_vector *vec, } static int milenage_gen_vec_auts(struct osmo_auth_vector *vec, - struct osmo_sub_auth_data *aud, + struct osmo_sub_auth_data2 *aud, const uint8_t *auts, const uint8_t *rand_auts, const uint8_t *_rand) { @@ -154,6 +158,13 @@ static int milenage_gen_vec_auts(struct osmo_auth_vector *vec, const uint8_t *opc; int rc; + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_MILENAGE); + + if (aud->u.umts.k_len != 16) + return -EINVAL; + if (aud->u.umts.opc_len != 16) + return -EINVAL; + opc = gen_opc_if_needed(aud, gen_opc); rc = milenage_auts(opc, aud->u.umts.k, rand_auts, auts, sqn_out); diff --git a/src/gsm/auth_tuak.c b/src/gsm/auth_tuak.c new file mode 100644 index 00000000..05fbf691 --- /dev/null +++ b/src/gsm/auth_tuak.c @@ -0,0 +1,207 @@ +/*! \file auth_tuak.c + * GSM/GPRS/3G authentication core infrastructure */ +/* + * (C) 2023 by Harald Welte <laforge@osmocom.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +/* NOTE: TUAK offers a lot of size variability in terms of size of length of MAC_A, MAC_S, + * but this is not used within 3GPP. The different sizes of Kc and RES are handled via + * osmo_sub_auth_data2. */ + +#include <errno.h> +#include <osmocom/crypt/auth.h> +#include <osmocom/core/bits.h> +#include "tuak/tuak.h" + +/*! \addtogroup auth + * @{ + */ + +static const uint8_t *gen_opc_if_needed(const struct osmo_sub_auth_data2 *aud, uint8_t *gen_opc) +{ + int rc; + + /* Check if we only know OP and compute OPC if required */ + if (aud->type == OSMO_AUTH_TYPE_UMTS && aud->u.umts.opc_is_op) { + rc = tuak_opc_gen(gen_opc, aud->u.umts.k, aud->u.umts.k_len, aud->u.umts.opc); + if (rc < 0) + return NULL; + return gen_opc; + } + + return aud->u.umts.opc; +} + +static int tuak_gen_vec(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *_rand) +{ + size_t res_len = vec->res_len; + uint64_t next_sqn; + uint8_t gen_opc[32]; + const uint8_t *opc; + uint8_t sqn[6]; + uint64_t ind_mask; + uint64_t seq_1; + + switch (vec->res_len) { + case 4: + case 8: + case 16: + break; + default: + return -EINVAL; + } + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_TUAK); + + if (aud->u.umts.k_len != 16 && aud->u.umts.k_len != 32) + return -EINVAL; + if (aud->u.umts.opc_len != 32) + return -EINVAL; + + opc = gen_opc_if_needed(aud, gen_opc); + if (!opc) + return -1; + + /* Determine next SQN, according to 3GPP TS 33.102: + * SQN consists of SEQ and a lower significant part of IND bits: + * + * |----------SEQ------------| + * |------------------------SQN-----------| + * |-----IND----| + * + * The IND part is used as "slots": e.g. a given HLR client will always + * get the same IND part, called ind here, with incrementing SEQ. In + * the USIM, each IND slot enforces that its SEQ are used in ascending + * order -- as long as that constraint is satisfied, the SQN may jump + * forwards and backwards. For example, for ind_bitlen == 5, asking the + * USIM for SQN = 32, 64, 33 is allowed, because 32 and 64 are + * SEQ || (ind == 0), and though 33 is below 64, it is ind == 1 and + * allowed. Not allowed would be 32, 96, 64, because 64 would go + * backwards after 96, both being ind == 0. + * + * From the last used SQN, we want to increment SEQ + 1, and then pick + * the matching IND part. + * + * IND size is suggested in TS 33.102 as 5 bits. SQN is 48 bits long. + * If ind_bitlen is passed too large here, the algorithms will break + * down. But at which point should we return an error? A sane limit + * seems to be ind_bitlen == 10, but to protect against failure, + * limiting ind_bitlen to 28 is enough, 28 being the number of bits + * suggested for the delta in 33.102, which is discussed to still + * require 2^15 > 32000 authentications to wrap the SQN back to the + * start. + * + * Note that if a caller with ind == 1 generates N vectors, the SQN + * stored after this will reflect SEQ + N. If then another caller with + * ind == 2 generates another N vectors, this will then use SEQ + N + * onwards and end up with SEQ + N + N. In other words, most of each + * SEQ's IND slots will remain unused. When looking at SQN being 48 + * bits wide, after dropping ind_bitlen (say 5) from it, we will still + * have a sequence range of 2^43 = 8.8e12, eight trillion sequences, + * which is large enough to not bother further. With the maximum + * ind_bitlen of 28 enforced below, we still get more than 1 million + * sequences, which is also sufficiently large. + * + * An ind_bitlen of zero may be passed from legacy callers that are not + * aware of the IND extension. For these, below algorithm works out as + * before, simply incrementing SQN by 1. + * + * This is also a mechanism for tools like the osmo-auc-gen to directly + * request a given SQN to be used. With ind_bitlen == 0 the caller can + * be sure that this code will increment SQN by exactly one before + * generating a tuple, thus a caller would simply pass + * { .ind_bitlen = 0, .ind = 0, .sqn = (desired_sqn - 1) } + */ + + if (aud->u.umts.ind_bitlen > OSMO_MILENAGE_IND_BITLEN_MAX) + return -2; + + seq_1 = 1LL << aud->u.umts.ind_bitlen; + ind_mask = ~(seq_1 - 1); + + /* the ind index must not affect the SEQ part */ + if (aud->u.umts.ind >= seq_1) + return -3; + + /* keep the incremented SQN local until gsm_milenage() succeeded. */ + next_sqn = ((aud->u.umts.sqn + seq_1) & ind_mask) + aud->u.umts.ind; + + osmo_store64be_ext(next_sqn, sqn, 6); + + tuak_generate(opc, aud->u.umts.amf, aud->u.umts.k, aud->u.umts.k_len, + sqn, _rand, vec->autn, vec->ik, vec->ck, vec->res, &res_len); + + /* generate the GSM Kc + SRES values using C2 + C3 functions */ + osmo_auth_c3(vec->kc, vec->ck, vec->ik); + osmo_auth_c2(vec->sres, vec->res, vec->res_len, 1); + + vec->auth_types = OSMO_AUTH_TYPE_UMTS | OSMO_AUTH_TYPE_GSM; + + /* for storage in the caller's AUC database */ + aud->u.umts.sqn = next_sqn; + + return 0; +} + +static int tuak_gen_vec_auts(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *auts, const uint8_t *rand_auts, + const uint8_t *_rand) +{ + uint8_t sqn_out[6]; + uint8_t gen_opc[32]; + const uint8_t *opc; + int rc; + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_TUAK); + + if (aud->u.umts.k_len != 16 && aud->u.umts.k_len != 32) + return -EINVAL; + if (aud->u.umts.opc_len != 32) + return -EINVAL; + + opc = gen_opc_if_needed(aud, gen_opc); + + rc = tuak_auts(opc, aud->u.umts.k, sizeof(aud->u.umts.k), rand_auts, auts, sqn_out); + if (rc < 0) + return rc; + + aud->u.umts.sqn_ms = osmo_load64be_ext(sqn_out, 6) >> 16; + /* Update our "largest used SQN" from the USIM -- milenage_gen_vec() + * below will increment SQN. */ + aud->u.umts.sqn = aud->u.umts.sqn_ms; + + return tuak_gen_vec(vec, aud, _rand); +} + +static struct osmo_auth_impl tuak_alg = { + .algo = OSMO_AUTH_ALG_TUAK, + .name = "TUAK (libosmogsm built-in)", + .priority = 1000, + .gen_vec = &tuak_gen_vec, + .gen_vec_auts = &tuak_gen_vec_auts, +}; + +static __attribute__((constructor)) void on_dso_load_tuak(void) +{ + osmo_auth_register(&tuak_alg); +} + +/*! @} */ diff --git a/src/gsm/auth_xor.c b/src/gsm/auth_xor.c new file mode 100644 index 00000000..a506a03d --- /dev/null +++ b/src/gsm/auth_xor.c @@ -0,0 +1,191 @@ +/*! \file auth_xor.c + * GSM/GPRS/3G authentication core infrastructure */ +/* + * (C) 2018 by Harald Welte <laforge@gnumonks.org> + * (C) 2017 by sysmocom s.f.m.c. GmbH + * + * All Rights Reserved + * + * Author: Daniel Willmann <dwillmann@sysmocom.de> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <string.h> +#include <stdint.h> +#include <errno.h> + +#include <osmocom/core/bit64gen.h> +#include <osmocom/crypt/auth.h> + +/*! \addtogroup auth + * @{ + */ + +static void xor(uint8_t *out, const uint8_t *a, const uint8_t *b, size_t len) +{ + size_t i; + + for (i = 0; i < len; i++) + out[i] = a[i] ^ b[i]; +} + +/* 3GPP TS 34.108, section 8.1.2.1 */ +static int xor_gen_vec(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *_rand) +{ + uint8_t xdout[16], cdout[8]; + uint8_t ak[6], xmac[8]; + int i; + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_XOR_3G); + + /* Step 1: xdout = (ki or k) ^ rand */ + if (aud->type == OSMO_AUTH_TYPE_GSM) + xor(xdout, aud->u.gsm.ki, _rand, sizeof(xdout)); + else if (aud->type == OSMO_AUTH_TYPE_UMTS) { + if (aud->u.umts.k_len != 16) + return -EINVAL; + xor(xdout, aud->u.umts.k, _rand, sizeof(xdout)); + } else + return -ENOTSUP; + + /** + * Step 2: res = xdout + * + * Suggested length for res is 128 bits, i.e. 16 bytes, + * but also can be in range: 30 < n < 128 bits. + */ + memcpy(vec->res, xdout, sizeof(xdout)); + vec->res_len = sizeof(xdout); + + /* ck = xdout[1-15,0] */ + memcpy(vec->ck, xdout + 1, sizeof(xdout) - 1); + vec->ck[15] = xdout[0]; + + /* ik = xdout[2-15,0-1] */ + memcpy(vec->ik, xdout + 2, sizeof(xdout) - 2); + memcpy(vec->ik + sizeof(xdout) - 2, xdout, 2); + + /* ak = xdout[3-8] */ + memcpy(ak, xdout + 3, sizeof(ak)); + + /** + * 3GPP TS 33.102, clause 6.8.1.2, b + * sres = c2(res) = res[0-3] ^ res[4-7] ^ res[8-11] ^ res[12-15] + */ + for (i = 0; i < 4; i++) { + vec->sres[i] = vec->res[i] ^ vec->res[i + 4]; + vec->sres[i] ^= vec->res[i + 8] ^ vec->res[i + 12]; + } + + /** + * 3GPP TS 33.102, clause 6.8.1.2, c + * kc = c3(ck, ik) = ck[0-7] ^ ck[8-15] ^ ik[0-7] ^ ik[8-15] + * FIXME: do we really have CK/IK for GSM? + */ + osmo_auth_c3(vec->kc, vec->ck, vec->ik); + + /* The further part is UMTS specific */ + if (aud->type != OSMO_AUTH_TYPE_UMTS) { + vec->auth_types = OSMO_AUTH_TYPE_GSM; + return 0; + } + + /** + * Step 3: cdout = sqn[0-5] || amf[0-1] + * NOTE (for USIM): sqn[0-5] = autn[0-5] ^ ak[0-5] + */ + osmo_store64be_ext(aud->u.umts.sqn, cdout, 6); + memcpy(cdout + 6, aud->u.umts.amf, 2); + + /* Step 4: xmac = xdout[0-8] ^ cdout[0-8] */ + xor(xmac, xdout, cdout, sizeof(xmac)); + + /** + * Step 5: autn = sqn ^ ak || amf || mac + * NOTE: cdout still contains SQN from step 3 + */ + xor(vec->autn, cdout, ak, sizeof(ak)); + memcpy(vec->autn + 6, aud->u.umts.amf, 2); + memcpy(vec->autn + 8, xmac, sizeof(xmac)); + + vec->auth_types = OSMO_AUTH_TYPE_UMTS | OSMO_AUTH_TYPE_GSM; + + return 0; +} + +/* 3GPP TS 34.108, section 8.1.2.2 */ +static int xor_gen_vec_auts(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *auts, + const uint8_t *rand_auts, + const uint8_t *_rand) +{ + uint8_t xdout[16], cdout[8]; + uint8_t ak[6], xmac[8]; + uint8_t sqnms[6]; + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_XOR_3G); + + /* Step 1: xdout = (ki or k) ^ rand */ + if (aud->type == OSMO_AUTH_TYPE_GSM) + xor(xdout, aud->u.gsm.ki, _rand, sizeof(xdout)); + else if (aud->type == OSMO_AUTH_TYPE_UMTS) { + if (aud->u.umts.k_len != 16) + return -EINVAL; + xor(xdout, aud->u.umts.k, _rand, sizeof(xdout)); + } else + return -ENOTSUP; + + /* Step 2: ak = xdout[2-8] */ + memcpy(ak, xdout + 3, 6); + + /* sqnms = auts[0-5] ^ ak[0-5] */ + xor(sqnms, auts, ak, sizeof(ak)); + + /* cdout = sqnms || amf* (dummy) */ + memcpy(cdout, sqnms, 6); + memset(cdout + 6, 0x00, 2); + + /* xmac = xdout[0-7] ^ cdout[0-7] */ + xor(xmac, xdout, cdout, 8); + + /* Compare the last 64 bits of received AUTS with the locally-generated MAC-S */ + if (memcmp(auts + 6, xmac, 8)) + return -1; + + /* Update the "largest used SQN" from the USIM, + * milenage_gen_vec() will increment it. */ + aud->u.umts.sqn_ms = osmo_load64be_ext(sqnms, 6) >> 16; + aud->u.umts.sqn = aud->u.umts.sqn_ms; + + return xor_gen_vec(vec, aud, _rand); +} + +static struct osmo_auth_impl xor_alg = { + .algo = OSMO_AUTH_ALG_XOR_3G, + .name = "XOR-3G (libosmogsm built-in)", + .priority = 1000, + .gen_vec = &xor_gen_vec, + .gen_vec_auts = &xor_gen_vec_auts, +}; + +static __attribute__((constructor)) void on_dso_load_xor(void) +{ + osmo_auth_register(&xor_alg); +} + +/*! @} */ diff --git a/src/gsm/auth_xor_2g.c b/src/gsm/auth_xor_2g.c new file mode 100644 index 00000000..367c79d4 --- /dev/null +++ b/src/gsm/auth_xor_2g.c @@ -0,0 +1,81 @@ +/*! \file auth_xor.c + * GSM XOR-2G algorithm as specified in Annex 4 (A.4.1.2) of 3GPP TS 51.010-1. + * This is implemented by typical GSM MS tester */ + +/* + * (C) 2023 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * Author: Daniel Willmann <dwillmann@sysmocom.de> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <string.h> +#include <stdint.h> +#include <errno.h> + +#include <osmocom/crypt/auth.h> + +/*! \addtogroup auth + * @{ + */ + +static void xor(uint8_t *out, const uint8_t *a, const uint8_t *b, size_t len) +{ + size_t i; + + for (i = 0; i < len; i++) + out[i] = a[i] ^ b[i]; +} + +/* GSM XOR-2G algorithm as specified in Annex 4 (A.4.1.2) of 3GPP TS 51.010-1. */ +static int xor2g_gen_vec(struct osmo_auth_vector *vec, + struct osmo_sub_auth_data2 *aud, + const uint8_t *_rand) +{ + uint8_t res1[16]; + + OSMO_ASSERT(aud->algo == OSMO_AUTH_ALG_XOR_2G); + + if (aud->type != OSMO_AUTH_TYPE_GSM) + return -ENOTSUP; + + /* Step 1: XOR to the challenge RAND, a predefined number Ki, having the same bit length (128 bits) as + * RAND. */ + xor(res1, aud->u.gsm.ki, _rand, sizeof(res1)); + + /* Step 2: The most significant 32 bits of RES1 form SRES. */ + memcpy(vec->sres, res1, 4); + /* The next 64 bits of RES1 form Kc */ + memcpy(vec->kc, res1+4, 8); + + vec->auth_types = OSMO_AUTH_TYPE_GSM; + return 0; +} + +static struct osmo_auth_impl xor2g_alg = { + .algo = OSMO_AUTH_ALG_XOR_2G, + .name = "XOR-2G (libosmogsm built-in)", + .priority = 1000, + .gen_vec = &xor2g_gen_vec, +}; + +static __attribute__((constructor)) void on_dso_load_xor(void) +{ + osmo_auth_register(&xor2g_alg); +} + +/*! @} */ diff --git a/src/gsm/bsslap.c b/src/gsm/bsslap.c new file mode 100644 index 00000000..70ded13f --- /dev/null +++ b/src/gsm/bsslap.c @@ -0,0 +1,325 @@ +/* 3GPP TS 48.071 BSSLAP protocol definitions */ +/* + * (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * All Rights Reserved + * + * Author: Neels Hofmeyr <neels@hofmeyr.de> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <errno.h> + +#include <osmocom/core/msgb.h> +#include <osmocom/gsm/bsslap.h> +#include <osmocom/gsm/tlv.h> + +#include <osmocom/core/logging.h> + +/*! \addtogroup bsslap + * @{ + * \file bsslap.c + * Message encoding and decoding for 3GPP TS 48.071 BSSLAP protocol. + */ + +static const struct tlv_definition osmo_bsslap_tlvdef = { + .def = { + [BSSLAP_IEI_TA] = { TLV_TYPE_TV }, + [BSSLAP_IEI_CELL_ID] = { TLV_TYPE_FIXED, 2 }, + [BSSLAP_IEI_CHAN_DESC] = { TLV_TYPE_FIXED, 3 }, + [BSSLAP_IEI_MEAS_REP] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_CAUSE] = { TLV_TYPE_TV }, + [BSSLAP_IEI_RRLP_FLAG] = { TLV_TYPE_TV }, + [BSSLAP_IEI_RRLP] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_CELL_ID_LIST] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_ENH_MEAS_REP] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_LAC] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_FREQ_LIST] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_MS_POWER] = { TLV_TYPE_TV }, + [BSSLAP_IEI_DELTA_TIMER] = { TLV_TYPE_TV }, + [BSSLAP_IEI_SERVING_CELL_ID] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_ENCR_KEY] = { TLV_TYPE_FIXED, 8 }, + [BSSLAP_IEI_CIPH_MODE_SET] = { TLV_TYPE_TV }, + [BSSLAP_IEI_CHAN_MODE] = { TLV_TYPE_TV, 2 }, + [BSSLAP_IEI_MR_CONFIG] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_POLLING_REPETITION] = { TLV_TYPE_TV }, + [BSSLAP_IEI_PACKET_CHAN_DESC] = { TLV_TYPE_FIXED, 4 }, + [BSSLAP_IEI_TLLI] = { TLV_TYPE_FIXED, 4 }, + [BSSLAP_IEI_TFI] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_TBF_START_TIME] = { TLV_TYPE_FIXED, 2 }, + [BSSLAP_IEI_PWRUP_START_TIME] = { TLV_TYPE_TLV }, + [BSSLAP_IEI_LONG_ENCR_KEY] = { TLV_TYPE_FIXED, 16 }, + [BSSLAP_IEI_CONCUR_POS_PROC_F] = { TLV_TYPE_TV }, + }, +}; + +#define DEC_ERR(RC, MSG_TYPE, IEI, CAUSE, fmt, args...) do { \ + if (err && !*err) { \ + *err = talloc_zero(err_ctx, struct osmo_bsslap_err); \ + **err = (struct osmo_bsslap_err){ \ + .rc = (RC), \ + .msg_type = (MSG_TYPE), \ + .iei = (IEI), \ + .cause = (CAUSE), \ + .logmsg = talloc_asprintf(*err, "Error decoding BSSLAP%s%s%s%s%s: " fmt, \ + (MSG_TYPE) >= 0 ? " " : "", \ + (MSG_TYPE) >= 0 ? osmo_bsslap_msgt_name(MSG_TYPE) : "", \ + (IEI) >= 0 ? ": " : "", \ + (IEI) >= 0 ? osmo_bsslap_iei_name(IEI) : "", \ + (IEI) >= 0 ? " IE" : "", \ +##args), \ + }; \ + } \ + return RC; \ + } while(0) + +static void osmo_bsslap_ie_enc_cell_id(struct msgb *msg, uint16_t cell_id) +{ + msgb_put_u8(msg, BSSLAP_IEI_CELL_ID); + msgb_put_u16(msg, cell_id); +} + +static int osmo_bsslap_ie_dec_cell_id(uint16_t *cell_id, + enum bsslap_msgt msgt, enum bsslap_iei iei, + struct osmo_bsslap_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + if (len != 2) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Expected 2 bytes, got %zu", len); + *cell_id = osmo_load16be(data); + return 0; +} + +static void osmo_bsslap_ie_enc_ta(struct msgb *msg, uint8_t ta) +{ + msgb_put_u8(msg, BSSLAP_IEI_TA); + msgb_put_u8(msg, ta); +} + +static int osmo_bsslap_ie_dec_ta(uint8_t *ta, + enum bsslap_msgt msgt, enum bsslap_iei iei, + struct osmo_bsslap_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + if (len != 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Expected 1 byte, got %zu", len); + *ta = data[0]; + return 0; +} + +static void osmo_bsslap_ie_enc_cause(struct msgb *msg, enum bsslap_cause cause) +{ + msgb_put_u8(msg, BSSLAP_IEI_CAUSE); + msgb_put_u8(msg, cause); +} + +static int osmo_bsslap_ie_dec_cause(enum bsslap_cause *cause, + enum bsslap_msgt msgt, enum bsslap_iei iei, + struct osmo_bsslap_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + if (len != 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Expected 1 byte, got %zu", len); + *cause = data[0]; + return 0; +} + +static void osmo_bsslap_ie_enc_chan_desc(struct msgb *msg, const struct gsm48_chan_desc *chan_desc) +{ + struct gsm48_chan_desc *put_chan_desc; + msgb_put_u8(msg, BSSLAP_IEI_CHAN_DESC); + put_chan_desc = (void*)msgb_put(msg, sizeof(*chan_desc)); + *put_chan_desc = *chan_desc; +} + +static int osmo_bsslap_ie_dec_chan_desc(struct gsm48_chan_desc *chan_desc, + enum bsslap_msgt msgt, enum bsslap_iei iei, + struct osmo_bsslap_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + if (len != sizeof(*chan_desc)) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Expected %zu bytes, got %zu", + sizeof(*chan_desc), len); + *chan_desc = *(struct gsm48_chan_desc*)data; + return 0; +} + +/*! Encode BSSLAP PDU and append to msgb (3GPP TS 48.071). + * \param[out] msg msgb to append to. + * \param[in] pdu PDU data to encode. + * \return number of bytes written, negative on error. + */ +int osmo_bsslap_enc(struct msgb *msg, const struct bsslap_pdu *pdu) +{ + uint8_t *old_tail = msg->tail; + + msgb_put_u8(msg, pdu->msg_type); + + switch (pdu->msg_type) { + case BSSLAP_MSGT_TA_REQUEST: + /* The TA Request message contains only the message type. */ + break; + + case BSSLAP_MSGT_TA_RESPONSE: + osmo_bsslap_ie_enc_cell_id(msg, pdu->ta_response.cell_id); + osmo_bsslap_ie_enc_ta(msg, pdu->ta_response.ta); + break; + + case BSSLAP_MSGT_REJECT: + osmo_bsslap_ie_enc_cause(msg, pdu->reject); + break; + + case BSSLAP_MSGT_RESET: + osmo_bsslap_ie_enc_cell_id(msg, pdu->reset.cell_id); + osmo_bsslap_ie_enc_ta(msg, pdu->reset.ta); + osmo_bsslap_ie_enc_chan_desc(msg, &pdu->reset.chan_desc); + osmo_bsslap_ie_enc_cause(msg, pdu->reset.cause); + break; + + case BSSLAP_MSGT_ABORT: + osmo_bsslap_ie_enc_cause(msg, pdu->abort); + break; + + case BSSLAP_MSGT_TA_LAYER3: + osmo_bsslap_ie_enc_ta(msg, pdu->ta_layer3.ta); + break; + + default: + return -ENOTSUP; + } + return (msg->tail - old_tail); +} + +/*! Decode BSSLAP PDU (3GPP TS 48.071). + * \param[out] pdu Write decoded values here. + * \param[out] err Returned pointer to error info, dynamically allocated; NULL to not return any. + * \param[in] err_ctx Talloc context to allocate err from, if required. + * \param[in] data Pointer to BSSLAP PDU raw data. + * \param[in] len Data length to decode. + * \return 0 on success, negative on error. + */ +int osmo_bsslap_dec(struct bsslap_pdu *pdu, + struct osmo_bsslap_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + const uint8_t *ies_start; + int ies_len; + struct tlv_parsed tp; + + memset(pdu, 0x00, sizeof(*pdu)); + if (err) + *err = NULL; + +#define DEC_IE_MANDATORY(IEI, DEC_FUN, DEC_FUN_ARG) do { \ + const struct tlv_p_entry *e; \ + int rc; \ + if (!(e = TLVP_GET(&tp, IEI))) \ + DEC_ERR(-EINVAL, pdu->msg_type, IEI, LCS_CAUSE_DATA_MISSING_IN_REQ, "missing mandatory IE"); \ + rc = DEC_FUN(DEC_FUN_ARG, pdu->msg_type, IEI, err, err_ctx, e->val, e->len); \ + if (rc) \ + DEC_ERR(rc, pdu->msg_type, IEI, LCS_CAUSE_UNSPECIFIED, "cannot parse IE"); \ + } while (0) + + if (len < 1) + DEC_ERR(-EINVAL, -1, -1, LCS_CAUSE_UNSPECIFIED, "PDU too short: %zu b", len); + + pdu->msg_type = data[0]; + + if (pdu->msg_type == BSSLAP_MSGT_TA_REQUEST) { + /* The TA Request message contains only the message type. */ + return 0; + } + + ies_start = &data[1]; + ies_len = len - 1; + + if (tlv_parse2(&tp, 1, &osmo_bsslap_tlvdef, ies_start, ies_len, 0, 0) <= 0) + DEC_ERR(-EINVAL, pdu->msg_type, -1, LCS_CAUSE_UNSPECIFIED, "failed to parse TLV structure"); + + switch (pdu->msg_type) { + + case BSSLAP_MSGT_TA_RESPONSE: + DEC_IE_MANDATORY(BSSLAP_IEI_CELL_ID, osmo_bsslap_ie_dec_cell_id, &pdu->ta_response.cell_id); + DEC_IE_MANDATORY(BSSLAP_IEI_TA, osmo_bsslap_ie_dec_ta, &pdu->ta_response.ta); + return 0; + + case BSSLAP_MSGT_REJECT: + DEC_IE_MANDATORY(BSSLAP_IEI_CAUSE, osmo_bsslap_ie_dec_cause, &pdu->reject); + return 0; + + case BSSLAP_MSGT_RESET: + DEC_IE_MANDATORY(BSSLAP_IEI_CELL_ID, osmo_bsslap_ie_dec_cell_id, &pdu->reset.cell_id); + DEC_IE_MANDATORY(BSSLAP_IEI_TA, osmo_bsslap_ie_dec_ta, &pdu->reset.ta); + DEC_IE_MANDATORY(BSSLAP_IEI_CHAN_DESC, osmo_bsslap_ie_dec_chan_desc, &pdu->reset.chan_desc); + DEC_IE_MANDATORY(BSSLAP_IEI_CAUSE, osmo_bsslap_ie_dec_cause, &pdu->reset.cause); + return 0; + + case BSSLAP_MSGT_ABORT: + DEC_IE_MANDATORY(BSSLAP_IEI_CAUSE, osmo_bsslap_ie_dec_cause, &pdu->abort); + return 0; + + case BSSLAP_MSGT_TA_LAYER3: + DEC_IE_MANDATORY(BSSLAP_IEI_TA, osmo_bsslap_ie_dec_ta, &pdu->ta_layer3.ta); + return 0; + + default: + DEC_ERR(-EINVAL, pdu->msg_type, -1, LCS_CAUSE_UNSPECIFIED, "Unsupported message type"); + } +} + +const struct value_string osmo_bsslap_msgt_names[] = { + { BSSLAP_MSGT_TA_REQUEST, "TA Request" }, + { BSSLAP_MSGT_TA_RESPONSE, "TA Response" }, + { BSSLAP_MSGT_REJECT, "Reject" }, + { BSSLAP_MSGT_RESET, "Reset" }, + { BSSLAP_MSGT_ABORT, "Abort" }, + { BSSLAP_MSGT_TA_LAYER3, "TA Layer3" }, + { BSSLAP_MSGT_MS_POS_CMD, "MS Position Command" }, + { BSSLAP_MSGT_MS_POS_RESP, "MS Position Response" }, + { BSSLAP_MSGT_UTDOA_REQ, "U-TDOA Request" }, + { BSSLAP_MSGT_UTDOA_RESP, "U-TDOA Response" }, + {} +}; + +const struct value_string osmo_bsslap_iei_names[] = { + { BSSLAP_IEI_TA, "Timing Advance" }, + { BSSLAP_IEI_CELL_ID, "Cell Identity" }, + { BSSLAP_IEI_CHAN_DESC, "Channel Description" }, + { BSSLAP_IEI_MEAS_REP, "Measurement Report" }, + { BSSLAP_IEI_CAUSE, "Cause" }, + { BSSLAP_IEI_RRLP_FLAG, "RRLP Flag" }, + { BSSLAP_IEI_RRLP, "RRLP" }, + { BSSLAP_IEI_CELL_ID_LIST, "Cell Identity List" }, + { BSSLAP_IEI_ENH_MEAS_REP, "Enhanced Measurement Report" }, + { BSSLAP_IEI_LAC, "Location Area Code" }, + { BSSLAP_IEI_FREQ_LIST, "Frequency List" }, + { BSSLAP_IEI_MS_POWER, "MS Power" }, + { BSSLAP_IEI_DELTA_TIMER, "Delta Timer" }, + { BSSLAP_IEI_SERVING_CELL_ID, "Serving Cell Identifier" }, + { BSSLAP_IEI_ENCR_KEY, "Encryption Key" }, + { BSSLAP_IEI_CIPH_MODE_SET, "Cipher Mode Setting" }, + { BSSLAP_IEI_CHAN_MODE, "Channel Mode" }, + { BSSLAP_IEI_MR_CONFIG, "MultiRate Configuration" }, + { BSSLAP_IEI_POLLING_REPETITION, "Polling Repetition" }, + { BSSLAP_IEI_PACKET_CHAN_DESC, "Packet Channel Description" }, + { BSSLAP_IEI_TLLI, "TLLI" }, + { BSSLAP_IEI_TFI, "TFI" }, + { BSSLAP_IEI_TBF_START_TIME, "TBF Starting Time" }, + { BSSLAP_IEI_PWRUP_START_TIME, "Powerup Starting Time" }, + { BSSLAP_IEI_LONG_ENCR_KEY, "Long Encryption Key" }, + { BSSLAP_IEI_CONCUR_POS_PROC_F, "Concurrent Positioning Flag" }, + {} +}; + +/*! @} */ diff --git a/src/gsm/bssmap_le.c b/src/gsm/bssmap_le.c new file mode 100644 index 00000000..1ee45517 --- /dev/null +++ b/src/gsm/bssmap_le.c @@ -0,0 +1,937 @@ +/* 3GPP TS 49.031 BSSMAP-LE protocol definitions */ +/* + * (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * All Rights Reserved + * + * Author: Neels Hofmeyr <neels@hofmeyr.de> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <string.h> + +#include <osmocom/core/byteswap.h> +#include <osmocom/core/endian.h> +#include <osmocom/core/msgb.h> +#include <osmocom/gsm/bssmap_le.h> +#include <osmocom/gsm/bsslap.h> +#include <osmocom/gsm/gad.h> +#include <osmocom/gsm/gsm48.h> +#include <osmocom/gsm/gsm0808.h> + +/*! \addtogroup bssmap_le + * @{ + * \file bssmap_le.c + * Message encoding and decoding for 3GPP TS 49.031 BSSMAP-LE. + */ + +#define BSSAP_LE_MSG_SIZE BSSMAP_MSG_SIZE +#define BSSAP_LE_MSG_HEADROOM BSSMAP_MSG_HEADROOM + +static const struct tlv_definition osmo_bssmap_le_tlvdef = { + .def = { + [BSSMAP_LE_IEI_LCS_QoS] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_LCS_PRIORITY] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_LOCATION_TYPE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_GANSS_LOCATION_TYPE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_GEO_LOCATION] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_POSITIONING_DATA] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_GANSS_POS_DATA] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_VELOCITY_DATA] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_LCS_CAUSE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_LCS_CLIENT_TYPE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_APDU] = { TLV_TYPE_TL16V }, + [BSSMAP_LE_IEI_NET_ELEM_ID] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_REQ_GPS_ASS_D] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_REQ_GANSS_ASS_D] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_DECIPH_KEYS] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_RET_ERR_REQ] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_RET_ERR_CAUSE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_SEGMENTATION] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CLASSMARK3_INFO] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CAUSE] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CELL_ID] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CHOSEN_CHAN] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_IMSI] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_LCS_CAPABILITY] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_PKT_MEAS_REP] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CELL_ID_LIST] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_IMEI] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_BSS_MLAT_CAP] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_CELL_INFO_LIST] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_BTS_RX_ACC_LVL] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_MLAT_METHOD] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_MLAT_TA] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_MS_SYNC_ACC] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_SHORT_ID_SET] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_RANDOM_ID_SET] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_SHORT_BSS_ID] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_RANDOM_ID] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_SHORT_ID] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_COVERAGE_CLASS] = { TLV_TYPE_TLV }, + [BSSMAP_LE_IEI_MTA_ACC_SEC_RQD] = { TLV_TYPE_TLV }, + }, +}; + +#define DEC_ERR_NO_RETURN(RC, MSG_TYPE, IEI, CAUSE, fmt, args...) do { \ + if (err && !*err) { \ + *err = talloc_zero(err_ctx, struct osmo_bssmap_le_err); \ + **err = (struct osmo_bssmap_le_err){ \ + .rc = (RC), \ + .msg_type = (MSG_TYPE), \ + .iei = (IEI), \ + .cause = (CAUSE), \ + }; \ + (*err)->logmsg = talloc_asprintf(*err, "Error decoding BSSMAP-LE%s%s%s%s%s: " fmt, \ + (MSG_TYPE) >= 0 ? " " : "", \ + (MSG_TYPE) >= 0 ? osmo_bssmap_le_msgt_name(MSG_TYPE) : "", \ + (IEI) >= 0 ? ": " : "", \ + (IEI) >= 0 ? osmo_bssmap_le_iei_name(IEI) : "", \ + (IEI) >= 0 ? " IE" : "", \ + ##args); \ + } \ + } while(0) + +#define DEC_ERR(RC, MSG_TYPE, IEI, CAUSE, fmt, args...) do { \ + DEC_ERR_NO_RETURN(RC, MSG_TYPE, IEI, CAUSE, fmt, ##args); \ + return RC; \ + } while(0) + +#define DEC_IE_MANDATORY(MSG_TYPE, IEI, DEC_FUN, DEC_FUN_ARG) do { \ + const struct tlv_p_entry *e; \ + int rc; \ + if (!(e = TLVP_GET(tp, IEI))) \ + DEC_ERR(-EINVAL, MSG_TYPE, IEI, LCS_CAUSE_DATA_MISSING_IN_REQ, "missing mandatory IE"); \ + rc = DEC_FUN(DEC_FUN_ARG, MSG_TYPE, IEI, err, err_ctx, e->val, e->len); \ + if (rc) \ + DEC_ERR(rc, MSG_TYPE, IEI, LCS_CAUSE_UNSPECIFIED, "cannot parse IE"); \ + } while (0) + +#define DEC_IE_OPTIONAL_FLAG(MSG_TYPE, IEI, DEC_FUN, DEC_FUN_ARG, PRESENCE_FLAG) do { \ + const struct tlv_p_entry *e; \ + int rc; \ + if ((e = TLVP_GET(tp, IEI))) {\ + rc = DEC_FUN(DEC_FUN_ARG, MSG_TYPE, IEI, err, err_ctx, e->val, e->len); \ + if (rc) \ + DEC_ERR(rc, MSG_TYPE, IEI, LCS_CAUSE_UNSPECIFIED, "cannot parse IE"); \ + PRESENCE_FLAG = true; \ + } \ + } while (0) + +#define DEC_IE_OPTIONAL(MSG_TYPE, IEI, DEC_FUN, DEC_FUN_ARG) do { \ + const struct tlv_p_entry *e; \ + int rc; \ + if ((e = TLVP_GET(tp, IEI))) {\ + rc = DEC_FUN(DEC_FUN_ARG, MSG_TYPE, IEI, err, err_ctx, e->val, e->len); \ + if (rc) \ + DEC_ERR(rc, MSG_TYPE, IEI, LCS_CAUSE_UNSPECIFIED, "cannot parse IE"); \ + } \ + } while (0) + +/*! Encode full BSSMAP-LE Location Type IE, including IEI tag and length. + * \param[inout] msg Message buffer to append to. + * \param[in] location_type Values to enconde. + * \returns length of bytes written to the msgb. + */ +uint8_t osmo_bssmap_le_ie_enc_location_type(struct msgb *msg, + const struct bssmap_le_location_type *location_type) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + OSMO_ASSERT(msg); + msgb_put_u8(msg, BSSMAP_LE_IEI_LOCATION_TYPE); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + msgb_put_u8(msg, location_type->location_information); + + switch (location_type->location_information) { + case BSSMAP_LE_LOC_INFO_ASSIST_TARGET_MS: + case BSSMAP_LE_LOC_INFO_BC_DECIPHER_KEYS: + msgb_put_u8(msg, location_type->positioning_method); + break; + default: + break; + } + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode BSSMAP-LE Location Type IE value part. + * \param[out] lt Buffer to write decoded values to. + * \param[in] elem Pointer to the value part, the V of a TLV. + * \param[in] len Length, the L of a TLV. + * \returns 0 on success, negative on error; lt is always overwritten: cleared on error, populated with values on + * success. + */ +int osmo_bssmap_le_ie_dec_location_type(struct bssmap_le_location_type *lt, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + memset(lt, 0x00, sizeof(*lt)); + + if (!elem || len < 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "zero length"); + + lt->location_information = elem[0]; + switch (lt->location_information) { + + case BSSMAP_LE_LOC_INFO_CURRENT_GEOGRAPHIC: + if (len != 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, + "location info type 'Current Geographic': length should be 1 byte, got %u", len); + lt->positioning_method = BSSMAP_LE_POS_METHOD_OMITTED; + return 0; + + case BSSMAP_LE_LOC_INFO_ASSIST_TARGET_MS: + case BSSMAP_LE_LOC_INFO_BC_DECIPHER_KEYS: + if (len != 2) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, + "location info type %d: length should be 2 bytes, got %u", + lt->location_information, len); + lt->positioning_method = elem[1]; + switch (lt->positioning_method) { + case BSSMAP_LE_POS_METHOD_MOBILE_ASSISTED_E_OTD: + case BSSMAP_LE_POS_METHOD_MOBILE_BASED_E_OTD: + case BSSMAP_LE_POS_METHOD_ASSISTED_GPS: + return 0; + default: + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, + "location info type %d: unknown Positioning Method: %d", + lt->location_information, lt->positioning_method); + } + + default: + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "unknown location info type %d", + lt->location_information); + } +} + +/*! Encode full BSSMAP-LE LCS Client Type IE, including IEI tag and length. + * \param[inout] msg Message buffer to append to. + * \param[in] client_type Value to enconde. + * \returns length of bytes written to the msgb. + */ +static uint8_t osmo_bssmap_le_ie_enc_lcs_client_type(struct msgb *msg, enum bssmap_le_lcs_client_type client_type) +{ + OSMO_ASSERT(msg); + msgb_put_u8(msg, BSSMAP_LE_IEI_LCS_CLIENT_TYPE); + /* length */ + msgb_put_u8(msg, 1); + msgb_put_u8(msg, client_type); + return 3; +} + +static int osmo_bssmap_le_ie_dec_lcs_client_type(enum bssmap_le_lcs_client_type *client_type, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + *client_type = 0; + + if (!elem || len < 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "zero length"); + + *client_type = elem[0]; + + switch (*client_type) { + case BSSMAP_LE_LCS_CTYPE_VALUE_ADDED_UNSPECIFIED: + case BSSMAP_LE_LCS_CTYPE_PLMN_OPER_UNSPECIFIED: + case BSSMAP_LE_LCS_CTYPE_PLMN_OPER_BCAST_SERVICE: + case BSSMAP_LE_LCS_CTYPE_PLMN_OPER_OAM: + case BSSMAP_LE_LCS_CTYPE_PLMN_OPER_ANON_STATS: + case BSSMAP_LE_LCS_CTYPE_PLMN_OPER_TGT_MS_SVC: + case BSSMAP_LE_LCS_CTYPE_EMERG_SVC_UNSPECIFIED: + case BSSMAP_LE_LCS_CTYPE_LI_UNSPECIFIED: + return 0; + default: + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "unknown LCS Client Type: %d", *client_type); + } +} + +/*! Encode full BSSMAP-LE LCS Priority IE, including IEI tag and length. + * \param[inout] msg Message buffer to append to. + * \param[in] priority Value to enconde. + * \returns length of bytes written to the msgb. + */ +static uint8_t osmo_bssmap_le_ie_enc_lcs_priority(struct msgb *msg, uint8_t priority) +{ + OSMO_ASSERT(msg); + msgb_put_u8(msg, BSSMAP_LE_IEI_LCS_PRIORITY); + /* length */ + msgb_put_u8(msg, 1); + msgb_put_u8(msg, priority); + return 3; +} + +static int osmo_bssmap_le_ie_dec_lcs_priority(uint8_t *priority, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + if (!elem || len != 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "unexpected length"); + + *priority = elem[0]; + return 0; +} + +/*! Encode full BSSMAP-LE LCS QoS IE, including IEI tag and length. + * \param[inout] msg Message buffer to append to. + * \param[in] priority Value to enconde. + * \returns length of bytes written to the msgb. + */ +static uint8_t osmo_bssmap_le_ie_enc_lcs_qos(struct msgb *msg, const struct osmo_bssmap_le_lcs_qos *qos) +{ + OSMO_ASSERT(msg); + msgb_tlv_put(msg, BSSMAP_LE_IEI_LCS_QoS, sizeof(*qos), (const uint8_t *)qos); + return 2 + sizeof(*qos); +} + +static int osmo_bssmap_le_ie_dec_lcs_qos(struct osmo_bssmap_le_lcs_qos *qos, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + if (!elem || len != sizeof(*qos)) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "unexpected length"); + + memcpy(qos, elem, len); + return 0; +} + +/*! Encode the value part of 3GPP TS 49.031 10.13 LCS Cause, without IEI and len. + * Identically used in 3GPP TS 48.008 3.2.2.66. Usage example: + * + * uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_LCS_CAUSE); + * int rc = osmo_lcs_cause_enc(msg, &lcs_cause); + * if (rc < 0) + * goto error; + * *l = rc; + * + * \param[inout] msg Message buffer to append the LCS Cause values to. + * \param[in] lcs_cause LCS Cause values to enconde. + * \returns length of bytes written to the msgb. + */ +int osmo_lcs_cause_enc(struct msgb *msg, const struct lcs_cause_ie *lcs_cause) +{ + msgb_put_u8(msg, lcs_cause->cause_val); + if (lcs_cause->cause_val == LCS_CAUSE_POS_METH_FAILURE && lcs_cause->diag_val_present) { + msgb_put_u8(msg, lcs_cause->diag_val); + return 2; + } + return 1; +} + +/*! Decode the value part of 3GPP TS 49.031 10.13 LCS Cause, without IEI and len. + * Identically used in 3GPP TS 48.008 3.2.2.66. + * + * \param[out] lcs_cause Write decoded LCS Cause values here. + * \param[in] data Encoded cause bytes. + * \param[in] len Length of data in bytes. + * \returns 0 on success, negative on error. + */ +int osmo_lcs_cause_dec(struct lcs_cause_ie *lcs_cause, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *data, uint8_t len) +{ + memset(lcs_cause, 0x00, sizeof(*lcs_cause)); + + if (!data || len < 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "zero length"); + + lcs_cause->present = true; + lcs_cause->cause_val = data[0]; + if (len > 1) { + lcs_cause->diag_val_present = true; + lcs_cause->diag_val = data[1]; + } + if (len > 2) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "expected length <= 2, got %u", len); + + return 0; +} + +static int osmo_bssmap_le_ie_enc_apdu(struct msgb *msg, const struct bsslap_pdu *bsslap) +{ + uint8_t *old_tail; + void *l; + msgb_put_u8(msg, BSSMAP_LE_IEI_APDU); + l = msgb_put(msg, 2); + old_tail = msg->tail; + msgb_put_u8(msg, BSSMAP_LE_APDU_PROT_BSSLAP); + int rc = osmo_bsslap_enc(msg, bsslap); + if (rc <= 0) + return -EINVAL; + osmo_store16be(msg->tail - old_tail, l); + return 0; +} + +static int osmo_bssmap_le_ie_dec_apdu(struct bsslap_pdu *bsslap, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + enum bssmap_le_apdu_proto proto; + struct osmo_bsslap_err *bsslap_err; + + if (!data || len < 1) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "zero length"); + + proto = data[0]; + + switch (proto) { + case BSSMAP_LE_APDU_PROT_BSSLAP: + if (osmo_bsslap_dec(bsslap, &bsslap_err, err_ctx, data + 1, len - 1)) { + DEC_ERR_NO_RETURN(bsslap_err ? bsslap_err->rc : -EINVAL, + msgt, iei, LCS_CAUSE_UNSPECIFIED, + "Error decoding BSSLAP%s%s", + bsslap_err && bsslap_err->logmsg ? ": " : "", + bsslap_err && bsslap_err->logmsg ? bsslap_err->logmsg : ""); + (*err)->bsslap_err = bsslap_err; + return (*err)->rc; + } + return 0; + case BSSMAP_LE_APDU_PROT_LLP: + case BSSMAP_LE_APDU_PROT_SMLCPP: + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Unimplemented APDU type: %d", proto); + default: + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Invalid APDU type: %d", proto); + } +} + +static int osmo_bssmap_le_ie_dec_cell_id(struct gsm0808_cell_id *cell_id, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + int rc; + rc = gsm0808_dec_cell_id(cell_id, elem, len); + if (rc <= 0) + DEC_ERR(rc, msgt, iei, LCS_CAUSE_UNSPECIFIED, "Error decoding Cell Identifier %s", + osmo_hexdump_c(err_ctx, elem, len)); + return 0; +} + +static int osmo_bssmap_le_ie_dec_imsi(struct osmo_mobile_identity *imsi, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + int rc; + rc = osmo_mobile_identity_decode(imsi, elem, len, false); + if (rc || imsi->type != GSM_MI_TYPE_IMSI) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, + "cannot parse IMSI identity %s", osmo_hexdump_c(err_ctx, elem, len)); + return 0; +} + +static int osmo_bssmap_le_ie_dec_imei(struct osmo_mobile_identity *imei, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + int rc; + rc = osmo_mobile_identity_decode(imei, elem, len, false); + if (rc || imei->type != GSM_MI_TYPE_IMEI) + DEC_ERR(-EINVAL, msgt, iei, LCS_CAUSE_UNSPECIFIED, + "cannot parse IMEI identity %s", osmo_hexdump_c(err_ctx, elem, len)); + return 0; +} + +static int osmo_bssmap_le_ie_dec_gad(union gad_raw *gad, + enum bssmap_le_msgt msgt, enum bssmap_le_iei iei, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *elem, uint8_t len) +{ + struct osmo_gad_err *gad_err; + if (osmo_gad_raw_read(gad, &gad_err, err_ctx, elem, len)) { + DEC_ERR_NO_RETURN(gad_err ? gad_err->rc : -EINVAL, + msgt, BSSMAP_LE_IEI_GEO_LOCATION, LCS_CAUSE_UNSPECIFIED, + "Error decoding GAD%s%s", + gad_err && gad_err->logmsg ? ": " : "", + gad_err && gad_err->logmsg ? gad_err->logmsg : ""); + (*err)->gad_err = gad_err; + return (*err)->rc; + } + return 0; +} + +struct osmo_bssap_le_header { + uint8_t type; + uint8_t length; + uint8_t data[0]; +} __attribute__((packed)); + +/*! Return the BSSMAP-LE msg_type from a BSSAP-LE PDU, e.g. from a msgb_l3(). + * \param[in] data BSSAP-LE PDU data, starting with BSSAP-LE discriminator. + * \param[in] len Length of data in bytes. + * \returns bssmap_le_msgt or negative on error or non-BSSMAP-LE discriminator. */ +enum bssmap_le_msgt osmo_bssmap_le_msgt(const uint8_t *data, uint8_t len) +{ + const struct osmo_bssap_le_header *h = (void*)data; + if (!data || len < sizeof(struct osmo_bssap_le_header) + 1) + return -1; + if (h->type != BSSAP_LE_MSG_DISCR_BSSMAP_LE) + return -1; + return h->data[0]; +} + +static int osmo_bssmap_le_enc_reset(struct msgb *msg, enum gsm0808_cause cause) +{ + /* The BSSMAP-LE Reset Cause is defined as identical to the 3GPP TS 48.008 Cause. */ + gsm0808_enc_cause(msg, cause); + return 0; +} + +static int osmo_bssmap_le_dec_reset(enum gsm0808_cause *cause, + enum bssmap_le_msgt msgt, + struct osmo_bssmap_le_err **err, void *err_ctx, + const struct tlv_parsed *tp) +{ + const struct tlv_p_entry *e; + + if (!(e = TLVP_GET(tp, BSSMAP_LE_IEI_CAUSE))) + DEC_ERR(-EINVAL, msgt, BSSMAP_LE_IEI_CAUSE, LCS_CAUSE_DATA_MISSING_IN_REQ, "missing mandatory IE"); + + *cause = gsm0808_get_cause(tp); + if (*cause < 0) + DEC_ERR(-EINVAL, msgt, BSSMAP_LE_IEI_CAUSE, LCS_CAUSE_UNSPECIFIED, "cannot parse IE"); + + return 0; +} + +static int osmo_bssmap_le_enc_perform_loc_req(struct msgb *msg, const struct bssmap_le_perform_loc_req *params) +{ + osmo_bssmap_le_ie_enc_location_type(msg, ¶ms->location_type); + + gsm0808_enc_cell_id(msg, ¶ms->cell_id); + + if (params->lcs_client_type_present) + osmo_bssmap_le_ie_enc_lcs_client_type(msg, params->lcs_client_type); + + if (params->more_items && params->lcs_priority_present) + osmo_bssmap_le_ie_enc_lcs_priority(msg, params->lcs_priority); + + if (params->more_items && params->lcs_qos_present) + osmo_bssmap_le_ie_enc_lcs_qos(msg, ¶ms->lcs_qos); + + if (params->apdu_present) { + int rc = osmo_bssmap_le_ie_enc_apdu(msg, ¶ms->apdu); + if (rc < 0) + return rc; + } + + if (params->imsi.type == GSM_MI_TYPE_IMSI) { + uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_IMSI); + int rc = osmo_mobile_identity_encode_msgb(msg, ¶ms->imsi, false); + if (rc < 0) + return rc; + *l = rc; + } + + if (params->imei.type == GSM_MI_TYPE_IMEI) { + uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_IMEI); + int rc = osmo_mobile_identity_encode_msgb(msg, ¶ms->imei, false); + if (rc < 0) + return rc; + *l = rc; + } + return 0; +} + +static int osmo_bssmap_le_dec_perform_loc_req(struct bssmap_le_perform_loc_req *params, + enum bssmap_le_msgt msgt, + struct osmo_bssmap_le_err **err, void *err_ctx, + const struct tlv_parsed *tp) +{ + memset(params, 0x00, sizeof(*params)); + + DEC_IE_MANDATORY(msgt, BSSMAP_LE_IEI_LOCATION_TYPE, osmo_bssmap_le_ie_dec_location_type, + ¶ms->location_type); + DEC_IE_MANDATORY(msgt, BSSMAP_LE_IEI_CELL_ID, osmo_bssmap_le_ie_dec_cell_id, + ¶ms->cell_id); + DEC_IE_OPTIONAL_FLAG(msgt, BSSMAP_LE_IEI_LCS_CLIENT_TYPE, osmo_bssmap_le_ie_dec_lcs_client_type, + ¶ms->lcs_client_type, params->lcs_client_type_present); + DEC_IE_OPTIONAL_FLAG(msgt, BSSMAP_LE_IEI_LCS_PRIORITY, osmo_bssmap_le_ie_dec_lcs_priority, + ¶ms->lcs_priority, params->lcs_priority_present); + DEC_IE_OPTIONAL_FLAG(msgt, BSSMAP_LE_IEI_LCS_QoS, osmo_bssmap_le_ie_dec_lcs_qos, + ¶ms->lcs_qos, params->lcs_qos_present); + DEC_IE_OPTIONAL_FLAG(msgt, BSSMAP_LE_IEI_APDU, osmo_bssmap_le_ie_dec_apdu, ¶ms->apdu, + params->apdu_present); + DEC_IE_OPTIONAL(msgt, BSSMAP_LE_IEI_IMSI, osmo_bssmap_le_ie_dec_imsi, ¶ms->imsi); + DEC_IE_OPTIONAL(msgt, BSSMAP_LE_IEI_IMEI, osmo_bssmap_le_ie_dec_imei, ¶ms->imei); + + if (params->lcs_priority_present || params->lcs_qos_present) + params->more_items = true; + + return 0; +} + +static int osmo_bssmap_le_enc_perform_loc_resp(struct msgb *msg, const struct bssmap_le_perform_loc_resp *params) +{ + if (params->location_estimate_present) { + uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_GEO_LOCATION); + int rc = osmo_gad_raw_write(msg, ¶ms->location_estimate); + if (rc < 0) + return rc; + *l = rc; + } + + if (params->lcs_cause.present) { + uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_LCS_CAUSE); + int rc = osmo_lcs_cause_enc(msg, ¶ms->lcs_cause); + if (rc < 0) + return rc; + *l = rc; + } + return 0; +} + +static int osmo_bssmap_le_dec_perform_loc_resp(struct bssmap_le_perform_loc_resp *params, + enum bssmap_le_msgt msgt, + struct osmo_bssmap_le_err **err, void *err_ctx, + const struct tlv_parsed *tp) +{ + memset(params, 0x00, sizeof(*params)); + + DEC_IE_OPTIONAL_FLAG(msgt, BSSMAP_LE_IEI_GEO_LOCATION, osmo_bssmap_le_ie_dec_gad, ¶ms->location_estimate, + params->location_estimate_present); + DEC_IE_OPTIONAL(msgt, BSSMAP_LE_IEI_LCS_CAUSE, osmo_lcs_cause_dec, ¶ms->lcs_cause); + + return 0; +} + +static int osmo_bssmap_le_enc_perform_loc_abort(struct msgb *msg, const struct lcs_cause_ie *params) +{ + uint8_t *l = msgb_tl_put(msg, BSSMAP_LE_IEI_LCS_CAUSE); + int rc = osmo_lcs_cause_enc(msg, params); + if (rc < 0) + return rc; + *l = rc; + return 0; +} + +static int osmo_bssmap_le_dec_perform_loc_abort(struct lcs_cause_ie *params, + enum bssmap_le_msgt msgt, + struct osmo_bssmap_le_err **err, void *err_ctx, + const struct tlv_parsed *tp) +{ + memset(params, 0x00, sizeof(*params)); + + DEC_IE_MANDATORY(msgt, BSSMAP_LE_IEI_LCS_CAUSE, osmo_lcs_cause_dec, params); + return 0; +} + +static int osmo_bssmap_le_enc_conn_oriented_info(struct msgb *msg, + const struct bssmap_le_conn_oriented_info *params) +{ + return osmo_bssmap_le_ie_enc_apdu(msg, ¶ms->apdu); +} + +static int osmo_bssmap_le_dec_conn_oriented_info(struct bssmap_le_conn_oriented_info *params, + enum bssmap_le_msgt msgt, + struct osmo_bssmap_le_err **err, void *err_ctx, + const struct tlv_parsed *tp) +{ + memset(params, 0x00, sizeof(*params)); + + DEC_IE_MANDATORY(msgt, BSSMAP_LE_IEI_APDU, osmo_bssmap_le_ie_dec_apdu, ¶ms->apdu); + return 0; +} + +/*! Encode BSSMAP-LE PDU and add to msgb (3GPP TS 49.031). + * See also osmo_bssap_le_enc(). + * \param[out] msg msgb to append to. + * \param[in] pdu PDU data to encode. + * \return number of bytes written, negative on error. + */ +static int osmo_bssmap_le_enc(struct msgb *msg, const struct bssmap_le_pdu *pdu) +{ + int rc; + uint8_t *old_tail; + old_tail = msg->tail; + + msgb_v_put(msg, pdu->msg_type); + + switch (pdu->msg_type) { + case BSSMAP_LE_MSGT_RESET: + rc = osmo_bssmap_le_enc_reset(msg, pdu->reset); + break; + case BSSMAP_LE_MSGT_RESET_ACK: + /* Consists only of the message type. */ + rc = 0; + break; + case BSSMAP_LE_MSGT_PERFORM_LOC_REQ: + rc = osmo_bssmap_le_enc_perform_loc_req(msg, &pdu->perform_loc_req); + break; + case BSSMAP_LE_MSGT_PERFORM_LOC_RESP: + rc = osmo_bssmap_le_enc_perform_loc_resp(msg, &pdu->perform_loc_resp); + break; + case BSSMAP_LE_MSGT_PERFORM_LOC_ABORT: + rc = osmo_bssmap_le_enc_perform_loc_abort(msg, &pdu->perform_loc_abort); + break; + case BSSMAP_LE_MSGT_CONN_ORIENTED_INFO: + rc = osmo_bssmap_le_enc_conn_oriented_info(msg, &pdu->conn_oriented_info); + break; + default: + rc = -ENOTSUP; + } + + if (rc < 0) + return rc; + + return (msg->tail - old_tail); +} + +/*! Decode BSSMAP-LE PDU (3GPP TS 49.031). + * See also osmo_bssap_le_dec(). + * \param[out] pdu Write decoded values here. + * \param[in] data Pointer to BSSMAP-LE PDU raw data. + * \param[in] len Data length to decode. + * \return NULL upon success, a human readable error message on failure. + */ +static int osmo_bssmap_le_dec(struct bssmap_le_pdu *pdu, + struct osmo_bssmap_le_err **err, void *err_ctx, + const uint8_t *data, size_t len) +{ + const uint8_t *ies_start; + int ies_len; + struct tlv_parsed tp; + + memset(pdu, 0x00, sizeof(*pdu)); + + if (len < 1) + DEC_ERR(-EINVAL, -1, -1, LCS_CAUSE_UNSPECIFIED, "zero length"); + pdu->msg_type = data[0]; + + /* BSSMAP-LE IEs */ + ies_start = &data[1]; + ies_len = len - 1; + + if (tlv_parse(&tp, &osmo_bssmap_le_tlvdef, ies_start, ies_len, 0, 0) < 0) + DEC_ERR(-EINVAL, pdu->msg_type, -1, LCS_CAUSE_UNSPECIFIED, "failed to parse TLV structure"); + + switch (pdu->msg_type) { + case BSSMAP_LE_MSGT_RESET: + return osmo_bssmap_le_dec_reset(&pdu->reset, pdu->msg_type, err, err_ctx, &tp); + case BSSMAP_LE_MSGT_RESET_ACK: + /* Consists only of the message type. */ + return 0; + case BSSMAP_LE_MSGT_PERFORM_LOC_REQ: + return osmo_bssmap_le_dec_perform_loc_req(&pdu->perform_loc_req, pdu->msg_type, err, err_ctx, &tp); + case BSSMAP_LE_MSGT_PERFORM_LOC_RESP: + return osmo_bssmap_le_dec_perform_loc_resp(&pdu->perform_loc_resp, pdu->msg_type, err, err_ctx, &tp); + case BSSMAP_LE_MSGT_PERFORM_LOC_ABORT: + return osmo_bssmap_le_dec_perform_loc_abort(&pdu->perform_loc_abort, pdu->msg_type, err, err_ctx, &tp); + case BSSMAP_LE_MSGT_CONN_ORIENTED_INFO: + return osmo_bssmap_le_dec_conn_oriented_info(&pdu->conn_oriented_info, pdu->msg_type, err, err_ctx, + &tp); + default: + DEC_ERR(-EINVAL, pdu->msg_type, -1, LCS_CAUSE_UNSPECIFIED, "Unsupported BSSMAP-LE message type"); + } +} + +/*! Encode BSSAP-LE PDU returned in new msgb (3GPP TS 49.031). + * By spec, BSSAP-LE contains either BSSMAP-LE or DTAP. + * \param[in] pdu PDU data to encode. + * \return msgb with encoded data and l2h set to the start. + */ +struct msgb *osmo_bssap_le_enc(const struct bssap_le_pdu *pdu) +{ + struct msgb *msg; + int rc; + + if (pdu->discr != BSSAP_LE_MSG_DISCR_BSSMAP_LE) + return NULL; + + msg = msgb_alloc_headroom(BSSAP_LE_MSG_SIZE, BSSAP_LE_MSG_HEADROOM, + osmo_bssmap_le_msgt_name(pdu->bssmap_le.msg_type)); + if (!msg) + return NULL; + + rc = osmo_bssmap_le_enc(msg, &pdu->bssmap_le); + if (rc <= 0) { + msgb_free(msg); + return NULL; + } + + /* prepend header with final length */ + msg->l2h = msgb_tv_push(msg, pdu->discr, msgb_length(msg)); + + return msg; +} + +/*! Decode BSSAP-LE PDU (3GPP TS 49.031). + * \param[out] pdu Write decoded values here. + * \param[in] data Pointer to BSSMAP-LE PDU raw data. + * \param[in] len Data length to decode. + * \return NULL upon success, a human readable error message on failure. + */ +int osmo_bssap_le_dec(struct bssap_le_pdu *pdu, struct osmo_bssap_le_err **err, void *err_ctx, struct msgb *msg) +{ + struct osmo_bssap_le_header *h; + unsigned int check_len; + struct osmo_bssmap_le_err *bssmap_le_err = NULL; + int rc; + +#define BSSAP_LE_DEC_ERR(RC, fmt, args...) do { \ + if (err && !*err) { \ + *err = talloc_zero(err_ctx, struct osmo_bssap_le_err); \ + **err = (struct osmo_bssap_le_err){ \ + .rc = (RC), \ + .logmsg = talloc_asprintf(*err, "Error decoding BSSAP-LE: " fmt, ##args), \ + }; \ + } \ + return RC; \ + } while(0) + + memset(pdu, 0x00, sizeof(*pdu)); + + h = msgb_l2(msg); + if (!h) + BSSAP_LE_DEC_ERR(-EINVAL, "missing msgb_l2() pointer"); + if (msgb_l2len(msg) < sizeof(*h)) + BSSAP_LE_DEC_ERR(-EINVAL, "message too short for header"); + check_len = msgb_l2len(msg) - sizeof(*h); + if (h->length < check_len) + BSSAP_LE_DEC_ERR(-EINVAL, "message truncated, header length (%u) longer than message (%u)", + h->length, check_len); + + switch (h->type) { + case BSSAP_LE_MSG_DISCR_BSSMAP_LE: + break; + default: + BSSAP_LE_DEC_ERR(-EINVAL, "unsupported discr %u, only BSSMAP-LE is implemented", h->type); + } + + rc = osmo_bssmap_le_dec(&pdu->bssmap_le, err ? &bssmap_le_err : NULL, err_ctx, + h->data, h->length); + if (rc) + BSSAP_LE_DEC_ERR(rc, "%s", + (bssmap_le_err && bssmap_le_err->logmsg) ? + bssmap_le_err->logmsg : "unknown error in BSSMAP-LE part"); + return 0; +} + +const struct value_string osmo_bssmap_le_msgt_names[] = { + { BSSMAP_LE_MSGT_PERFORM_LOC_REQ, "PERFORM LOCATION REQUEST" }, + { BSSMAP_LE_MSGT_PERFORM_LOC_RESP, "PERFORM LOCATION RESPONSE" }, + { BSSMAP_LE_MSGT_PERFORM_LOC_ABORT, "PERFORM LOCATION ABORT" }, + { BSSMAP_LE_MSGT_PERFORM_LOC_INFO, "PERFORM LOCATION INFO" }, + { BSSMAP_LE_MSGT_ASSIST_INFO_REQ, "ASSISTANCE INFORMATION REQUEST" }, + { BSSMAP_LE_MSGT_ASSIST_INFO_RESP, "ASSISTANCE INFORMATION RESPONSE" }, + { BSSMAP_LE_MSGT_CONN_ORIENTED_INFO, "CONNECTION ORIENTED INFORMATON" }, + { BSSMAP_LE_MSGT_CONN_LESS_INFO, "CONNECTIONLESS INFORMATION" }, + { BSSMAP_LE_MSGT_RESET, "RESET" }, + { BSSMAP_LE_MSGT_RESET_ACK, "RESET ACKNOWLEDGE" }, + {} +}; + +const struct value_string osmo_bssmap_le_iei_names[] = { + { BSSMAP_LE_IEI_LCS_QoS, "LCS_QoS" }, + { BSSMAP_LE_IEI_LCS_PRIORITY, "LCS_PRIORITY" }, + { BSSMAP_LE_IEI_LOCATION_TYPE, "LOCATION_TYPE" }, + { BSSMAP_LE_IEI_GANSS_LOCATION_TYPE, "GANSS_LOCATION_TYPE" }, + { BSSMAP_LE_IEI_GEO_LOCATION, "GEO_LOCATION" }, + { BSSMAP_LE_IEI_POSITIONING_DATA, "POSITIONING_DATA" }, + { BSSMAP_LE_IEI_GANSS_POS_DATA, "GANSS_POS_DATA" }, + { BSSMAP_LE_IEI_VELOCITY_DATA, "VELOCITY_DATA" }, + { BSSMAP_LE_IEI_LCS_CAUSE, "LCS_CAUSE" }, + { BSSMAP_LE_IEI_LCS_CLIENT_TYPE, "LCS_CLIENT_TYPE" }, + { BSSMAP_LE_IEI_APDU, "APDU" }, + { BSSMAP_LE_IEI_NET_ELEM_ID, "NET_ELEM_ID" }, + { BSSMAP_LE_IEI_REQ_GPS_ASS_D, "REQ_GPS_ASS_D" }, + { BSSMAP_LE_IEI_REQ_GANSS_ASS_D, "REQ_GANSS_ASS_D" }, + { BSSMAP_LE_IEI_DECIPH_KEYS, "DECIPH_KEYS" }, + { BSSMAP_LE_IEI_RET_ERR_REQ, "RET_ERR_REQ" }, + { BSSMAP_LE_IEI_RET_ERR_CAUSE, "RET_ERR_CAUSE" }, + { BSSMAP_LE_IEI_SEGMENTATION, "SEGMENTATION" }, + { BSSMAP_LE_IEI_CLASSMARK3_INFO, "CLASSMARK3_INFO" }, + { BSSMAP_LE_IEI_CAUSE, "CAUSE" }, + { BSSMAP_LE_IEI_CELL_ID, "CELL_ID" }, + { BSSMAP_LE_IEI_CHOSEN_CHAN, "CHOSEN_CHAN" }, + { BSSMAP_LE_IEI_IMSI, "IMSI" }, + { BSSMAP_LE_IEI_LCS_CAPABILITY, "LCS_CAPABILITY" }, + { BSSMAP_LE_IEI_PKT_MEAS_REP, "PKT_MEAS_REP" }, + { BSSMAP_LE_IEI_CELL_ID_LIST, "CELL_ID_LIST" }, + { BSSMAP_LE_IEI_IMEI, "IMEI" }, + { BSSMAP_LE_IEI_BSS_MLAT_CAP, "BSS_MLAT_CAP" }, + { BSSMAP_LE_IEI_CELL_INFO_LIST, "CELL_INFO_LIST" }, + { BSSMAP_LE_IEI_BTS_RX_ACC_LVL, "BTS_RX_ACC_LVL" }, + { BSSMAP_LE_IEI_MLAT_METHOD, "MLAT_METHOD" }, + { BSSMAP_LE_IEI_MLAT_TA, "MLAT_TA" }, + { BSSMAP_LE_IEI_MS_SYNC_ACC, "MS_SYNC_ACC" }, + { BSSMAP_LE_IEI_SHORT_ID_SET, "SHORT_ID_SET" }, + { BSSMAP_LE_IEI_RANDOM_ID_SET, "RANDOM_ID_SET" }, + { BSSMAP_LE_IEI_SHORT_BSS_ID, "SHORT_BSS_ID" }, + { BSSMAP_LE_IEI_RANDOM_ID, "RANDOM_ID" }, + { BSSMAP_LE_IEI_SHORT_ID, "SHORT_ID" }, + { BSSMAP_LE_IEI_COVERAGE_CLASS, "COVERAGE_CLASS" }, + { BSSMAP_LE_IEI_MTA_ACC_SEC_RQD, "MTA_ACC_SEC_RQD" }, + {} +}; + +/*! Return a human readable string describing a BSSAP-LE PDU. + * \param[out] buf String buffer to write to. + * \param[in] buflen sizeof(buf). + * \param[in] bssap_le Decoded BSSAP-LE PDU data. + * \returns number of chars that would be written, like snprintf(). + */ +int osmo_bssap_le_pdu_to_str_buf(char *buf, size_t buflen, const struct bssap_le_pdu *bssap_le) +{ + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + const struct bssmap_le_pdu *bssmap_le; + + switch (bssap_le->discr) { + case BSSAP_LE_MSG_DISCR_BSSMAP_LE: + bssmap_le = &bssap_le->bssmap_le; + OSMO_STRBUF_PRINTF(sb, "BSSMAP-LE %s", osmo_bssmap_le_msgt_name(bssmap_le->msg_type)); + switch (bssmap_le->msg_type) { + case BSSMAP_LE_MSGT_PERFORM_LOC_REQ: + if (bssmap_le->perform_loc_req.apdu_present) + OSMO_STRBUF_PRINTF(sb, " with BSSLAP %s", + osmo_bsslap_msgt_name(bssmap_le->perform_loc_req.apdu.msg_type)); + break; + + case BSSMAP_LE_MSGT_CONN_ORIENTED_INFO: + OSMO_STRBUF_PRINTF(sb, " with BSSLAP %s", + osmo_bsslap_msgt_name(bssmap_le->conn_oriented_info.apdu.msg_type)); + break; + + default: + break; + } + break; + default: + OSMO_STRBUF_PRINTF(sb, "BSSAP-LE discr %d not implemented", bssap_le->discr); + break; + } + + return sb.chars_needed; +} + +/*! Return a human readable string describing a BSSAP-LE PDU. + * \param[in] ctx Talloc context to allocate string buffer from. + * \param[in] bssap_le Decoded BSSAP-LE PDU data. + * \returns string. + */ +char *osmo_bssap_le_pdu_to_str_c(void *ctx, const struct bssap_le_pdu *bssap_le) +{ + OSMO_NAME_C_IMPL(ctx, 32, "ERROR", osmo_bssap_le_pdu_to_str_buf, bssap_le) +} + +/*! @} */ diff --git a/src/gsm/bts_features.c b/src/gsm/bts_features.c index e4ff76c8..b6cd82ec 100644 --- a/src/gsm/bts_features.c +++ b/src/gsm/bts_features.c @@ -14,13 +14,9 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301, USA. */ +#include <osmocom/core/utils.h> #include <osmocom/gsm/bts_features.h> const struct value_string osmo_bts_features_descs[] = { @@ -39,11 +35,61 @@ const struct value_string osmo_bts_features_descs[] = { { BTS_FEAT_SPEECH_F_AMR, "Fullrate speech AMR" }, { BTS_FEAT_SPEECH_H_AMR, "Halfrate speech AMR" }, { BTS_FEAT_ETWS_PN, "ETWS Primary Notification via PCH" }, + { BTS_FEAT_PAGING_COORDINATION, "BSS Paging Coordination" }, + { BTS_FEAT_IPV6_NSVC, "NSVC IPv6" }, + { BTS_FEAT_ACCH_REP, "FACCH/SACCH Repetition" }, + { BTS_FEAT_CCN, "Cell Change Notification (CCN)" }, + { BTS_FEAT_VAMOS, "VAMOS (Voice services over Adaptive Multi-user channels on One Slot)" }, + { BTS_FEAT_ABIS_OSMO_PCU, "OsmoPCU over OML link IPA multiplex" }, + { BTS_FEAT_BCCH_POWER_RED, "BCCH carrier power reduction mode" }, + { BTS_FEAT_DYN_TS_SDCCH8, "Dynamic Timeslot configuration as SDCCH8" }, + { BTS_FEAT_ACCH_TEMP_OVP, "FACCH/SACCH Temporary overpower" }, + { BTS_FEAT_OSMUX, "Osmux (Osmocom RTP multiplexing)" }, + { BTS_FEAT_VBS, "Voice Broadcast Service" }, + { BTS_FEAT_VGCS, "Voice Group Call Service" }, { 0, NULL } }; -/*! return string representation of a BTS feature */ +/* Ensure that all BTS_FEAT_* entries are present in osmo_bts_features_descs[] */ +osmo_static_assert(ARRAY_SIZE(osmo_bts_features_descs) == _NUM_BTS_FEAT + 1, _bts_features_descs); + +/*! return description string of a BTS feature (osmo_bts_features_descs). + * To get the plain feature name, use osmo_bts_features_name() instead. */ const char *osmo_bts_feature_name(enum osmo_bts_features feature) { return get_value_string(osmo_bts_features_descs, feature); } + +const struct value_string osmo_bts_features_names[] = { + { BTS_FEAT_HSCSD, "HSCSD" }, + { BTS_FEAT_GPRS, "GPRS" }, + { BTS_FEAT_EGPRS, "EGPRS" }, + { BTS_FEAT_ECSD, "ECSD" }, + { BTS_FEAT_HOPPING, "HOPPING" }, + { BTS_FEAT_MULTI_TSC, "MULTI_TSC" }, + { BTS_FEAT_OML_ALERTS, "OML_ALERTS" }, + { BTS_FEAT_AGCH_PCH_PROP, "AGCH_PCH_PROP" }, + { BTS_FEAT_CBCH, "CBCH" }, + { BTS_FEAT_SPEECH_F_V1, "SPEECH_F_V1" }, + { BTS_FEAT_SPEECH_H_V1, "SPEECH_H_V1" }, + { BTS_FEAT_SPEECH_F_EFR, "SPEECH_F_EFR" }, + { BTS_FEAT_SPEECH_F_AMR, "SPEECH_F_AMR" }, + { BTS_FEAT_SPEECH_H_AMR, "SPEECH_H_AMR" }, + { BTS_FEAT_ETWS_PN, "ETWS_PN" }, + { BTS_FEAT_PAGING_COORDINATION, "PAGING_COORDINATION" }, + { BTS_FEAT_IPV6_NSVC, "IPV6_NSVC" }, + { BTS_FEAT_ACCH_REP, "ACCH_REP" }, + { BTS_FEAT_CCN, "CCN" }, + { BTS_FEAT_VAMOS, "VAMOS" }, + { BTS_FEAT_ABIS_OSMO_PCU, "ABIS_OSMO_PCU" }, + { BTS_FEAT_BCCH_POWER_RED, "BCCH_PWR_RED" }, + { BTS_FEAT_DYN_TS_SDCCH8, "DYN_TS_SDCCH8" }, + { BTS_FEAT_ACCH_TEMP_OVP, "ACCH_TEMP_OVP" }, + { BTS_FEAT_OSMUX, "OSMUX" }, + { BTS_FEAT_VBS, "VBS" }, + { BTS_FEAT_VGCS, "VGCS" }, + {} +}; + +/* Ensure that all BTS_FEAT_* entries are present in osmo_bts_features_names[] */ +osmo_static_assert(ARRAY_SIZE(osmo_bts_features_names) == _NUM_BTS_FEAT + 1, _bts_features_names); diff --git a/src/gsm/cbsp.c b/src/gsm/cbsp.c index ccc2df53..a5e58f4c 100644 --- a/src/gsm/cbsp.c +++ b/src/gsm/cbsp.c @@ -1,3 +1,4 @@ +/* Cell Broadcast Service Protocol (CBSP, 3GPP TS 48.049): Message encoding, decoding and reception */ /* * Copyright (C) 2019 Harald Welte <laforge@gnumonks.org> * @@ -14,10 +15,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include "config.h" @@ -33,7 +30,7 @@ #include <osmocom/gsm/cbsp.h> #include <osmocom/gsm/gsm0808_utils.h> -const __thread char *osmo_cbsp_errstr; +__thread const char *osmo_cbsp_errstr; struct msgb *osmo_cbsp_msgb_alloc(void *ctx, const char *name) { @@ -129,13 +126,13 @@ static int encode_wperiod(uint32_t secs) if (secs <= 10) return secs; if (secs <= 30) - return (secs-10)/2; + return 10 + (secs-10)/2; if (secs <= 120) - return (secs-30)/5; + return 30 + (secs-30)/5; if (secs <= 600) - return (secs-120)/10; + return 120 + (secs-120)/10; if (secs <= 60*60) - return (secs-600)/30; + return 600 + (secs-600)/30; osmo_cbsp_errstr = "warning period out of range"; return -1; } @@ -176,12 +173,15 @@ static int cbsp_enc_write_repl(struct msgb *msg, const struct osmo_cbsp_write_re } } else { int wperiod = encode_wperiod(in->u.emergency.warning_period); + uint8_t *cur; if (wperiod < 0) return -EINVAL; msgb_tv_put(msg, CBSP_IEI_EMERG_IND, in->u.emergency.indicator); msgb_tv16_put(msg, CBSP_IEI_WARN_TYPE, in->u.emergency.warning_type); - msgb_tlv_put(msg, CBSP_IEI_WARN_SEC_INFO, sizeof(in->u.emergency.warning_sec_info), - in->u.emergency.warning_sec_info); + /* Tag + fixed length value! */ + msgb_put_u8(msg, CBSP_IEI_WARN_SEC_INFO); + cur = msgb_put(msg, sizeof(in->u.emergency.warning_sec_info)); + memcpy(cur, in->u.emergency.warning_sec_info, sizeof(in->u.emergency.warning_sec_info)); msgb_tv_put(msg, CBSP_IEI_WARNING_PERIOD, wperiod); } return 0; @@ -348,7 +348,12 @@ static int cbsp_enc_reset_fail(struct msgb *msg, const struct osmo_cbsp_reset_fa /* 8.1.3.18a KEEP ALIVE */ static int cbsp_enc_keep_alive(struct msgb *msg, const struct osmo_cbsp_keep_alive *in) { - msgb_tv_put(msg, CBSP_IEI_KEEP_ALIVE_REP_PERIOD, in->repetition_period); + int rperiod = encode_wperiod(in->repetition_period); + if (in->repetition_period > 120) + return -EINVAL; + if (rperiod < 0) + return -EINVAL; + msgb_tv_put(msg, CBSP_IEI_KEEP_ALIVE_REP_PERIOD, rperiod); return 0; } @@ -532,8 +537,8 @@ static int cbsp_decode_fail_list(struct llist_head *fl, void *ctx, struct osmo_cbsp_fail_ent *ent = talloc_zero(ctx, struct osmo_cbsp_fail_ent); unsigned int len_remain = len - (cur - buf); OSMO_ASSERT(ent); - ent->id_discr = cur[0]; - rc = gsm0808_decode_cell_id_u(&ent->cell_id, ent->id_discr, cur+1, len_remain-1); + ent->id_discr = *cur++; + rc = gsm0808_decode_cell_id_u(&ent->cell_id, ent->id_discr, cur, len_remain-1); if (rc < 0) { osmo_cbsp_errstr = "fail list: error decoding cell_id_union"; return rc; @@ -634,6 +639,7 @@ static int cbsp_dec_write_repl(struct osmo_cbsp_write_replace *out, const struct struct msgb *in, void *ctx) { unsigned int i; + int rc; /* check for mandatory IEs */ if (!TLVP_PRESENT(tp, CBSP_IEI_MSG_ID) || @@ -651,8 +657,10 @@ static int cbsp_dec_write_repl(struct osmo_cbsp_write_replace *out, const struct } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; if (TLVP_PRESENT(tp, CBSP_IEI_CHANNEL_IND)) { uint8_t num_of_pages; @@ -674,6 +682,7 @@ static int cbsp_dec_write_repl(struct osmo_cbsp_write_replace *out, const struct out->u.cbs.category = *TLVP_VAL(tp, CBSP_IEI_CATEGORY); out->u.cbs.rep_period = tlvp_val16be(tp, CBSP_IEI_REP_PERIOD); out->u.cbs.num_bcast_req = tlvp_val16be(tp, CBSP_IEI_NUM_BCAST_REQ); + out->u.cbs.dcs = *TLVP_VAL(tp, CBSP_IEI_DCS); num_of_pages = *TLVP_VAL(tp, CBSP_IEI_NUM_OF_PAGES); if (num_of_pages < 1) return -EINVAL; @@ -712,6 +721,8 @@ static int cbsp_dec_write_repl(struct osmo_cbsp_write_replace *out, const struct static int cbsp_dec_write_repl_compl(struct osmo_cbsp_write_replace_complete *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_NEW_SERIAL_NR, 2)) { osmo_cbsp_errstr = "missing/short mandatory IE"; @@ -727,14 +738,18 @@ static int cbsp_dec_write_repl_compl(struct osmo_cbsp_write_replace_complete *ou INIT_LLIST_HEAD(&out->num_compl_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7)) { - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; if (TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { out->channel_ind = talloc(ctx, enum cbsp_channel_ind); @@ -747,6 +762,8 @@ static int cbsp_dec_write_repl_compl(struct osmo_cbsp_write_replace_complete *ou static int cbsp_dec_write_repl_fail(struct osmo_cbsp_write_replace_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_NEW_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5)) { @@ -762,21 +779,27 @@ static int cbsp_dec_write_repl_fail(struct osmo_cbsp_write_replace_failure *out, } INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; INIT_LLIST_HEAD(&out->num_compl_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7)) { - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; } INIT_LLIST_HEAD(&out->cell_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; } if (TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { @@ -790,6 +813,8 @@ static int cbsp_dec_write_repl_fail(struct osmo_cbsp_write_replace_failure *out, static int cbsp_dec_kill(struct osmo_cbsp_kill *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { @@ -801,8 +826,10 @@ static int cbsp_dec_kill(struct osmo_cbsp_kill *out, const struct tlv_parsed *tp out->old_serial_nr = tlvp_val16be(tp, CBSP_IEI_OLD_SERIAL_NR); INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; if (TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { out->channel_ind = talloc(ctx, enum cbsp_channel_ind); @@ -815,6 +842,8 @@ static int cbsp_dec_kill(struct osmo_cbsp_kill *out, const struct tlv_parsed *tp static int cbsp_dec_kill_compl(struct osmo_cbsp_kill_complete *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { @@ -827,14 +856,18 @@ static int cbsp_dec_kill_compl(struct osmo_cbsp_kill_complete *out, const struct INIT_LLIST_HEAD(&out->num_compl_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7)) { - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; if (TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { out->channel_ind = talloc(ctx, enum cbsp_channel_ind); @@ -847,6 +880,8 @@ static int cbsp_dec_kill_compl(struct osmo_cbsp_kill_complete *out, const struct static int cbsp_dec_kill_fail(struct osmo_cbsp_kill_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5)) { @@ -858,21 +893,27 @@ static int cbsp_dec_kill_fail(struct osmo_cbsp_kill_failure *out, const struct t out->old_serial_nr = tlvp_val16be(tp, CBSP_IEI_OLD_SERIAL_NR); INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; INIT_LLIST_HEAD(&out->num_compl_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7)) { - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; } INIT_LLIST_HEAD(&out->cell_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; } if (TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { @@ -886,6 +927,8 @@ static int cbsp_dec_kill_fail(struct osmo_cbsp_kill_failure *out, const struct t static int cbsp_dec_load_query(struct osmo_cbsp_load_query *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1) || !TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; @@ -893,8 +936,10 @@ static int cbsp_dec_load_query(struct osmo_cbsp_load_query *out, const struct tl } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); return 0; @@ -904,6 +949,8 @@ static int cbsp_dec_load_query(struct osmo_cbsp_load_query *out, const struct tl static int cbsp_dec_load_query_compl(struct osmo_cbsp_load_query_complete *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_RR_LOADING_LIST, 6) || !TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; @@ -911,9 +958,11 @@ static int cbsp_dec_load_query_compl(struct osmo_cbsp_load_query_complete *out, } INIT_LLIST_HEAD(&out->loading_list.list); - cbsp_decode_loading_list(&out->loading_list, ctx, - TLVP_VAL(tp, CBSP_IEI_RR_LOADING_LIST), - TLVP_LEN(tp, CBSP_IEI_RR_LOADING_LIST)); + rc = cbsp_decode_loading_list(&out->loading_list, ctx, + TLVP_VAL(tp, CBSP_IEI_RR_LOADING_LIST), + TLVP_LEN(tp, CBSP_IEI_RR_LOADING_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); return 0; @@ -923,6 +972,8 @@ static int cbsp_dec_load_query_compl(struct osmo_cbsp_load_query_complete *out, static int cbsp_dec_load_query_fail(struct osmo_cbsp_load_query_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5) || !TLVP_PRES_LEN(tp, CBSP_IEI_CHANNEL_IND, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; @@ -930,25 +981,29 @@ static int cbsp_dec_load_query_fail(struct osmo_cbsp_load_query_failure *out, } INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); INIT_LLIST_HEAD(&out->loading_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_RR_LOADING_LIST, 6)) { - cbsp_decode_loading_list(&out->loading_list, ctx, - TLVP_VAL(tp, CBSP_IEI_RR_LOADING_LIST), - TLVP_LEN(tp, CBSP_IEI_RR_LOADING_LIST)); + rc = cbsp_decode_loading_list(&out->loading_list, ctx, + TLVP_VAL(tp, CBSP_IEI_RR_LOADING_LIST), + TLVP_LEN(tp, CBSP_IEI_RR_LOADING_LIST)); } - return 0; + return rc; } /* 8.1.3.10 STATUS QUERY */ static int cbsp_dec_msg_status_query(struct osmo_cbsp_msg_status_query *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1) || @@ -961,8 +1016,10 @@ static int cbsp_dec_msg_status_query(struct osmo_cbsp_msg_status_query *out, out->old_serial_nr = tlvp_val16be(tp, CBSP_IEI_OLD_SERIAL_NR); INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); return 0; @@ -972,6 +1029,8 @@ static int cbsp_dec_msg_status_query(struct osmo_cbsp_msg_status_query *out, static int cbsp_dec_msg_status_query_compl(struct osmo_cbsp_msg_status_query_complete *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7) || @@ -984,9 +1043,11 @@ static int cbsp_dec_msg_status_query_compl(struct osmo_cbsp_msg_status_query_com out->old_serial_nr = tlvp_val16be(tp, CBSP_IEI_OLD_SERIAL_NR); INIT_LLIST_HEAD(&out->num_compl_list.list); - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); return 0; } @@ -995,6 +1056,8 @@ static int cbsp_dec_msg_status_query_compl(struct osmo_cbsp_msg_status_query_com static int cbsp_dec_msg_status_query_fail(struct osmo_cbsp_msg_status_query_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_MSG_ID, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_OLD_SERIAL_NR, 2) || !TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5) || @@ -1007,17 +1070,21 @@ static int cbsp_dec_msg_status_query_fail(struct osmo_cbsp_msg_status_query_fail out->old_serial_nr = tlvp_val16be(tp, CBSP_IEI_OLD_SERIAL_NR); INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; out->channel_ind = *TLVP_VAL(tp, CBSP_IEI_CHANNEL_IND); INIT_LLIST_HEAD(&out->num_compl_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST, 7)) { - cbsp_decode_num_compl_list(&out->num_compl_list, ctx, - TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), - TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + rc = cbsp_decode_num_compl_list(&out->num_compl_list, ctx, + TLVP_VAL(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST), + TLVP_LEN(tp, CBSP_IEI_NUM_BCAST_COMPL_LIST)); + if (rc < 0) + return rc; } return 0; } @@ -1026,14 +1093,19 @@ static int cbsp_dec_msg_status_query_fail(struct osmo_cbsp_msg_status_query_fail static int cbsp_dec_reset(struct osmo_cbsp_reset *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; return -EINVAL; } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; + return 0; } @@ -1041,14 +1113,19 @@ static int cbsp_dec_reset(struct osmo_cbsp_reset *out, const struct tlv_parsed * static int cbsp_dec_reset_compl(struct osmo_cbsp_reset_complete *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; return -EINVAL; } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; + return 0; } @@ -1056,20 +1133,26 @@ static int cbsp_dec_reset_compl(struct osmo_cbsp_reset_complete *out, const stru static int cbsp_dec_reset_fail(struct osmo_cbsp_reset_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5)) { osmo_cbsp_errstr = "missing/short mandatory IE"; return -EINVAL; } INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; INIT_LLIST_HEAD(&out->cell_list.list); if (TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1)) { - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; } return 0; } @@ -1078,12 +1161,14 @@ static int cbsp_dec_reset_fail(struct osmo_cbsp_reset_failure *out, const struct static int cbsp_dec_keep_alive(struct osmo_cbsp_keep_alive *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + uint8_t rperiod; if (!TLVP_PRES_LEN(tp, CBSP_IEI_KEEP_ALIVE_REP_PERIOD, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; return -EINVAL; } - out->repetition_period = *TLVP_VAL(tp, CBSP_IEI_KEEP_ALIVE_REP_PERIOD); + rperiod = *TLVP_VAL(tp, CBSP_IEI_KEEP_ALIVE_REP_PERIOD); + out->repetition_period = decode_wperiod(rperiod); return 0; } @@ -1098,6 +1183,8 @@ static int cbsp_dec_keep_alive_compl(struct osmo_cbsp_keep_alive_complete *out, static int cbsp_dec_restart(struct osmo_cbsp_restart *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_CELL_LIST, 1) || !TLVP_PRES_LEN(tp, CBSP_IEI_BCAST_MSG_TYPE, 1) || !TLVP_PRES_LEN(tp, CBSP_IEI_RECOVERY_IND, 1)) { @@ -1106,8 +1193,10 @@ static int cbsp_dec_restart(struct osmo_cbsp_restart *out, const struct tlv_pars } INIT_LLIST_HEAD(&out->cell_list.list); - cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), - TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + rc = cbsp_decode_cell_list(&out->cell_list, ctx, TLVP_VAL(tp, CBSP_IEI_CELL_LIST), + TLVP_LEN(tp, CBSP_IEI_CELL_LIST)); + if (rc < 0) + return rc; out->bcast_msg_type = *TLVP_VAL(tp, CBSP_IEI_BCAST_MSG_TYPE); out->recovery_ind = *TLVP_VAL(tp, CBSP_IEI_RECOVERY_IND); @@ -1118,6 +1207,8 @@ static int cbsp_dec_restart(struct osmo_cbsp_restart *out, const struct tlv_pars static int cbsp_dec_failure(struct osmo_cbsp_failure *out, const struct tlv_parsed *tp, struct msgb *in, void *ctx) { + int rc; + if (!TLVP_PRES_LEN(tp, CBSP_IEI_FAILURE_LIST, 5) || !TLVP_PRES_LEN(tp, CBSP_IEI_BCAST_MSG_TYPE, 1)) { osmo_cbsp_errstr = "missing/short mandatory IE"; @@ -1125,9 +1216,11 @@ static int cbsp_dec_failure(struct osmo_cbsp_failure *out, const struct tlv_pars } INIT_LLIST_HEAD(&out->fail_list); - cbsp_decode_fail_list(&out->fail_list, ctx, - TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), - TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + rc = cbsp_decode_fail_list(&out->fail_list, ctx, + TLVP_VAL(tp, CBSP_IEI_FAILURE_LIST), + TLVP_LEN(tp, CBSP_IEI_FAILURE_LIST)); + if (rc < 0) + return rc; out->bcast_msg_type = *TLVP_VAL(tp, CBSP_IEI_BCAST_MSG_TYPE); return 0; @@ -1168,6 +1261,7 @@ static int cbsp_dec_error_ind(struct osmo_cbsp_error_ind *out, const struct tlv_ * \returns callee-allocated decoded representation of CBSP message; NULL on error */ struct osmo_cbsp_decoded *osmo_cbsp_decode(void *ctx, struct msgb *in) { + OSMO_ASSERT(in->l1h != NULL && in->l2h != NULL); struct osmo_cbsp_decoded *out = talloc_zero(ctx, struct osmo_cbsp_decoded); const struct cbsp_header *h = msgb_l1(in); struct tlv_parsed tp[16]; /* max. number of pages in a given CBS message */ @@ -1433,6 +1527,10 @@ int osmo_cbsp_recv_buffered(void *ctx, int fd, struct msgb **rmsg, struct msgb * needed = len - msgb_l2len(msg); if (needed > 0) { + if (needed > msgb_tailroom(msg)) { + rc = -ENOMEM; + goto discard_msg; + } rc = recv(fd, msg->tail, needed, 0); if (rc == 0) goto discard_msg; @@ -1456,12 +1554,7 @@ int osmo_cbsp_recv_buffered(void *ctx, int fd, struct msgb **rmsg, struct msgb * } } /* else: complete message received */ - rc = msgb_l2len(msg); - if (rc == 0) { - /* drop empty message */ - rc = -EAGAIN; - goto discard_msg; - } + rc = msgb_length(msg); if (tmp_msg) *tmp_msg = NULL; *rmsg = msg; @@ -1474,4 +1567,45 @@ discard_msg: return rc; } +/*! call-back function to segment the data at message boundaries. + * Returns the size of the next message. If it returns -EAGAIN or a value larger than msgb_length() (message + * is incomplete), the caller (e.g. osmo_io) has to wait for more data to be read. */ +int osmo_cbsp_segmentation_cb(struct msgb *msg) +{ + const struct cbsp_header *h; + int len; + + if (msgb_length(msg) < sizeof(*h)) + return -EAGAIN; + + h = (const struct cbsp_header *) msg->data; + msg->l1h = msg->data; + msg->l2h = msg->data + sizeof(*h); + /* then read the length as specified in the header */ + len = h->len[0] << 16 | h->len[1] << 8 | h->len[2]; + + return sizeof(*h) + len; +} + +/*! value_string[] for enum osmo_cbsp_cause. */ +const struct value_string osmo_cbsp_cause_names[] = { + { OSMO_CBSP_CAUSE_PARAM_NOT_RECOGNISED, "Parameter-not-recognised" }, + { OSMO_CBSP_CAUSE_PARAM_VALUE_INVALID, "Parameter-value-invalid" }, + { OSMO_CBSP_CAUSE_MSG_REF_NOT_IDENTIFIED, "Message-reference-not-identified" }, + { OSMO_CBSP_CAUSE_CELL_ID_NOT_VALID, "Cell-identity-not-valid" }, + { OSMO_CBSP_CAUSE_UNRECOGNISED_MESSAGE, "Unrecognised-message" }, + { OSMO_CBSP_CAUSE_MISSING_MANDATORY_ELEMENT, "Missing-mandatory-element" }, + { OSMO_CBSP_CAUSE_BSC_CAPACITY_EXCEEDED, "BSC-capacity-exceeded" }, + { OSMO_CBSP_CAUSE_CELL_MEMORY_EXCEEDED, "Cell-memory-exceeded" }, + { OSMO_CBSP_CAUSE_BSC_MEMORY_EXCEEDED, "BSC-memory-exceeded" }, + { OSMO_CBSP_CAUSE_CELL_BROADCAST_NOT_SUPPORTED, "Cell-broadcast-not-supported" }, + { OSMO_CBSP_CAUSE_CELL_BROADCAST_NOT_OPERATIONAL, "Cell-broadcast-not-operational" }, + { OSMO_CBSP_CAUSE_INCOMPATIBLE_DRX_PARAM, "Incompatible-DRX-parameter:"}, + { OSMO_CBSP_CAUSE_EXT_CHAN_NOT_SUPPORTED, "Extended-channel-not-supported"}, + { OSMO_CBSP_CAUSE_MSG_REF_ALREADY_USED, "Message-reference-already-used"}, + { OSMO_CBSP_CAUSE_UNSPECIFIED_ERROR, "Unspecified-error"}, + { OSMO_CBSP_CAUSE_LAI_OR_LAC_NOT_VALID, "LAI-or-LAC-not-valid"}, + { 0, NULL } +}; + #endif /* HAVE_SYS_SOCKET_H */ diff --git a/src/gsm/comp128.c b/src/gsm/comp128.c index b28a8437..e252b04c 100644 --- a/src/gsm/comp128.c +++ b/src/gsm/comp128.c @@ -58,10 +58,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <string.h> diff --git a/src/gsm/comp128v23.c b/src/gsm/comp128v23.c index 550f6a49..0947017c 100644 --- a/src/gsm/comp128v23.c +++ b/src/gsm/comp128v23.c @@ -23,10 +23,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> diff --git a/src/gsm/gad.c b/src/gsm/gad.c new file mode 100644 index 00000000..23b907ac --- /dev/null +++ b/src/gsm/gad.c @@ -0,0 +1,491 @@ +/* 3GPP TS 23.032 GAD: Universal Geographical Area Description */ +/* + * (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * All Rights Reserved + * + * Author: Neels Hofmeyr <neels@hofmeyr.de> + * + * 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 <http://www.gnu.org/licenses/>. + * + */ + +#include <errno.h> +#include <inttypes.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/utils.h> +#include <osmocom/gsm/gad.h> + +/*! \addtogroup gad + * @{ + * \file gad.c + * Message encoding and decoding for 3GPP TS 23.032 GAD: Universal Geographical Area Description. + */ + +const struct value_string osmo_gad_type_names[] = { + { GAD_TYPE_ELL_POINT, "Ellipsoid-point" }, + { GAD_TYPE_ELL_POINT_UNC_CIRCLE, "Ellipsoid-point-with-uncertainty-circle" }, + { GAD_TYPE_ELL_POINT_UNC_ELLIPSE, "Ellipsoid-point-with-uncertainty-ellipse" }, + { GAD_TYPE_POLYGON, "Polygon" }, + { GAD_TYPE_ELL_POINT_ALT, "Ellipsoid-point-with-altitude" }, + { GAD_TYPE_ELL_POINT_ALT_UNC_ELL, "Ellipsoid-point-with-altitude-and-uncertainty-ellipsoid" }, + { GAD_TYPE_ELL_ARC, "Ellipsoid-arc" }, + { GAD_TYPE_HA_ELL_POINT_UNC_ELLIPSE, "High-accuracy-ellipsoid-point-with-uncertainty-ellipse" }, + { GAD_TYPE_HA_ELL_POINT_ALT_UNC_ELL, "High-accuracy-ellipsoid-point-with-altitude-and-uncertainty-ellipsoid" }, + {} +}; + +/*! Encode a latitude value according to 3GPP TS 23.032. + * Useful to clamp a latitude to an actually encodable accuracy: + * set_lat = osmo_gad_dec_lat(osmo_gad_enc_lat(orig_lat)); + * \param[in] deg_1e6 Latitude in micro degrees (degrees * 1e6), -90'000'000 (S) .. 90'000'000 (N). + * \returns encoded latitude in host-byte-order (24bit). + */ +uint32_t osmo_gad_enc_lat(int32_t deg_1e6) +{ + /* N <= ((2**23)/90)*X < N+1 + * N: encoded latitude + * X: latitude in degrees + */ + int32_t sign = 0; + int64_t x; + deg_1e6 = OSMO_MAX(-90000000, OSMO_MIN(90000000, deg_1e6)); + if (deg_1e6 < 0) { + sign = 1 << 23; + deg_1e6 = -deg_1e6; + } + x = deg_1e6; + x <<= 23; + x += (1 << 23) - 1; + x /= 90 * 1000000; + return sign | (x & 0x7fffff); +} + +/*! Decode a latitude value according to 3GPP TS 23.032. + * Useful to clamp a latitude to an actually encodable accuracy: + * set_lat = osmo_gad_dec_lat(osmo_gad_enc_lat(orig_lat)); + * \param[in] lat encoded latitude in host-byte-order (24bit). + * \returns decoded latitude in micro degrees (degrees * 1e6), -90'000'000 (S) .. 90'000'000 (N). + */ +int32_t osmo_gad_dec_lat(uint32_t lat) +{ + int64_t sign = 1; + int64_t x; + if (lat & 0x800000) { + sign = -1; + lat &= 0x7fffff; + } + x = lat; + x *= 90 * 1000000; + x >>= 23; + x *= sign; + return x; +} + +/*! Encode a longitude value according to 3GPP TS 23.032. + * Useful to clamp a longitude to an actually encodable accuracy: + * set_lon = osmo_gad_dec_lon(osmo_gad_enc_lon(orig_lon)); + * \param[in] deg_1e6 Longitude in micro degrees (degrees * 1e6), -180'000'000 (W) .. 180'000'000 (E). + * \returns encoded longitude in host-byte-order (24bit). + */ +uint32_t osmo_gad_enc_lon(int32_t deg_1e6) +{ + /* -180 .. 180 degrees mapped to a signed 24 bit integer. + * N <= ((2**24)/360) * X < N+1 + * N: encoded longitude + * X: longitude in degrees + */ + int64_t x; + deg_1e6 = OSMO_MAX(-180000000, OSMO_MIN(180000000, deg_1e6)); + x = deg_1e6; + x *= (1 << 24); + if (deg_1e6 >= 0) + x += (1 << 24) - 1; + else + x -= (1 << 24) - 1; + x /= 360 * 1000000; + return (uint32_t)(x & 0xffffff); +} + +/*! Decode a longitude value according to 3GPP TS 23.032. + * Normally, encoding and decoding is done via osmo_gad_enc() and osmo_gad_dec() for entire PDUs. But calling this + * directly can be useful to clamp a longitude to an actually encodable accuracy: + * int32_t set_lon = osmo_gad_dec_lon(osmo_gad_enc_lon(orig_lon)); + * \param[in] lon Encoded longitude. + * \returns Longitude in micro degrees (degrees * 1e6), -180'000'000 (W) .. 180'000'000 (E). + */ +int32_t osmo_gad_dec_lon(uint32_t lon) +{ + /* -180 .. 180 degrees mapped to a signed 24 bit integer. + * N <= ((2**24)/360) * X < N+1 + * N: encoded longitude + * X: longitude in degrees + */ + int32_t slon; + int64_t x; + if (lon & 0x800000) { + /* make the 24bit negative number to a 32bit negative number */ + slon = lon | 0xff000000; + } else { + slon = lon; + } + x = slon; + x *= 360 * 1000000; + x /= (1 << 24); + return x; +} + +/* + * r = C((1+x)**K - 1) + * C = 10, x = 0.1 + * + * def r(k): + * return 10.*(((1+0.1)**k) -1 ) + * for k in range(128): + * print('%d,' % (r(k) * 1000.)) + */ +static uint32_t table_uncertainty_1e3[128] = { + 0, 1000, 2100, 3310, 4641, 6105, 7715, 9487, 11435, 13579, 15937, 18531, 21384, 24522, 27974, 31772, 35949, + 40544, 45599, 51159, 57274, 64002, 71402, 79543, 88497, 98347, 109181, 121099, 134209, 148630, 164494, 181943, + 201137, 222251, 245476, 271024, 299126, 330039, 364043, 401447, 442592, 487851, 537636, 592400, 652640, 718904, + 791795, 871974, 960172, 1057189, 1163908, 1281299, 1410429, 1552472, 1708719, 1880591, 2069650, 2277615, + 2506377, 2758014, 3034816, 3339298, 3674227, 4042650, 4447915, 4893707, 5384077, 5923485, 6516834, 7169517, + 7887469, 8677216, 9545938, 10501531, 11552685, 12708953, 13980849, 15379933, 16918927, 18611820, 20474002, + 22522402, 24775642, 27254206, 29980627, 32979690, 36278659, 39907525, 43899277, 48290205, 53120226, 58433248, + 64277573, 70706330, 77777964, 85556760, 94113436, 103525780, 113879358, 125268293, 137796123, 151576735, + 166735409, 183409950, 201751945, 221928139, 244121953, 268535149, 295389664, 324929630, 357423593, 393166952, + 432484648, 475734112, 523308524, 575640376, 633205414, 696526955, 766180651, 842799716, 927080688, 1019789756, + 1121769732, 1233947705, 1357343476, 1493078824, 1642387706, 1806627477, +}; + +/*! Decode an uncertainty circle value according to 3GPP TS 23.032. + * Useful to clamp a value to an actually encodable accuracy: + * set_unc = osmo_gad_dec_unc(osmo_gad_enc_unc(orig_unc)); + * \param[in] unc Encoded uncertainty value. + * \returns Uncertainty value in millimeters. + */ +uint32_t osmo_gad_dec_unc(uint8_t unc) +{ + return table_uncertainty_1e3[unc & 0x7f]; +} + +/*! Encode an uncertainty circle value according to 3GPP TS 23.032. + * Normally, encoding and decoding is done via osmo_gad_enc() and osmo_gad_dec() for entire PDUs. But calling this + * directly can be useful to clamp a value to an actually encodable accuracy: + * uint32_t set_unc = osmo_gad_dec_unc(osmo_gad_enc_unc(orig_unc)); + * \param[in] mm Uncertainty value in millimeters. + * \returns Encoded uncertainty value. + */ +uint8_t osmo_gad_enc_unc(uint32_t mm) +{ + uint8_t unc; + for (unc = 0; unc < ARRAY_SIZE(table_uncertainty_1e3); unc++) { + if (table_uncertainty_1e3[unc] > mm) + return unc - 1; + } + return 127; +} + +/* So far we don't encode a high-accuracy uncertainty anywhere, so these static items would flag as compiler warnings + * for unused items. As soon as any HA items get used, remove this ifdef. */ +#ifdef GAD_FUTURE + +/* + * r = C((1+x)**K - 1) + * C = 0.3, x = 0.02 + * + * def r(k): + * return 0.3*(((1+0.02)**k) -1 ) + * for k in range(256): + * print('%d,' % (r(k) * 1000.)) + */ +static uint32_t table_ha_uncertainty_1e3[256] = { + 0, 6, 12, 18, 24, 31, 37, 44, 51, 58, 65, 73, 80, 88, 95, 103, 111, 120, 128, 137, 145, 154, 163, 173, 182, 192, + 202, 212, 222, 232, 243, 254, 265, 276, 288, 299, 311, 324, 336, 349, 362, 375, 389, 402, 417, 431, 445, 460, + 476, 491, 507, 523, 540, 556, 574, 591, 609, 627, 646, 665, 684, 703, 724, 744, 765, 786, 808, 830, 853, 876, + 899, 923, 948, 973, 998, 1024, 1051, 1078, 1105, 1133, 1162, 1191, 1221, 1252, 1283, 1314, 1347, 1380, 1413, + 1447, 1482, 1518, 1554, 1592, 1629, 1668, 1707, 1748, 1788, 1830, 1873, 1916, 1961, 2006, 2052, 2099, 2147, + 2196, 2246, 2297, 2349, 2402, 2456, 2511, 2567, 2625, 2683, 2743, 2804, 2866, 2929, 2994, 3060, 3127, 3195, + 3265, 3336, 3409, 3483, 3559, 3636, 3715, 3795, 3877, 3961, 4046, 4133, 4222, 4312, 4404, 4498, 4594, 4692, + 4792, 4894, 4998, 5104, 5212, 5322, 5435, 5549, 5666, 5786, 5907, 6032, 6158, 6287, 6419, 6554, 6691, 6830, + 6973, 7119, 7267, 7418, 7573, 7730, 7891, 8055, 8222, 8392, 8566, 8743, 8924, 9109, 9297, 9489, 9685, 9884, + 10088, 10296, 10508, 10724, 10944, 11169, 11399, 11633, 11871, 12115, 12363, 12616, 12875, 13138, 13407, 13681, + 13961, 14246, 14537, 14834, 15136, 15445, 15760, 16081, 16409, 16743, 17084, 17431, 17786, 18148, 18517, 18893, + 19277, 19669, 20068, 20475, 20891, 21315, 21747, 22188, 22638, 23096, 23564, 24042, 24529, 25025, 25532, 26048, + 26575, 27113, 27661, 28220, 28791, 29372, 29966, 30571, 31189, 31818, 32461, 33116, 33784, 34466, 35161, 35871, + 36594, 37332, 38085, 38852, 39635, 40434, 41249, 42080, 42927, 43792, 44674, 45573, 46491, +}; + +static uint32_t osmo_gad_dec_ha_unc(uint8_t unc) +{ + return table_uncertainty_1e3[unc]; +} + +static uint8_t osmo_gad_enc_ha_unc(uint32_t mm) +{ + uint8_t unc; + for (unc = 0; unc < ARRAY_SIZE(table_ha_uncertainty_1e3); unc++) { + if (table_uncertainty_1e3[unc] > mm) + return unc - 1; + } + return 255; +} + +#endif /* GAD_FUTURE */ + +/* Return error code, and, if required, allocate and populate struct osmo_gad_err. */ +#define DEC_ERR(RC, TYPE, fmt, args...) do { \ + if (err) { \ + *err = talloc_zero(err_ctx, struct osmo_gad_err); \ + **err = (struct osmo_gad_err){ \ + .rc = (RC), \ + .type = (TYPE), \ + .logmsg = talloc_asprintf(*err, "Error decoding GAD%s%s: " fmt, \ + ((int)(TYPE)) >= 0 ? " " : "", \ + ((int)(TYPE)) >= 0 ? osmo_gad_type_name(TYPE) : "", ##args), \ + }; \ + } \ + return RC; \ + } while(0) + +static int osmo_gad_enc_ell_point_unc_circle(struct gad_raw_ell_point_unc_circle *raw, const struct osmo_gad_ell_point_unc_circle *v) +{ + if (v->lat < -90000000 || v->lat > 90000000) + return -EINVAL; + if (v->lon < -180000000 || v->lon > 180000000) + return -EINVAL; + *raw = (struct gad_raw_ell_point_unc_circle){ + .h = { .type = GAD_TYPE_ELL_POINT_UNC_CIRCLE }, + .unc = osmo_gad_enc_unc(v->unc), + }; + osmo_store32be_ext(osmo_gad_enc_lat(v->lat), raw->lat, 3); + osmo_store32be_ext(osmo_gad_enc_lon(v->lon), raw->lon, 3); + return sizeof(raw); +} + +static int osmo_gad_dec_ell_point_unc_circle(struct osmo_gad_ell_point_unc_circle *v, + struct osmo_gad_err **err, void *err_ctx, + const struct gad_raw_ell_point_unc_circle *raw) +{ + /* Load 24bit big endian */ + v->lat = osmo_gad_dec_lat(osmo_load32be_ext_2(raw->lat, 3)); + v->lon = osmo_gad_dec_lon(osmo_load32be_ext_2(raw->lon, 3)); + + if (raw->spare2) + DEC_ERR(-EINVAL, raw->h.type, "Bit 8 of Uncertainty code should be zero"); + + v->unc = osmo_gad_dec_unc(raw->unc); + return 0; +} + +static int osmo_gad_raw_len(const union gad_raw *gad_raw) +{ + switch (gad_raw->h.type) { + case GAD_TYPE_ELL_POINT: + return sizeof(gad_raw->ell_point); + case GAD_TYPE_ELL_POINT_UNC_CIRCLE: + return sizeof(gad_raw->ell_point_unc_circle); + case GAD_TYPE_ELL_POINT_UNC_ELLIPSE: + return sizeof(gad_raw->ell_point_unc_ellipse); + case GAD_TYPE_POLYGON: + if (gad_raw->polygon.h.num_points < 3) + return -EINVAL; + return sizeof(gad_raw->polygon.h) + + gad_raw->polygon.h.num_points * sizeof(gad_raw->polygon.point[0]); + case GAD_TYPE_ELL_POINT_ALT: + return sizeof(gad_raw->ell_point_alt); + case GAD_TYPE_ELL_POINT_ALT_UNC_ELL: + return sizeof(gad_raw->ell_point_alt_unc_ell); + case GAD_TYPE_ELL_ARC: + return sizeof(gad_raw->ell_arc); + case GAD_TYPE_HA_ELL_POINT_UNC_ELLIPSE: + return sizeof(gad_raw->ha_ell_point_unc_ell); + case GAD_TYPE_HA_ELL_POINT_ALT_UNC_ELL: + return sizeof(gad_raw->ha_ell_point_alt_unc_ell); + default: + return -ENOTSUP; + } +} + +/*! Append a GAD PDU to the msgb. + * Write the correct number of bytes depending on the GAD type and possibly on variable length attributes. + * \param[out] msg Append to this msgb. + * \param[in] gad_raw GAD data to write. + * \returns number of bytes appended to msgb, or negative on failure. + */ +int osmo_gad_raw_write(struct msgb *msg, const union gad_raw *gad_raw) +{ + int len; + uint8_t *dst; + len = osmo_gad_raw_len(gad_raw); + if (len < 0) + return len; + dst = msgb_put(msg, len); + memcpy(dst, (void*)gad_raw, len); + return len; +} + +/*! Read a GAD PDU and validate structure. + * Memcpy from data to gad_raw struct, and validate correct length depending on the GAD type and possibly on variable + * length attributes. + * \param[out] gad_raw Copy GAD PDU here. + * \param[out] err Returned pointer to error info, dynamically allocated; NULL to not return any. + * \param[in] err_ctx Talloc context to allocate err from, if required. + * \param[in] data Encoded GAD bytes buffer. + * \param[in] len Length of data in bytes. + * \returns 0 on success, negative on error. If returning negative and err was non-NULL, *err is guaranteed to point to + * an allocated struct osmo_gad_err. + */ +int osmo_gad_raw_read(union gad_raw *gad_raw, struct osmo_gad_err **err, void *err_ctx, const uint8_t *data, uint8_t len) +{ + int gad_len; + const union gad_raw *src; + if (err) + *err = NULL; + if (len < sizeof(src->h)) + DEC_ERR(-EINVAL, -1, "GAD data too short for header (%u bytes)", len); + + src = (void*)data; + gad_len = osmo_gad_raw_len(src); + if (gad_len < 0) + DEC_ERR(-EINVAL, src->h.type, "GAD data invalid (rc=%d)", gad_len); + if (gad_len != len) + DEC_ERR(-EINVAL, src->h.type, "GAD data with unexpected length: expected %d bytes, got %u", + gad_len, len); + + memcpy((void*)gad_raw, data, gad_len); + return 0; +} + +/*! Write GAD values with consistent units to raw GAD PDU representation. + * \param[out] gad_raw Write to this buffer. + * \param[in] gad GAD values to encode. + * \returns number of bytes written, or negative on failure. + */ +int osmo_gad_enc(union gad_raw *gad_raw, const struct osmo_gad *gad) +{ + switch (gad->type) { + case GAD_TYPE_ELL_POINT_UNC_CIRCLE: + return osmo_gad_enc_ell_point_unc_circle(&gad_raw->ell_point_unc_circle, &gad->ell_point_unc_circle); + default: + return -ENOTSUP; + } +} + +/*! Decode GAD raw PDU to values with consistent units. + * \param[out] gad Decoded GAD values are written here. + * \param[out] err Returned pointer to error info, dynamically allocated; NULL to not return any. + * \param[in] err_ctx Talloc context to allocate err from, if required. + * \param[in] raw Raw GAD data in network-byte-order. + * \returns 0 on success, negative on error. If returning negative and err was non-NULL, *err is guaranteed to point to + * an allocated struct osmo_gad_err. + */ +int osmo_gad_dec(struct osmo_gad *gad, struct osmo_gad_err **err, void *err_ctx, const union gad_raw *raw) +{ + *gad = (struct osmo_gad){ + .type = raw->h.type, + }; + switch (raw->h.type) { + case GAD_TYPE_ELL_POINT_UNC_CIRCLE: + return osmo_gad_dec_ell_point_unc_circle(&gad->ell_point_unc_circle, err, err_ctx, + &raw->ell_point_unc_circle); + default: + DEC_ERR(-ENOTSUP, raw->h.type, "unsupported GAD type"); + } +} + +/*! Return a human readable representation of a raw GAD PDU. + * Convert to GAD values and feed the result to osmo_gad_to_str_buf(). + * \param[out] buf Buffer to write string to. + * \param[in] buflen sizeof(buf). + * \param[in] gad Location data. + * \returns number of chars that would be written, like snprintf(). + */ +int osmo_gad_raw_to_str_buf(char *buf, size_t buflen, const union gad_raw *raw) +{ + struct osmo_gad gad; + if (osmo_gad_dec(&gad, NULL, NULL, raw)) { + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + OSMO_STRBUF_PRINTF(sb, "invalid"); + return sb.chars_needed; + } + return osmo_gad_to_str_buf(buf, buflen, &gad); +} + +/*! Return a human readable representation of a raw GAD PDU. + * Convert to GAD values and feed the result to osmo_gad_to_str_buf(). + * \param[in] ctx Talloc ctx to allocate string buffer from. + * \param[in] raw GAD data in network-byte-order. + * \returns resulting string, dynamically allocated. + */ +char *osmo_gad_raw_to_str_c(void *ctx, const union gad_raw *raw) +{ + OSMO_NAME_C_IMPL(ctx, 128, "ERROR", osmo_gad_raw_to_str_buf, raw) +} + +/*! Return a human readable representation of GAD (location estimate) values. + * \param[out] buf Buffer to write string to. + * \param[in] buflen sizeof(buf). + * \param[in] gad Location data. + * \returns number of chars that would be written, like snprintf(). + */ +int osmo_gad_to_str_buf(char *buf, size_t buflen, const struct osmo_gad *gad) +{ + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + + if (!gad) { + OSMO_STRBUF_PRINTF(sb, "null"); + return sb.chars_needed; + } + + OSMO_STRBUF_PRINTF(sb, "%s{", osmo_gad_type_name(gad->type)); + + switch (gad->type) { + case GAD_TYPE_ELL_POINT: + OSMO_STRBUF_PRINTF(sb, "lat="); + OSMO_STRBUF_APPEND(sb, osmo_int_to_float_str_buf, gad->ell_point.lat, 6); + OSMO_STRBUF_PRINTF(sb, ",lon="); + OSMO_STRBUF_APPEND(sb, osmo_int_to_float_str_buf, gad->ell_point.lon, 6); + break; + + case GAD_TYPE_ELL_POINT_UNC_CIRCLE: + OSMO_STRBUF_PRINTF(sb, "lat="); + OSMO_STRBUF_APPEND(sb, osmo_int_to_float_str_buf, gad->ell_point_unc_circle.lat, 6); + OSMO_STRBUF_PRINTF(sb, ",lon="); + OSMO_STRBUF_APPEND(sb, osmo_int_to_float_str_buf, gad->ell_point_unc_circle.lon, 6); + OSMO_STRBUF_PRINTF(sb, ",unc="); + OSMO_STRBUF_APPEND(sb, osmo_int_to_float_str_buf, gad->ell_point_unc_circle.unc, 3); + OSMO_STRBUF_PRINTF(sb, "m"); + break; + + default: + OSMO_STRBUF_PRINTF(sb, "to-str-not-implemented"); + break; + } + + OSMO_STRBUF_PRINTF(sb, "}"); + return sb.chars_needed; +} + +/*! Return a human readable representation of GAD (location estimate) values. + * \param[in] ctx Talloc ctx to allocate string buffer from. + * \param[in] val Value to convert to float. + * \returns resulting string, dynamically allocated. + */ +char *osmo_gad_to_str_c(void *ctx, const struct osmo_gad *gad) +{ + OSMO_NAME_C_IMPL(ctx, 128, "ERROR", osmo_gad_to_str_buf, gad) +} + +/*! @} */ diff --git a/src/gsm/gea.c b/src/gsm/gea.c index 5756bb08..aedc898e 100644 --- a/src/gsm/gea.c +++ b/src/gsm/gea.c @@ -12,10 +12,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <osmocom/core/bits.h> diff --git a/src/gsm/gprs_cipher_core.c b/src/gsm/gprs_cipher_core.c index 7f2b1a50..97e581db 100644 --- a/src/gsm/gprs_cipher_core.c +++ b/src/gsm/gprs_cipher_core.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include "config.h" diff --git a/src/gsm/gprs_gea.c b/src/gsm/gprs_gea.c index 73147886..eae1cf12 100644 --- a/src/gsm/gprs_gea.c +++ b/src/gsm/gprs_gea.c @@ -16,10 +16,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <osmocom/crypt/gprs_cipher.h> diff --git a/src/gsm/gprs_rlc.c b/src/gsm/gprs_rlc.c index bdfc8eac..4f02a7a9 100644 --- a/src/gsm/gprs_rlc.c +++ b/src/gsm/gprs_rlc.c @@ -13,9 +13,8 @@ #include <string.h> #include <osmocom/core/utils.h> -#include <osmocom/gprs/gprs_rlc.h> #include <osmocom/coding/gsm0503_coding.h> -#include <osmocom/gprs/protocol/gsm_04_60.h> +#include <osmocom/gsm/protocol/gsm_44_060.h> #define EGPRS_CPS_TYPE1_TBL_SZ 29 #define EGPRS_CPS_TYPE2_TBL_SZ 8 diff --git a/src/gsm/gsm0341.c b/src/gsm/gsm0341.c index 89f5de3f..a6238b36 100644 --- a/src/gsm/gsm0341.c +++ b/src/gsm/gsm0341.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ diff --git a/src/gsm/gsm0411_smc.c b/src/gsm/gsm0411_smc.c index 50d0f3ec..a7d5e11c 100644 --- a/src/gsm/gsm0411_smc.c +++ b/src/gsm/gsm0411_smc.c @@ -51,8 +51,9 @@ * */ -#include <string.h> +#include <sys/types.h> #include <inttypes.h> +#include <string.h> #include <errno.h> #include <osmocom/core/msgb.h> #include <osmocom/core/logging.h> diff --git a/src/gsm/gsm0411_smr.c b/src/gsm/gsm0411_smr.c index 21d28c51..03cf0051 100644 --- a/src/gsm/gsm0411_smr.c +++ b/src/gsm/gsm0411_smr.c @@ -49,9 +49,10 @@ */ +#include <sys/types.h> +#include <inttypes.h> #include <string.h> #include <errno.h> -#include <inttypes.h> #include <osmocom/core/msgb.h> #include <osmocom/core/logging.h> #include <osmocom/core/timer.h> @@ -130,7 +131,7 @@ const struct value_string gsm411_rp_cause_strs[] = { { GSM411_RP_CAUSE_INV_TRANS_REF, "Invalid Transaction Reference" }, { GSM411_RP_CAUSE_SEMANT_INC_MSG, "Semantically Incorrect Message" }, { GSM411_RP_CAUSE_INV_MAND_INF, "Invalid Mandatory Information" }, - { GSM411_RP_CAUSE_MSGTYPE_NOTEXIST, "Message Type non-existant" }, + { GSM411_RP_CAUSE_MSGTYPE_NOTEXIST, "Message Type non-existent" }, { GSM411_RP_CAUSE_MSG_INCOMP_STATE, "Message incompatible with protocol state" }, { GSM411_RP_CAUSE_IE_NOTEXIST, "Information Element not existing" }, { GSM411_RP_CAUSE_PROTOCOL_ERR, "Protocol Error" }, diff --git a/src/gsm/gsm0411_utils.c b/src/gsm/gsm0411_utils.c index ccefe546..470b017f 100644 --- a/src/gsm/gsm0411_utils.c +++ b/src/gsm/gsm0411_utils.c @@ -27,7 +27,7 @@ * */ -#include "../../config.h" +#include "config.h" #include <time.h> #include <string.h> @@ -293,7 +293,7 @@ enum sms_alphabet gsm338_get_sms_alphabet(uint8_t dcs) * \param[in] type GSM340_TYPE_* * \param[in] plan Numbering Plan * \param[in] number string containing number - * \reurns number of bytes of \a oa that have been used */ + * \returns number of bytes of \a oa that have been used */ int gsm340_gen_oa(uint8_t *oa, unsigned int oa_len, uint8_t type, uint8_t plan, const char *number) { @@ -348,7 +348,7 @@ int gsm411_push_rp_header(struct msgb *msg, uint8_t rp_msg_type, * \param[in] proto Protocol * \param[in] trans Transaction * \param[in] msg_type Message Type - * \retrns 0 */ + * \returns 0 */ int gsm411_push_cp_header(struct msgb *msg, uint8_t proto, uint8_t trans, uint8_t msg_type) { diff --git a/src/gsm/gsm0480.c b/src/gsm/gsm0480.c index 3ae591a6..7a7f71f0 100644 --- a/src/gsm/gsm0480.c +++ b/src/gsm/gsm0480.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <osmocom/gsm/gsm48.h> @@ -821,6 +817,7 @@ struct msgb *gsm0480_gen_ussd_resp_7bit(uint8_t invoke_id, const char *text) * not only the FACILITY value, but the full L3 message including message header * and FACILITY IE Tag+Length. */ +OSMO_DEPRECATED("Use gsm0480_gen_ussd_resp_7bit() instead") struct msgb *gsm0480_create_ussd_resp(uint8_t invoke_id, uint8_t trans_id, const char *text) { struct msgb *msg; diff --git a/src/gsm/gsm0502.c b/src/gsm/gsm0502.c index 1a71e617..e4a761da 100644 --- a/src/gsm/gsm0502.c +++ b/src/gsm/gsm0502.c @@ -2,6 +2,8 @@ * Paging helper code */ /* * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2010 by Sylvain Munaut <tnt@246tNt.com> + * * All Rights Reserved * * SPDX-License-Identifier: GPL-2.0+ @@ -32,7 +34,7 @@ #include <inttypes.h> unsigned int -gsm0502_calc_paging_group(struct gsm48_control_channel_descr *chan_desc, uint64_t imsi) +gsm0502_calc_paging_group(const struct gsm48_control_channel_descr *chan_desc, uint64_t imsi) { int ccch_conf; int bs_cc_chans; @@ -177,7 +179,6 @@ uint32_t gsm0502_fn_remap(uint32_t fn, enum gsm0502_fn_remap_channel channel) uint8_t fn_cycle; uint8_t i; int sub = -1; - uint32_t fn_map; struct fn_remap_table *table; OSMO_ASSERT(channel < ARRAY_SIZE(fn_remap_table_ptr)); @@ -193,11 +194,104 @@ uint32_t gsm0502_fn_remap(uint32_t fn, enum gsm0502_fn_remap_channel channel) } if (sub == -1) { - LOGP(DLGLOBAL, LOGL_ERROR, "could not remap frame number!, fn=%"PRIu32"\n", fn); + LOGP(DLGLOBAL, LOGL_ERROR, "could not remap frame number!, fn=%" PRIu32 "\n", fn); return fn; } - fn_map = (fn + GSM_MAX_FN - sub) % GSM_MAX_FN; + return GSM_TDMA_FN_SUB(fn, sub); +} + +/* Magic numbers (RNTABLE) for pseudo-random hopping sequence generation. */ +static const uint8_t rn_table[114] = { + 48, 98, 63, 1, 36, 95, 78, 102, 94, 73, + 0, 64, 25, 81, 76, 59, 124, 23, 104, 100, + 101, 47, 118, 85, 18, 56, 96, 86, 54, 2, + 80, 34, 127, 13, 6, 89, 57, 103, 12, 74, + 55, 111, 75, 38, 109, 71, 112, 29, 11, 88, + 87, 19, 3, 68, 110, 26, 33, 31, 8, 45, + 82, 58, 40, 107, 32, 5, 106, 92, 62, 67, + 77, 108, 122, 37, 60, 66, 121, 42, 51, 126, + 117, 114, 4, 90, 43, 52, 53, 113, 120, 72, + 16, 49, 7, 79, 119, 61, 22, 84, 9, 97, + 91, 15, 21, 24, 46, 39, 93, 105, 65, 70, + 125, 99, 17, 123, +}; - return fn_map; +/*! Hopping sequence generation as per 3GPP TS 45.002, section 6.2.3. + * \param[in] t GSM time (TDMA frame number, T1/T2/T3). + * \param[in] hsn Hopping Sequence Number. + * \param[in] maio Mobile Allocation Index Offset. + * \param[in] n number of entries in mobile allocation (arfcn table). + * \param[in] ma array of ARFCNs (sorted in ascending order) + * representing the Mobile Allocation. + * \returns ARFCN to use for given input parameters at time 't' + * or Mobile Allocation Index if ma == NULL. + */ +uint16_t gsm0502_hop_seq_gen(const struct gsm_time *t, + uint8_t hsn, uint8_t maio, + size_t n, const uint16_t *ma) +{ + unsigned int mai; + + if (hsn == 0) { + /* cyclic hopping */ + mai = (t->fn + maio) % n; + } else { + /* pseudo random hopping */ + int m, mp, tp, s, pnm; + + pnm = (n >> 0) | (n >> 1) + | (n >> 2) | (n >> 3) + | (n >> 4) | (n >> 5) + | (n >> 6); + + m = t->t2 + rn_table[(hsn ^ (t->t1 & 63)) + t->t3]; + mp = m & pnm; + + if (mp < n) + s = mp; + else { + tp = t->t3 & pnm; + s = (mp + tp) % n; + } + + mai = (s + maio) % n; + } + + return ma ? ma[mai] : mai; +} + +#define CB_FCCH -1 +#define CB_SCH -2 +#define CB_BCCH -3 +#define CB_IDLE -4 + +/* Clause 7 Table 3 and Figure 8a */ +static const int ccch_block_table[51] = { + CB_FCCH, CB_SCH, /* 0..1 */ + CB_BCCH, CB_BCCH, CB_BCCH, CB_BCCH, /* 2..5: BCCH */ + 0, 0, 0, 0, /* 6..9: B0 */ + CB_FCCH, CB_SCH, /* 10..11 */ + 1, 1, 1, 1, /* 12..15: B1 */ + 2, 2, 2, 2, /* 16..19: B2 */ + CB_FCCH, CB_SCH, /* 20..21 */ + 3, 3, 3, 3, /* 22..25: B3 */ + 4, 4, 4, 4, /* 26..29: B4 */ + CB_FCCH, CB_SCH, /* 30..31 */ + 5, 5, 5, 5, /* 32..35: B5 */ + 6, 6, 6, 6, /* 36..39: B6 */ + CB_FCCH, CB_SCH, /* 40..41 */ + 7, 7, 7, 7, /* 42..45: B7 */ + 8, 8, 8, 8, /* 46..49: B8 */ + CB_IDLE /* 50: Idle */ +}; + +/*! Calculate CCCH block number from the given TDMA frame number. + * \param[in] fn TDMA frame number (of first or last burst). + * \returns CCCH block number 0..8 or a negative value, + * if the given frame number cannot carry CCCH. + */ +int gsm0502_fn2ccch_block(uint32_t fn) +{ + return ccch_block_table[fn % ARRAY_SIZE(ccch_block_table)]; } diff --git a/src/gsm/gsm0808.c b/src/gsm/gsm0808.c index 514d7f22..529dbdfe 100644 --- a/src/gsm/gsm0808.c +++ b/src/gsm/gsm0808.c @@ -15,17 +15,20 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ +#include "config.h" + +#include <string.h> + #include <osmocom/core/byteswap.h> +#include <osmocom/core/endian.h> #include <osmocom/gsm/gsm0808.h> +#include <osmocom/gsm/gsm0808_lcs.h> #include <osmocom/gsm/gsm0808_utils.h> #include <osmocom/gsm/protocol/gsm_08_08.h> #include <osmocom/gsm/gsm48.h> +#include <osmocom/gsm/gad.h> /*! \addtogroup gsm0808 * @{ @@ -34,6 +37,9 @@ * message generation/encoding. */ +/*! Char buffer to return strings from functions */ +static __thread char str_buff[512]; + /*! Create "Complete L3 Info" for AoIP, legacy implementation. * Instead use gsm0808_create_layer3_aoip2(), which is capable of three-digit MNC with leading zeros. * \param[in] msg_l3 msgb containing Layer 3 Message @@ -96,13 +102,19 @@ struct msgb *gsm0808_create_layer3_2(const struct msgb *msg_l3, const struct osm msgb_l3len(msg_l3), msg_l3->l3h); /* AoIP: add Codec List (BSS Supported) 3.2.2.103 */ - if (scl) - gsm0808_enc_speech_codec_list(msg, scl); + if (scl) { + if (gsm0808_enc_speech_codec_list2(msg, scl) < 0) + goto exit_free; + } /* push the bssmap header */ msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create "Complete L3 Info" for A, legacy implementation. @@ -220,22 +232,32 @@ struct msgb *gsm0808_create_clear_command2(uint8_t cause, bool csfb_ind) return msg; } -/*! Create BSSMAP Cipher Mode Command message +/*! Superseded by gsm0808_create_cipher2() to include Kc128. + * Create BSSMAP Cipher Mode Command message (without Kc128). * \param[in] ei Mandatory Encryption Information + * \param[in] kc128 optional kc128 key for A5/4 * \param[in] cipher_response_mode optional 1-byte Cipher Response Mode * \returns callee-allocated msgb with BSSMAP Cipher Mode Command message */ struct msgb *gsm0808_create_cipher(const struct gsm0808_encrypt_info *ei, const uint8_t *cipher_response_mode) { + struct gsm0808_cipher_mode_command cmc = { + .ei = *ei, + .cipher_response_mode_present = (cipher_response_mode != NULL), + .cipher_response_mode = (cipher_response_mode ? *cipher_response_mode : 0), + }; + return gsm0808_create_cipher2(&cmc); +} + +/*! Create BSSMAP Cipher Mode Command message. + * \param[in] cmc Information to encode. + * \returns callee-allocated msgb with BSSMAP Cipher Mode Command message */ +struct msgb *gsm0808_create_cipher2(const struct gsm0808_cipher_mode_command *cmc) +{ /* See also: 3GPP TS 48.008 3.2.1.30 CIPHER MODE COMMAND */ struct msgb *msg; - /* Mandatory emelent! */ - OSMO_ASSERT(ei); - - msg = - msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, - "cipher-mode-command"); + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "cipher-mode-command"); if (!msg) return NULL; @@ -243,12 +265,16 @@ struct msgb *gsm0808_create_cipher(const struct gsm0808_encrypt_info *ei, msgb_v_put(msg, BSS_MAP_MSG_CIPHER_MODE_CMD); /* Encryption Information 3.2.2.10 */ - gsm0808_enc_encrypt_info(msg, ei); + gsm0808_enc_encrypt_info(msg, &cmc->ei); /* Cipher Response Mode 3.2.2.34 */ - if (cipher_response_mode) + if (cmc->cipher_response_mode_present) msgb_tv_put(msg, GSM0808_IE_CIPHER_RESPONSE_MODE, - *cipher_response_mode); + cmc->cipher_response_mode); + + /* Kc128 3.2.2.109 */ + if (cmc->kc128_present) + gsm0808_enc_kc128(msg, cmc->kc128); /* pre-pend the header */ msg->l3h = @@ -277,8 +303,9 @@ struct msgb *gsm0808_create_cipher_complete(struct msgb *layer3, uint8_t alg_id) msgb_l3len(layer3), layer3->l3h); } - /* and the optional BSS message */ - msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, alg_id); + /* Optional Chosen Encryption Algorithm IE */ + if (alg_id > 0) + msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, alg_id); /* pre-pend the header */ msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); @@ -354,7 +381,7 @@ struct msgb *gsm0808_create_lcls_conn_ctrl(enum gsm0808_lcls_config config, if (config != GSM0808_LCLS_CFG_NA) msgb_tv_put(msg, GSM0808_IE_LCLS_CONFIG, config); if (control != GSM0808_LCLS_CSC_NA) - msgb_tv_put(msg, GSM0808_IE_LCLS_CONFIG, control); + msgb_tv_put(msg, GSM0808_IE_LCLS_CONN_STATUS_CTRL, control); msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; @@ -399,7 +426,7 @@ struct msgb *gsm0808_create_lcls_notification(enum gsm0808_lcls_status status, b /*! Create BSSMAP Classmark Request message * \returns callee-allocated msgb with BSSMAP Classmark Request message */ -struct msgb *gsm0808_create_classmark_request() +struct msgb *gsm0808_create_classmark_request(void) { struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "classmark-request"); @@ -438,8 +465,9 @@ struct msgb *gsm0808_create_classmark_update(const uint8_t *cm2, uint8_t cm2_len /*! Create BSSMAP SAPI N Reject message * \param[in] link_id Link Identifier + * \param[in] cause BSSAP Cause value (see 3GPP TS 48.008, section 3.2.2.5) * \returns callee-allocated msgb with BSSMAP SAPI N Reject message */ -struct msgb *gsm0808_create_sapi_reject(uint8_t link_id) +struct msgb *gsm0808_create_sapi_reject_cause(uint8_t link_id, uint16_t cause) { struct msgb *msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "bssmap: sapi 'n' reject"); @@ -447,14 +475,24 @@ struct msgb *gsm0808_create_sapi_reject(uint8_t link_id) return NULL; msgb_v_put(msg, BSS_MAP_MSG_SAPI_N_REJECT); - msgb_v_put(msg, link_id); - msgb_v_put(msg, GSM0808_CAUSE_BSS_NOT_EQUIPPED); + msgb_tv_put(msg, GSM0808_IE_DLCI, link_id); + gsm0808_enc_cause(msg, cause); msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; } +/*! Create BSSMAP SAPI N Reject message (with hard-coded cause "BSS not equipped"). + * DEPRECATED: use gsm0808_create_sapi_reject_cause() instead. + * \param[in] link_id Link Identifier + * \param[in] cause BSSAP Cause value (see 3GPP TS 48.008, section 3.2.2.5) + * \returns callee-allocated msgb with BSSMAP SAPI N Reject message */ +struct msgb *gsm0808_create_sapi_reject(uint8_t link_id) +{ + return gsm0808_create_sapi_reject_cause(link_id, GSM0808_CAUSE_BSS_NOT_EQUIPPED); +} + /*! Create BSSMAP Assignment Request message, 3GPP TS 48.008 §3.2.1.1. * This is identical to gsm0808_create_ass(), but adds KC and LCLS IEs. * \param[in] ct Channel Type @@ -474,8 +512,6 @@ struct msgb *gsm0808_create_ass2(const struct gsm0808_channel_type *ct, { /* See also: 3GPP TS 48.008 3.2.1.1 ASSIGNMENT REQUEST */ struct msgb *msg; - uint16_t cic_sw; - uint32_t ci_sw; /* Mandatory emelent! */ OSMO_ASSERT(ct); @@ -493,11 +529,8 @@ struct msgb *gsm0808_create_ass2(const struct gsm0808_channel_type *ct, gsm0808_enc_channel_type(msg, ct); /* Circuit Identity Code 3.2.2.2 */ - if (cic) { - cic_sw = osmo_htons(*cic); - msgb_tv_fixed_put(msg, GSM0808_IE_CIRCUIT_IDENTITY_CODE, - sizeof(cic_sw), (uint8_t *) & cic_sw); - } + if (cic) + msgb_tv16_put(msg, GSM0808_IE_CIRCUIT_IDENTITY_CODE, *cic); /* AoIP: AoIP Transport Layer Address (MGW) 3.2.2.102 */ if (ss) { @@ -505,14 +538,17 @@ struct msgb *gsm0808_create_ass2(const struct gsm0808_channel_type *ct, } /* AoIP: Codec List (MSC Preferred) 3.2.2.103 */ - if (scl) - gsm0808_enc_speech_codec_list(msg, scl); + if (scl) { + if (gsm0808_enc_speech_codec_list2(msg, scl) < 0) + goto exit_free; + } /* AoIP: Call Identifier 3.2.2.105 */ if (ci) { - ci_sw = osmo_htonl(*ci); - msgb_tv_fixed_put(msg, GSM0808_IE_CALL_ID, sizeof(ci_sw), - (uint8_t *) & ci_sw); + /* NOTE: 3GPP TS 48.008, section 3.2.2.105 specifies that + * the least significant byte shall be transmitted first. */ + msgb_v_put(msg, GSM0808_IE_CALL_ID); + osmo_store32le(*ci, msgb_put(msg, sizeof(*ci))); } if (kc) @@ -526,6 +562,10 @@ struct msgb *gsm0808_create_ass2(const struct gsm0808_channel_type *ct, msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP Assignment Request message, 3GPP TS 48.008 §3.2.1.1. @@ -577,7 +617,8 @@ struct msgb *gsm0808_create_ass_compl2(uint8_t rr_cause, uint8_t chosen_channel, msgb_tv_put(msg, GSM0808_IE_CHOSEN_CHANNEL, chosen_channel); /* write chosen encryption algorithm 3.2.2.44 */ - msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, encr_alg_id); + if (encr_alg_id > 0) + msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, encr_alg_id); /* write circuit pool 3.2.2.45 */ /* write speech version chosen: 3.2.2.51 when BTS picked it */ @@ -589,12 +630,16 @@ struct msgb *gsm0808_create_ass_compl2(uint8_t rr_cause, uint8_t chosen_channel, gsm0808_enc_aoip_trasp_addr(msg, ss); /* AoIP: Speech Codec (Chosen) 3.2.2.104 */ - if (sc) - gsm0808_enc_speech_codec(msg, sc); + if (sc) { + if (gsm0808_enc_speech_codec2(msg, sc) < 0) + goto exit_free; + } /* AoIP: add Codec List (BSS Supported) 3.2.2.103 */ - if (scl) - gsm0808_enc_speech_codec_list(msg, scl); + if (scl) { + if (gsm0808_enc_speech_codec_list2(msg, scl) < 0) + goto exit_free; + } /* FIXME: write LSA identifier 3.2.2.15 - see 3GPP TS 43.073 */ @@ -605,6 +650,10 @@ struct msgb *gsm0808_create_ass_compl2(uint8_t rr_cause, uint8_t chosen_channel, msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP Assignment Completed message @@ -666,13 +715,19 @@ struct msgb *gsm0808_create_ass_fail(uint8_t cause, const uint8_t *rr_cause, /* Circuit pool list 3.2.2.46 */ /* AoIP: add Codec List (BSS Supported) 3.2.2.103 */ - if (scl) - gsm0808_enc_speech_codec_list(msg, scl); + if (scl) { + if (gsm0808_enc_speech_codec_list2(msg, scl) < 0) + goto exit_free; + } /* update the size */ msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP Assignment Failure message @@ -774,7 +829,7 @@ struct msgb *gsm0808_create_paging(const char *imsi, const uint32_t *tmsi, { struct gsm0808_cell_id_list2 cil2 = {}; - /* Mandatory emelents! */ + /* Mandatory elements! */ OSMO_ASSERT(cil); if (cil->id_list_len > GSM0808_CELL_ID_LIST2_MAXLEN) @@ -816,6 +871,11 @@ static uint8_t put_old_bss_to_new_bss_information(struct msgb *msg, msgb_tlv_put(msg, GSM0808_FE_IE_CURRENT_CHANNEL_TYPE_2, 2, val); } + if (i->last_eutran_plmn_id_present) { + msgb_put_u8(msg, GSM0808_FE_IE_LAST_USED_EUTRAN_PLMN_ID); + osmo_plmn_to_bcd(msgb_put(msg, 3), &i->last_eutran_plmn_id); + } + *tlv_len = (uint8_t) (msg->tail - old_tail); return *tlv_len + 2; } @@ -938,7 +998,7 @@ struct msgb *gsm0808_create_handover_request(const struct gsm0808_handover_reque } /* Chosen Encryption Algorithm (Serving) 3.2.2.44 */ - if (params->chosen_encryption_algorithm_serving) + if (params->chosen_encryption_algorithm_serving > 0) msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, params->chosen_encryption_algorithm_serving); /* Old BSS to New BSS Information 3.2.2.58 */ @@ -960,8 +1020,10 @@ struct msgb *gsm0808_create_handover_request(const struct gsm0808_handover_reque if (params->aoip_transport_layer) gsm0808_enc_aoip_trasp_addr(msg, params->aoip_transport_layer); - if (params->codec_list_msc_preferred) - gsm0808_enc_speech_codec_list(msg, params->codec_list_msc_preferred); + if (params->codec_list_msc_preferred) { + if (gsm0808_enc_speech_codec_list2(msg, params->codec_list_msc_preferred) < 0) + goto exit_free; + } if (params->call_id_present) { uint8_t val[4]; @@ -969,6 +1031,9 @@ struct msgb *gsm0808_create_handover_request(const struct gsm0808_handover_reque msgb_tv_fixed_put(msg, GSM0808_IE_CALL_ID, 4, val); } + if (params->more_items && params->kc128_present) + gsm0808_enc_kc128(msg, params->kc128); + if (params->global_call_reference && params->global_call_reference_len) { msgb_tlv_put(msg, GSM0808_IE_GLOBAL_CALL_REF, params->global_call_reference_len, params->global_call_reference); @@ -978,6 +1043,10 @@ struct msgb *gsm0808_create_handover_request(const struct gsm0808_handover_reque msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP HANDOVER REQUEST ACKNOWLEDGE message, 3GPP TS 48.008 3.2.1.10. @@ -1001,7 +1070,7 @@ struct msgb *gsm0808_create_handover_request_ack2(const struct gsm0808_handover_ if (params->chosen_channel_present) msgb_tv_put(msg, GSM0808_IE_CHOSEN_CHANNEL, params->chosen_channel); - if (params->chosen_encr_alg) + if (params->chosen_encr_alg > 0) msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, params->chosen_encr_alg); if (params->chosen_speech_version != 0) @@ -1010,14 +1079,28 @@ struct msgb *gsm0808_create_handover_request_ack2(const struct gsm0808_handover_ if (params->aoip_transport_layer) gsm0808_enc_aoip_trasp_addr(msg, params->aoip_transport_layer); + /* AoIP: add Codec List (BSS Supported) 3.2.2.103. + * (codec_list_bss_supported was added to struct gsm0808_handover_request_ack later than speech_codec_chosen + * below, but it needs to come before it in the message coding). */ + if (params->more_items && params->codec_list_bss_supported.len) { + if (gsm0808_enc_speech_codec_list2(msg, ¶ms->codec_list_bss_supported) < 0) + goto exit_free; + } + /* AoIP: Speech Codec (Chosen) 3.2.2.104 */ - if (params->speech_codec_chosen_present) - gsm0808_enc_speech_codec(msg, ¶ms->speech_codec_chosen); + if (params->speech_codec_chosen_present) { + if (gsm0808_enc_speech_codec2(msg, ¶ms->speech_codec_chosen) < 0) + goto exit_free; + } /* prepend header with final length */ msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Same as gsm0808_create_handover_request_ack2() but with less parameters. @@ -1069,7 +1152,7 @@ struct msgb *gsm0808_create_handover_command(const struct gsm0808_handover_comma /*! Create BSSMAP HANDOVER DETECT message, 3GPP TS 48.008 3.2.1.40. * Sent from the MT BSC back to the MSC when the MS has sent a handover RACH request and the MT BSC has * received the Handover Detect message. */ -struct msgb *gsm0808_create_handover_detect() +struct msgb *gsm0808_create_handover_detect(void) { struct msgb *msg; @@ -1088,7 +1171,7 @@ struct msgb *gsm0808_create_handover_detect() /*! Create BSSMAP HANDOVER SUCCEEDED message, 3GPP TS 48.008 3.2.1.13. * Sent from the MSC back to the old BSS to notify that the MS has successfully accessed the new BSS. */ -struct msgb *gsm0808_create_handover_succeeded() +struct msgb *gsm0808_create_handover_succeeded(void) { struct msgb *msg; @@ -1123,15 +1206,19 @@ struct msgb *gsm0808_create_handover_complete(const struct gsm0808_handover_comp msgb_tlv_put(msg, GSM0808_IE_RR_CAUSE, 1, ¶ms->rr_cause); /* AoIP: Speech Codec (Chosen) 3.2.2.104 */ - if (params->speech_codec_chosen_present) - gsm0808_enc_speech_codec(msg, ¶ms->speech_codec_chosen); + if (params->speech_codec_chosen_present) { + if (gsm0808_enc_speech_codec2(msg, ¶ms->speech_codec_chosen) < 0) + goto exit_free; + } /* AoIP: add Codec List (BSS Supported) 3.2.2.103 */ - if (params->codec_list_bss_supported.len) - gsm0808_enc_speech_codec_list(msg, ¶ms->codec_list_bss_supported); + if (params->codec_list_bss_supported.len) { + if (gsm0808_enc_speech_codec_list2(msg, ¶ms->codec_list_bss_supported) < 0) + goto exit_free; + } /* Chosen Encryption Algorithm 3.2.2.44 */ - if (params->chosen_encr_alg_present) + if (params->chosen_encr_alg_present && params->chosen_encr_alg > 0) msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, params->chosen_encr_alg); /* LCLS-BSS-Status 3.2.2.119 */ @@ -1142,6 +1229,10 @@ struct msgb *gsm0808_create_handover_complete(const struct gsm0808_handover_comp msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP HANDOVER FAILURE message, 3GPP TS 48.008 3.2.1.16. @@ -1165,13 +1256,19 @@ struct msgb *gsm0808_create_handover_failure(const struct gsm0808_handover_failu msgb_tlv_put(msg, GSM0808_IE_RR_CAUSE, 1, ¶ms->rr_cause); /* AoIP: add Codec List (BSS Supported) 3.2.2.103 */ - if (params->codec_list_bss_supported.len) - gsm0808_enc_speech_codec_list(msg, ¶ms->codec_list_bss_supported); + if (params->codec_list_bss_supported.len) { + if (gsm0808_enc_speech_codec_list2(msg, ¶ms->codec_list_bss_supported) < 0) + goto exit_free; + } /* prepend header with final length */ msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; } /*! Create BSSMAP HANDOVER PERFORMED message, 3GPP TS 48.008 3.2.1.25. @@ -1199,7 +1296,7 @@ struct msgb *gsm0808_create_handover_performed(const struct gsm0808_handover_per msgb_tv_put(msg, GSM0808_IE_CHOSEN_CHANNEL, params->chosen_channel); /* Chosen Encryption Algorithm 3.2.2.44 */ - if (params->chosen_encr_alg_present) + if (params->chosen_encr_alg_present && params->chosen_encr_alg > 0) msgb_tv_put(msg, GSM0808_IE_CHOSEN_ENCR_ALG, params->chosen_encr_alg); /* Speech Version (chosen) 3.2.2.51 */ @@ -1207,8 +1304,10 @@ struct msgb *gsm0808_create_handover_performed(const struct gsm0808_handover_per msgb_tv_put(msg, GSM0808_IE_SPEECH_VERSION, params->speech_version_chosen); /* AoIP: Speech Codec (chosen) 3.2.2.104 */ - if (params->speech_codec_chosen_present) - gsm0808_enc_speech_codec(msg, ¶ms->speech_codec_chosen); + if (params->speech_codec_chosen_present) { + if (gsm0808_enc_speech_codec2(msg, ¶ms->speech_codec_chosen) < 0) + goto exit_free; + } /* LCLS-BSS-Status 3.2.2.119 */ if (params->lcls_bss_status_present) @@ -1218,6 +1317,65 @@ struct msgb *gsm0808_create_handover_performed(const struct gsm0808_handover_per msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); return msg; + +exit_free: + msgb_free(msg); + return NULL; +} + +/*! Create BSSMAP COMMON ID message, 3GPP TS 48.008 3.2.1.68. + * \param[in] imsi IMSI digits (decimal string). + * \param[in] selected_plmn_id Selected PLMN ID to encode, or NULL to not encode this IE. + * \param[in] last_used_eutran_plnm_id Last used E-UTRAN PLMN ID to encode, or NULL to not encode this IE. + * \returns callee-allocated msgb with BSSMAP COMMON ID message, or NULL if encoding failed. */ +struct msgb *gsm0808_create_common_id(const char *imsi, + const struct osmo_plmn_id *selected_plmn_id, + const struct osmo_plmn_id *last_used_eutran_plnm_id) +{ + struct msgb *msg; + uint8_t *out; + struct osmo_mobile_identity mi; + int rc; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "COMMON-ID"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_COMMON_ID); + + /* mandatory IMSI 3.2.2.6 */ + mi = (struct osmo_mobile_identity){ .type = GSM_MI_TYPE_IMSI }; + OSMO_STRLCPY_ARRAY(mi.imsi, imsi); + out = msgb_tl_put(msg, GSM0808_IE_IMSI); + rc = osmo_mobile_identity_encode_msgb(msg, &mi, false); + if (rc < 0) { + msgb_free(msg); + return NULL; + } + /* write the MI value length */ + *out = rc; + + /* not implemented: SNA Access Information */ + + /* Selected PLMN ID */ + if (selected_plmn_id) { + msgb_v_put(msg, GSM0808_IE_SELECTED_PLMN_ID); + out = msgb_put(msg, 3); + osmo_plmn_to_bcd(out, selected_plmn_id); + } + + /* Last used E-UTRAN PLMN ID */ + if (last_used_eutran_plnm_id) { + msgb_v_put(msg, GSM0808_IE_LAST_USED_EUTRAN_PLMN_ID); + out = msgb_put(msg, 3); + osmo_plmn_to_bcd(out, last_used_eutran_plnm_id); + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; } /*! Prepend a DTAP header to given Message Buffer @@ -1258,6 +1416,763 @@ struct msgb *gsm0808_create_dtap(struct msgb *msg_l3, uint8_t link_id) return msg; } +struct msgb *gsm0808_create_perform_location_request(const struct gsm0808_perform_location_request *params) +{ + struct msgb *msg; + uint8_t *out; + int rc; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-PERFORM-LOCATION-REQUEST"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_PERFORM_LOCATION_RQST); + + /* Location Type 3.2.2.63 */ + osmo_bssmap_le_ie_enc_location_type(msg, ¶ms->location_type); + + if (params->imsi.type == GSM_MI_TYPE_IMSI) { + /* IMSI 3.2.2.6 */ + out = msgb_tl_put(msg, GSM0808_IE_IMSI); + rc = osmo_mobile_identity_encode_msgb(msg, ¶ms->imsi, false); + if (rc < 0) { + msgb_free(msg); + return NULL; + } + /* write the MI value length */ + *out = rc; + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +struct msgb *gsm0808_create_perform_location_response(const struct gsm0808_perform_location_response *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-PERFORM-LOCATION-RESPONSE"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_PERFORM_LOCATION_RESPONSE); + + if (params->location_estimate_present) { + uint8_t *l = msgb_tl_put(msg, GSM0808_IE_LOCATION_ESTIMATE); + int rc = osmo_gad_raw_write(msg, ¶ms->location_estimate); + if (rc < 0) { + msgb_free(msg); + return NULL; + } + *l = rc; + } + + if (params->lcs_cause.present) { + uint8_t *l = msgb_tl_put(msg, GSM0808_IE_LCS_CAUSE); + int rc = osmo_lcs_cause_enc(msg, ¶ms->lcs_cause); + if (rc < 0) { + msgb_free(msg); + return NULL; + } + *l = rc; + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +int gsm0808_enc_lcs_cause(struct msgb *msg, const struct lcs_cause_ie *lcs_cause) +{ + uint8_t *l = msgb_tl_put(msg, GSM0808_IE_LCS_CAUSE); + int rc = osmo_lcs_cause_enc(msg, lcs_cause); + if (rc <= 0) + return rc; + *l = rc; + return rc + 2; +} + +struct msgb *gsm0808_create_perform_location_abort(const struct lcs_cause_ie *lcs_cause) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-PERFORM-LOCATION-ABORT"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_PERFORM_LOCATION_ABORT); + + gsm0808_enc_lcs_cause(msg, lcs_cause); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS SETUP message, 3GPP TS 48.008 3.2.1.50. + * Sent from the MSC to the BSC to request VGCS/VBS call. */ +struct msgb *gsm0808_create_vgcs_vbs_setup(const struct gsm0808_vgcs_vbs_setup *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-SETUP"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_SETUP); + + /* Group Call Reference, 3.2.2.55 */ + gsm0808_enc_group_callref(msg, ¶ms->callref); + + /* Priority, 3.2.2.18 */ + if (params->priority_present) + gsm0808_enc_priority(msg, ¶ms->priority); + + /* VGCS Feature Flags, 3.2.2.88 */ + if (params->vgcs_feature_flags_present) + gsm0808_enc_vgcs_feature_flags(msg, ¶ms->flags); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS SETUP ACK message, 3GPP TS 48.008 3.2.1.51. + * Sent from the BSC to the MSC to confirm VGCS/VBS call. */ +struct msgb *gsm0808_create_vgcs_vbs_setup_ack(const struct gsm0808_vgcs_vbs_setup_ack *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-SETUP-ACK"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_SETUP_ACK); + + /* VGCS Feature Flags, 3.2.2.88 */ + if (params->vgcs_feature_flags_present) + gsm0808_enc_vgcs_feature_flags(msg, ¶ms->flags); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS SETUP REFUSE message, 3GPP TS 48.008 3.2.1.52. + * Sent from the BSC to the MSC to reject VGCS/VBS call. */ +struct msgb *gsm0808_create_vgcs_vbs_setup_refuse(enum gsm0808_cause cause) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-SETUP-REFUSE"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_SETUP_REFUSE); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, cause); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS ASSIGNMENT REQUEST message, 3GPP TS 48.008 3.2.1.53. + * Sent from the MSC to the BSC to assign radio resources for a VGCS/VBS. */ +struct msgb *gsm0808_create_vgcs_vbs_assign_req(const struct gsm0808_vgcs_vbs_assign_req *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-ASSIGNMENT-REQUEST"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_RQST); + + /* Channel Type, 3.2.2.11 */ + gsm0808_enc_channel_type(msg, ¶ms->channel_type); + + /* Assignment Requrirement, 3.2.2.52 */ + gsm0808_enc_assign_req(msg, params->ass_req); + + /* Cell Identifier, 3.2.2.17 */ + gsm0808_enc_cell_id(msg, ¶ms->cell_identifier); + + /* Group Call Reference, 3.2.2.55 */ + gsm0808_enc_group_callref(msg, ¶ms->callref); + + /* Priority, 3.2.2.18 */ + if (params->priority_present) + gsm0808_enc_priority(msg, ¶ms->priority); + + /* Circuit Identity Code, 3.2.2.2 */ + if (params->cic_present) + msgb_tv16_put(msg, GSM0808_IE_CIRCUIT_IDENTITY_CODE, params->cic); + + /* Downlink DTX Flag, 3.2.2.26 */ + if (params->downlink_dtx_flag_present) + msgb_tv_put(msg, GSM0808_IE_DOWNLINK_DTX_FLAG, params->downlink_dtx_flag); + + /* Encryption Information, 3.2.2.10 */ + if (params->encryption_information_present) + gsm0808_enc_encrypt_info(msg, ¶ms->encryption_information); + + /* VSTK_RAND Imformation, 3.2.2.83 */ + if (params->vstk_rand_present) + msgb_tlv_put(msg, GSM0808_IE_VSTK_RAND_INFO, sizeof(params->vstk_rand), params->vstk_rand); + + /* VSTK Information, 3.2.2.84 */ + if (params->vstk_present) + msgb_tlv_put(msg, GSM0808_IE_VSTK_INFO, sizeof(params->vstk), params->vstk); + + /* Cell Identifier List Segment, 3.2.2.27a */ + if (params->cils_present) + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEGMENT, ¶ms->cils); + + /* AoIP Transport Layer Address (MGW), 3.2.2.102 */ + if (params->aoip_transport_layer_present) + gsm0808_enc_aoip_trasp_addr(msg, ¶ms->aoip_transport_layer); + + /* Call Identifier, 3.2.2.105 */ + if (params->call_id_present) { + /* NOTE: 3GPP TS 48.008, section 3.2.2.105 specifies that + * the least significant byte shall be transmitted first. */ + msgb_v_put(msg, GSM0808_IE_CALL_ID); + osmo_store32le(params->call_id, msgb_put(msg, sizeof(uint32_t))); + } + + /* Codec List (MSC Preferred) 3.2.2.103 */ + if (params->codec_list_present) { + if (gsm0808_enc_speech_codec_list2(msg, ¶ms->codec_list_msc_preferred) < 0) + goto exit_free; + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; + +exit_free: + msgb_free(msg); + return NULL; +} + +/*! Create BSSMAP VGCS/VBS ASSIGNMENT RESULT message, 3GPP TS 48.008 3.2.1.54. + * Sent from the BSC to the MSC to indicate assignment/deassingment of radio resources for a VGCS/VBS. */ +struct msgb *gsm0808_create_vgcs_vbs_assign_res(const struct gsm0808_vgcs_vbs_assign_res *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-ASSIGNMENT-RESULT"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_RESULT); + + /* Channel Type, 3.2.2.11 */ + gsm0808_enc_channel_type(msg, ¶ms->channel_type); + + /* Cell Identifier, 3.2.2.17 */ + gsm0808_enc_cell_id(msg, ¶ms->cell_identifier); + + /* Chosen Channel, 3.2.2.33 */ + if (params->chosen_channel_present) + msgb_tv_put(msg, GSM0808_IE_CHOSEN_CHANNEL, params->chosen_channel); + + /* Circuit Identity Code, 3.2.2.2 */ + if (params->cic_present) + msgb_tv16_put(msg, GSM0808_IE_CIRCUIT_IDENTITY_CODE, params->cic); + + /* Circuit Pool, 3.2.2.45 */ + if (params->circuit_pool_present) + msgb_tv_put(msg, GSM0808_IE_CIRCUIT_POOL, params->circuit_pool); + + /* AoIP Transport Layer Address (BSS), 3.2.2.102 */ + if (params->aoip_transport_layer_present) + gsm0808_enc_aoip_trasp_addr(msg, ¶ms->aoip_transport_layer); + + /* Codec (MSC Chosen) 3.2.2.103 */ + if (params->codec_present) { + if (gsm0808_enc_speech_codec2(msg, ¶ms->codec_msc_chosen) < 0) + goto exit_free; + } + + /* Call Identifier, 3.2.2.105 */ + if (params->call_id_present) { + /* NOTE: 3GPP TS 48.008, section 3.2.2.105 specifies that + * the least significant byte shall be transmitted first. */ + msgb_v_put(msg, GSM0808_IE_CALL_ID); + osmo_store32le(params->call_id, msgb_put(msg, sizeof(uint32_t))); + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; + +exit_free: + msgb_free(msg); + return NULL; +} + +/*! Create BSSMAP VGCS/VBS ASSIGNMENT FAILURE message, 3GPP TS 48.008 3.2.1.55. + * Sent from the BSC to the MSC to indicate assignment failure for a VGCS/VBS. */ +struct msgb *gsm0808_create_vgcs_vbs_assign_fail(const struct gsm0808_vgcs_vbs_assign_fail *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-ASSIGNMENT-RESULT"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_FAILURE); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, params->cause); + + /* Circuit Pool, 3.2.2.45 */ + if (params->circuit_pool_present) + msgb_tv_put(msg, GSM0808_IE_CIRCUIT_POOL, params->circuit_pool); + + /* Circuit Pool List, 3.2.2.46 */ + if (params->circuit_pool_present) + msgb_tlv_put(msg, GSM0808_IE_CIRCUIT_POOL_LIST, params->cpl.list_len, params->cpl.pool); + + /* Codec List (BSS Supported) 3.2.2.103 */ + if (params->codec_list_present) { + if (gsm0808_enc_speech_codec_list2(msg, ¶ms->codec_list_bss_supported) < 0) + goto exit_free; + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; + +exit_free: + msgb_free(msg); + return NULL; +} + +/*! Create BSSMAP VGCS/VBS QUEUING INDICATION message, 3GPP TS 48.008 3.2.1.56. + * Sent from the BSC to the MSC to indicate delay in assignment for a VGCS/VBS. */ +struct msgb *gsm0808_create_vgcs_queuing_ind(void) +{ + struct msgb *msg; + uint8_t val = BSS_MAP_MSG_VGCS_VBS_QUEUING_INDICATION; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-QUEUING-INDICATION"); + if (!msg) + return NULL; + + msg->l3h = msg->data; + msgb_tlv_put(msg, BSSAP_MSG_BSS_MANAGEMENT, 1, &val); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK REQUEST message, 3GPP TS 48.008 3.2.1.57. + * Sent from the BSC to the MSC to indicate that a mobile requested access to uplink. */ +struct msgb *gsm0808_create_uplink_request(const struct gsm0808_uplink_request *params) +{ + struct msgb *msg; + int rc; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-REQUEST"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_RQST); + + /* Talker Priority, 3.2.2.89 */ + if (params->talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->talker_priority); + + /* Cell Identifier, 3.2.2.17 */ + if (params->cell_identifier_present) + gsm0808_enc_cell_id(msg, ¶ms->cell_identifier); + + /* Layer 3 Information, 3.2.2.24 */ + if (params->l3_present) + msgb_tlv_put(msg, GSM0808_IE_LAYER_3_INFORMATION, params->l3.l3_len, params->l3.l3); + + /* Mobile Identity, 3.2.2.41 */ + if (params->mi_present) { + rc = osmo_mobile_identity_encode_msgb(msg, ¶ms->mi, false); + if (rc < 0) { + msgb_free(msg); + return NULL; + } + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK REQUEST ACKNOWLEDGE message, 3GPP TS 48.008 3.2.1.58. + * Sent from the MSC to the BSC to indicate that access to uplink was granted. */ +struct msgb *gsm0808_create_uplink_request_ack(const struct gsm0808_uplink_request_ack *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-REQUEST-ACKNOWLEDGE"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_RQST_ACKNOWLEDGE); + + /* Talker Priority, 3.2.2.89 */ + if (params->talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->talker_priority); + + /* Emergency set indication, 3.2.2.90 */ + if (params->emerg_set_ind_present) + msgb_v_put(msg, GSM0808_IE_EMERGENCY_SET_INDICATION); + + /* Talker Identity, 3.2.2.91 */ + if (params->talker_identity_present) + gsm0808_enc_talker_identity(msg, ¶ms->talker_identity); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK CONFIRM message, 3GPP TS 48.008 3.2.1.59. + * Sent from the BSC to the MSC to indicate that access to uplink was has been successfully established. */ +struct msgb *gsm0808_create_uplink_request_cnf(const struct gsm0808_uplink_request_cnf *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-REQUEST-CONFIRM"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_RQST_CONFIRMATION); + + /* Cell Identifier, 3.2.2.17 */ + gsm0808_enc_cell_id(msg, ¶ms->cell_identifier); + + /* Talker Identity, 3.2.2.91 */ + if (params->talker_identity_present) + gsm0808_enc_talker_identity(msg, ¶ms->talker_identity); + + /* Layer 3 Information, 3.2.2.24 */ + msgb_tlv_put(msg, GSM0808_IE_LAYER_3_INFORMATION, params->l3.l3_len, params->l3.l3); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK APPLICATION DATA message, 3GPP TS 48.008 3.2.1.59a. + * Sent from the BSC to the MSC to pass L3 info from the talker. */ +struct msgb *gsm0808_create_uplink_app_data(const struct gsm0808_uplink_app_data *params) +{ + struct msgb *msg; + uint8_t val; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-APPLICATION-DATA"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_APP_DATA); + + /* Cell Identifier, 3.2.2.17 */ + gsm0808_enc_cell_id(msg, ¶ms->cell_identifier); + + /* Layer 3 Information, 3.2.2.24 */ + msgb_tlv_put(msg, GSM0808_IE_LAYER_3_INFORMATION, params->l3.l3_len, params->l3.l3); + + /* Application Data Information, 3.2.2.100 */ + val = params->bt_ind; + msgb_tlv_put(msg, GSM0808_IE_APP_DATA_INFO, 1, &val); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK RELEASE INDICATION message, 3GPP TS 48.008 3.2.1.60. + * Sent from the BSC to the MSC to indicate that the uplink has been released. */ +struct msgb *gsm0808_create_uplink_release_ind(const struct gsm0808_uplink_release_ind *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-RELEASE-INDICATION"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_RELEASE_INDICATION); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, params->cause); + + /* Talker Priority, 3.2.2.89 */ + if (params->talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->talker_priority); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK REJECT COMMAND message, 3GPP TS 48.008 3.2.1.61. + * Sent from the MSC to the BSC to indicate that the uplink is not available for allocation. */ +struct msgb *gsm0808_create_uplink_reject_cmd(const struct gsm0808_uplink_reject_cmd *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-REJECT-COMMAND"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_REJECT_CMD); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, params->cause); + + /* Talker Priority, 3.2.2.89 */ + if (params->current_talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->current_talker_priority); + + /* Talker Priority, 3.2.2.89 */ + if (params->rejected_talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->rejected_talker_priority); + + /* Talker Identity, 3.2.2.91 */ + if (params->talker_identity_present) + gsm0808_enc_talker_identity(msg, ¶ms->talker_identity); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK RELEASE COMMAND message, 3GPP TS 48.008 3.2.1.62. + * Sent from the MSC to the BSC to indicate that the uplink is available for allocation. */ +struct msgb *gsm0808_create_uplink_release_cmd(const enum gsm0808_cause cause) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-RELEASE-COMMAND"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_RELEASE_CMD); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, cause); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS) UPLINK SEIZED COMMAND message, 3GPP TS 48.008 3.2.1.62. + * Sent from the MSC to the BSC to indicate that the uplink is no longer available for allocation. */ +struct msgb *gsm0808_create_uplink_seized_cmd(const struct gsm0808_uplink_seized_cmd *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-UPLINK-SEIZED-COMMAND"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_UPLINK_SEIZED_CMD); + + /* Cause, 3.2.2.5 */ + gsm0808_enc_cause(msg, params->cause); + + /* Talker Priority, 3.2.2.89 */ + if (params->talker_priority_present) + msgb_tv_put(msg, GSM0808_IE_TALKER_PRIORITY, params->talker_priority); + + /* Emergency set indication, 3.2.2.90 */ + if (params->emerg_set_ind_present) + msgb_v_put(msg, GSM0808_IE_EMERGENCY_SET_INDICATION); + + /* Talker Identity, 3.2.2.91 */ + if (params->talker_identity_present) + gsm0808_enc_talker_identity(msg, ¶ms->talker_identity); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS ADDITIONAL INFORMATION message, 3GPP TS 48.008 3.2.1.78. + * Sent from the MSC to the BSC to transfer talker identity. */ +struct msgb *gsm0808_create_vgcs_additional_info(const struct gsm0808_talker_identity *ti) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS-ADDITIONAL-INFO"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_ADDL_INFO); + + /* Talker Identity, 3.2.2.91 */ + gsm0808_enc_talker_identity(msg, ti); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS AREA CELL INFO message, 3GPP TS 48.008 3.2.1.79. + * Sent from the BSC to the MSC to transfer additional infos about cells. */ +struct msgb *gsm0808_create_vgcs_vbs_area_cell_info(const struct gsm0808_vgcs_vbs_area_cell_info *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-AREA-CELL-INFO"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_RQST); + + /* Cell Identifier List Segment, 3.2.2.27a */ + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEGMENT, ¶ms->cils); + + /* Assignment Requrirement, 3.2.2.52 */ + if (params->ass_req_present) + gsm0808_enc_assign_req(msg, params->ass_req); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS/VBS ASSIGNMENT STATUS message, 3GPP TS 48.008 3.2.1.80. + * Sent from the BSC to the MSC to indicate assignment status for each cell. */ +struct msgb *gsm0808_create_vgcs_vbs_assign_stat(const struct gsm0808_vgcs_vbs_assign_stat *params) +{ + struct msgb *msg; + uint8_t val; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS/VBS-ASSIGNMENT-STATUS"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_STATUS); + + /* Cell Identifier List Segment, 3.2.2.27b */ + if (params->cils_est_present) + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEG_EST_CELLS, ¶ms->cils_est); + /* Cell Identifier List Segment, 3.2.2.27c */ + if (params->cils_tbe_present) + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEG_CELLS_TBE, ¶ms->cils_tbe); + /* Cell Identifier List Segment, 3.2.2.27e */ + if (params->cils_rel_present) + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEG_REL_CELLS, ¶ms->cils_rel); + /* Cell Identifier List Segment, 3.2.2.27f */ + if (params->cils_ne_present) + gsm0808_enc_cell_id_list_segment(msg, GSM0808_IE_CELL_ID_LIST_SEG_NE_CELLS, ¶ms->cils_ne); + + /* VGCS/VBS Cell Status, 3.2.2.94 */ + if (params->cell_status_present) { + val = params->cell_status; + msgb_tlv_put(msg, GSM0808_IE_VGCS_VBS_CELL_STATUS, 1, &val); + } + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP VGCS SMS message, 3GPP TS 48.008 3.2.1.81. + * Sent from the MSC to the BSC to send an SMS to VGCS. */ +struct msgb *gsm0808_create_vgcs_sms(const struct gsm0808_sms_to_vgcs *sms) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS-SMS"); + if (!msg) + return NULL; + + /* SMS to VGCS, 3.2.2.92 */ + msgb_tlv_put(msg, GSM0808_IE_VGCS_VBS_CELL_STATUS, sms->sms_len, sms->sms); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/*! Create BSSMAP (VGCS/VBS) NOTIFICATION DATA message, 3GPP TS 48.008 3.2.1.82. + * Sent from the MSC to the BSC to send application specific data. */ +struct msgb *gsm0808_create_notification_data(const struct gsm0808_notification_data *params) +{ + struct msgb *msg; + + msg = msgb_alloc_headroom(BSSMAP_MSG_SIZE, BSSMAP_MSG_HEADROOM, "BSSMAP-VGCS-SMS"); + if (!msg) + return NULL; + + /* Message Type, 3.2.2.1 */ + msgb_v_put(msg, BSS_MAP_MSG_NOTIFICATION_DATA); + + /* Application Data, 3.2.2.98 */ + msgb_tlv_put(msg, GSM0808_IE_APP_DATA, params->app_data.data_len, params->app_data.data); + + /* Data Identity, 3.2.2.99 */ + gsm0808_enc_data_identity(msg, ¶ms->data_ident); + + /* MSISDN, 3.2.2.101 */ + if (params->msisdn_present) + gsm0808_enc_msisdn(msg, params->msisdn); + + /* prepend header with final length */ + msg->l3h = msgb_tv_push(msg, BSSAP_MSG_BSS_MANAGEMENT, msgb_length(msg)); + + return msg; +} + +/* Note that EMERGENCY RESET INDICATION and EMERGENCY RESET COMMAND cannot be implemented, due to lack of + * message type information in the specifications. */ + /* As per 3GPP TS 48.008 version 11.7.0 Release 11 */ static const struct tlv_definition bss_att_tlvdef = { .def = { @@ -1329,6 +2244,7 @@ static const struct tlv_definition bss_att_tlvdef = { [GSM0808_IE_LOCATION_ESTIMATE] = { TLV_TYPE_TLV }, [GSM0808_IE_POSITIONING_DATA] = { TLV_TYPE_TLV }, [GSM0808_IE_LCS_CAUSE] = { TLV_TYPE_TLV }, + [GSM0808_IE_LCS_CLIENT_TYPE] = { TLV_TYPE_TLV }, [GSM0808_IE_APDU] = { TLV_TYPE_TLV }, [GSM0808_IE_NETWORK_ELEMENT_IDENTITY] = { TLV_TYPE_TLV }, [GSM0808_IE_GPS_ASSISTANCE_DATA] = { TLV_TYPE_TLV }, @@ -1345,6 +2261,7 @@ static const struct tlv_definition bss_att_tlvdef = { [GSM0800_IE_INTER_SYSTEM_INFO] = { TLV_TYPE_TLV }, [GSM0808_IE_SNA_ACCESS_INFO] = { TLV_TYPE_TLV }, [GSM0808_IE_VSTK_RAND_INFO] = { TLV_TYPE_TLV }, + [GSM0808_IE_VSTK_INFO] = { TLV_TYPE_TLV }, [GSM0808_IE_PAGING_INFO] = { TLV_TYPE_TV }, [GSM0808_IE_IMEI] = { TLV_TYPE_TLV }, [GSM0808_IE_VELOCITY_ESTIMATE] = { TLV_TYPE_TLV }, @@ -1387,6 +2304,11 @@ static const struct tlv_definition bss_att_tlvdef = { [GSM0808_IE_CN_TO_MS_TRANSP_INFO] = { TLV_TYPE_TLV }, [GSM0808_IE_SELECTED_PLMN_ID] = { TLV_TYPE_FIXED, 3 }, [GSM0808_IE_LAST_USED_EUTRAN_PLMN_ID] = { TLV_TYPE_FIXED, 3 }, + [GSM0808_IE_OLD_LAI] = { TLV_TYPE_FIXED, 5 }, + [GSM0808_IE_ATTACH_INDICATOR] = { TLV_TYPE_T }, + [GSM0808_IE_SELECTED_OPERATOR] = { TLV_TYPE_FIXED, 3 }, + [GSM0808_IE_PS_REGISTERED_OPERATOR] = { TLV_TYPE_FIXED, 3 }, + [GSM0808_IE_CS_REGISTERED_OPERATOR] = { TLV_TYPE_FIXED, 3 }, /* Osmocom extensions */ [GSM0808_IE_OSMO_OSMUX_SUPPORT] = { TLV_TYPE_T }, @@ -1399,6 +2321,38 @@ const struct tlv_definition *gsm0808_att_tlvdef(void) return &bss_att_tlvdef; } +/* As per 3GPP TS 48.008 version 16.0.0 Release 16 § 3.2.2.58 Old BSS to New BSS Information */ +const struct tlv_definition gsm0808_old_bss_to_new_bss_info_att_tlvdef = { + .def = { + [GSM0808_FE_IE_EXTRA_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_CURRENT_CHANNEL_TYPE_2] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_TARGET_CELL_RADIO_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_GPRS_SUSPEND_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_MULTIRATE_CONFIGURATION_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_DUAL_TRANSFER_MODE_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_INTER_RAT_HANDOVER_INFO] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_CDMA2000_CAPABILITY_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_DOWNLINK_CELL_LOAD_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_UPLINK_CELL_LOAD_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_CELL_LOAD_INFORMATION_GROUP] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_CELL_LOAD_INFORMATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_PS_INDICATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_DTM_HANDOVER_COMMAND_INDICATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_D_RNTI] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_IRAT_MEASUREMENT_CONFIGURATION] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_SOURCE_CELL_ID] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_IRAT_MEASUREMENT_CONFIGURATION_EXTENDED_E_ARFCNS] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_VGCS_TALKER_MODE] = { TLV_TYPE_TLV }, + [GSM0808_FE_IE_LAST_USED_EUTRAN_PLMN_ID] = { TLV_TYPE_FIXED, 3 }, + }, +}; + +const struct value_string gsm0406_dlci_sapi_names[] = { + { DLCI_SAPI_RR_MM_CC, "RR/MM/CC" }, + { DLCI_SAPI_SMS, "SMS" }, + { 0, NULL } +}; + static const struct value_string gsm0808_msgt_names[] = { { BSS_MAP_MSG_ASSIGMENT_RQST, "ASSIGNMENT REQ" }, { BSS_MAP_MSG_ASSIGMENT_COMPLETE, "ASSIGNMENT COMPL" }, @@ -1480,7 +2434,9 @@ static const struct value_string gsm0808_msgt_names[] = { { BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_RQST, "VGCS/VBS ASSIGN REQ" }, { BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_RESULT, "VGCS/VBS ASSIGN RES" }, { BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_FAILURE, "VGCS/VBS ASSIGN FAIL" }, + { BSS_MAP_MSG_VGCS_VBS_ASSIGNMENT_STATUS, "VGCS/VBS ASSIGN STATUS" }, { BSS_MAP_MSG_VGCS_VBS_QUEUING_INDICATION, "VGCS/VBS QUEUING IND" }, + { BSS_MAP_MSG_VGCS_VBS_AREA_CELL_INFO, "VGCS/VBS AREA CELL INFO" }, { BSS_MAP_MSG_UPLINK_RQST, "UPLINK REQ" }, { BSS_MAP_MSG_UPLINK_RQST_ACKNOWLEDGE, "UPLINK REQ ACK" }, { BSS_MAP_MSG_UPLINK_RQST_CONFIRMATION, "UPLINK REQ CONF" }, @@ -1489,11 +2445,12 @@ static const struct value_string gsm0808_msgt_names[] = { { BSS_MAP_MSG_UPLINK_RELEASE_CMD, "UPLINK REL CMD" }, { BSS_MAP_MSG_UPLINK_SEIZED_CMD, "UPLINK SEIZED CMD" }, { BSS_MAP_MSG_VGCS_ADDL_INFO, "VGCS ADDL INFO" }, + { BSS_MAP_MSG_VGCS_SMS, "VGCS SMS" }, { BSS_MAP_MSG_NOTIFICATION_DATA, "NOTIF DATA" }, { BSS_MAP_MSG_UPLINK_APP_DATA, "UPLINK APP DATA" }, { BSS_MAP_MSG_LCLS_CONNECT_CTRL, "LCLS-CONNECT-CONTROL" }, - { BSS_MAP_MSG_LCLS_CONNECT_CTRL_ACK, "CLS-CONNECT-CONTROL-ACK" }, + { BSS_MAP_MSG_LCLS_CONNECT_CTRL_ACK, "LCLS-CONNECT-CONTROL-ACK" }, { BSS_MAP_MSG_LCLS_NOTIFICATION, "LCLS-NOTIFICATION" }, { 0, NULL } @@ -1527,6 +2484,7 @@ const struct value_string gsm0808_speech_codec_type_names[] = { { GSM0808_SCT_HR3, "HR3" }, { GSM0808_SCT_HR4, "HR4" }, { GSM0808_SCT_HR6, "HR6" }, + { GSM0808_SCT_EXT, "Codec Extension" }, { GSM0808_SCT_CSD, "CSD" }, { 0, NULL } }; @@ -1648,6 +2606,55 @@ const char *gsm0808_cause_name(enum gsm0808_cause cause) return get_value_string(gsm0808_cause_names, cause); } +enum gsm0808_cause gsm0808_get_cause(const struct tlv_parsed *tp) +{ + const uint8_t *buf = TLVP_VAL_MINLEN(tp, GSM0808_IE_CAUSE, 1); + + if (!buf) + return -EBADMSG; + + if (TLVP_LEN(tp, GSM0808_IE_CAUSE) > 1) { + if (!gsm0808_cause_ext(buf[0])) + return -EINVAL; + return buf[1]; + } + + return buf[0]; +} + +const char *gsm0808_diagnostics_octet_location_str(uint8_t pointer) +{ + switch (pointer) { + case 0: + return "Error location not determined"; + case 1: + return "The first octet of the message received (i.e. the message type) was found erroneous (unknown)"; + case 0xfd: + return "The first octet of the BSSAP header (Discrimination) was found erroneous"; + case 0xfe: + return "(DTAP only) The DLCI (second) octet of the BSSAP header was found erroneous"; + case 0xff: + return "The last octet of the BSSAP header (length indicator) was found erroneous"; + default: + snprintf(str_buff, sizeof(str_buff), "The %d octet of the message received was found erroneous", pointer); + return str_buff; + } +} + +const char *gsm0808_diagnostics_bit_location_str(uint8_t bit_pointer) +{ + if (bit_pointer == 0) { + return "No particular part of the octet is indicated"; + } else if (bit_pointer > 8) { + return "Reserved value"; + } + + snprintf(str_buff, sizeof(str_buff), + "An error was provoked by the field whose most significant bit is in bit position %d", + bit_pointer); + return str_buff; +} + const struct value_string gsm0808_lcls_config_names[] = { { GSM0808_LCLS_CFG_BOTH_WAY, "Connect both-way" }, { GSM0808_LCLS_CFG_BOTH_WAY_AND_BICAST_UL, @@ -1684,4 +2691,101 @@ const struct value_string gsm0808_lcls_status_names[] = { { 0, NULL } }; +/* Convert one S0-S15 bit to its set of AMR modes, for HR AMR and FR AMR. + * This is 3GPP TS 28.062 Table 7.11.3.1.3-2: "Preferred Configurations", with some configurations removed as specified + * in 3GPP TS 48.008 3.2.2.103: + * + * FR_AMR is coded ‘0011’. + * S11, S13 and S15 are reserved and coded with zeroes. + * + * HR_AMR is coded ‘0100’. + * S6 - S7 and S11 – S15 are reserved and coded with zeroes. + * + * Meaning: for FR, exclude all Optimisation Mode configurations. + * For HR, exclude all that are not supported by HR AMR -- drop all that include at least one of + * 10.2 or 12.2. + * + * Also, for HR, drop 12.2k from S1. + * + * The first array dimension is 0 for half rate and 1 for full rate. + * The second array dimension is the configuration number (0..15) aka Sn. + * The values are bitmask combinations of (1 << GSM0808_AMR_MODE_nnnn). + * + * For example, accumulate all modes that are possible in a given my_s15_s0: + * + * uint8_t modes = 0; + * for (s_bit = 0; s_bit < 15; s_bit++) + * if (my_s15_s0 & (1 << s_bit)) + * modes |= gsm0808_amr_modes_from_cfg[full_rate ? 1 : 0][s_bit]; + * for (i = 0; i < 8; i++) + * if (modes & (1 << i)) + * printf(" %s", gsm0808_amr_mode_name(i)); + */ +const uint8_t gsm0808_amr_modes_from_cfg[2][16] = { +#define B(X) (1 << (GSM0808_AMR_MODE_##X)) + /* HR */ + { + /* Sn = modes */ + [0] = B(4_75), + [1] = B(4_75) | B(5_90) | B(7_40), + [2] = B(5_90), + [3] = B(6_70), + [4] = B(7_40), + [5] = B(7_95), + [6] = 0, + [7] = 0, + + [8] = B(4_75) | B(5_90), + [9] = B(4_75) | B(5_90) | B(6_70), + [10] = B(4_75) | B(5_90) | B(6_70) | B(7_40), + [11] = 0, + [12] = 0, + [13] = 0, + [14] = 0, + [15] = 0, + }, + /* FR */ + { + /* Sn = modes */ + [0] = B(4_75), + [1] = B(4_75) | B(5_90) | B(7_40) | B(12_2), + [2] = B(5_90), + [3] = B(6_70), + [4] = B(7_40), + [5] = B(7_95), + [6] = B(10_2), + [7] = B(12_2), + + [8] = B(4_75) | B(5_90), + [9] = B(4_75) | B(5_90) | B(6_70), + [10] = B(4_75) | B(5_90) | B(6_70) | B(7_40), + [11] = 0, + [12] = B(4_75) | B(5_90) | B(6_70) | B(10_2), + [13] = 0, + [14] = B(4_75) | B(5_90) | B(7_95) | B(12_2), + [15] = 0, + } +}; + +/* AMR mode names from GSM0808_AMR_MODE_*, for use with gsm0808_amr_modes_from_cfg. + * + * For example: + * printf("S9: "); + * uint8_t s9_modes = gsm0808_amr_modes_from_cfg[full_rate ? 1 : 0][9]; + * for (bit = 0; bit < 8; bit++) + * if (s9_modes & (1 << bit)) + * printf("%s,", gsm0808_amr_mode_name(bit)); + */ +const struct value_string gsm0808_amr_mode_names[] = { + { GSM0808_AMR_MODE_4_75, "4.75" }, + { GSM0808_AMR_MODE_5_15, "5.15" }, + { GSM0808_AMR_MODE_5_90, "5.90" }, + { GSM0808_AMR_MODE_6_70, "6.70" }, + { GSM0808_AMR_MODE_7_40, "7.40" }, + { GSM0808_AMR_MODE_7_95, "7.95" }, + { GSM0808_AMR_MODE_10_2, "10.2" }, + { GSM0808_AMR_MODE_12_2, "12.2" }, + {} +}; + /*! @} */ diff --git a/src/gsm/gsm0808_utils.c b/src/gsm/gsm0808_utils.c index 7416d8f5..c9f26d35 100644 --- a/src/gsm/gsm0808_utils.c +++ b/src/gsm/gsm0808_utils.c @@ -78,7 +78,7 @@ uint8_t gsm0808_enc_cause(struct msgb *msg, uint16_t cause) /*! Encode TS 08.08 AoIP transport address IE * \param[out] msg Message Buffer to which to append IE * \param[in] ss Socket Address to be used in IE - * \returns number of bytes added to \a msg */ + * \returns number of bytes added to \a msg; 0 if msg is too small */ uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg, const struct sockaddr_storage *ss) { @@ -87,16 +87,25 @@ uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg, struct sockaddr_in6 *sin6; uint16_t port = 0; uint8_t *ptr; - uint8_t *old_tail; - uint8_t *tlv_len; + const uint8_t len_tl = 2; + uint8_t len_v = sizeof(port); - OSMO_ASSERT(msg); - OSMO_ASSERT(ss); OSMO_ASSERT(ss->ss_family == AF_INET || ss->ss_family == AF_INET6); + switch (ss->ss_family) { + case AF_INET: + len_v += IP_V4_ADDR_LEN; + break; + case AF_INET6: + len_v += IP_V6_ADDR_LEN; + break; + } + + if (msgb_tailroom(msg) < len_tl + len_v) + return 0; + msgb_put_u8(msg, GSM0808_IE_AOIP_TRASP_ADDR); - tlv_len = msgb_put(msg,1); - old_tail = msg->tail; + msgb_put_u8(msg, len_v); switch (ss->ss_family) { case AF_INET: @@ -114,9 +123,7 @@ uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg, } msgb_put_u16(msg, port); - - *tlv_len = (uint8_t) (msg->tail - old_tail); - return *tlv_len + 2; + return len_tl + len_v; } /*! Decode TS 08.08 AoIP transport address IE @@ -132,7 +139,6 @@ int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss, struct sockaddr_in6 sin6; const uint8_t *old_elem = elem; - OSMO_ASSERT(ss); if (!elem) return -EINVAL; if (len == 0) @@ -180,7 +186,6 @@ int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss, * \returns number of bytes parsed */ int gsm0808_dec_osmux_cid(uint8_t *cid, const uint8_t *elem, uint8_t len) { - OSMO_ASSERT(cid); if (!elem) return -EINVAL; if (len != 1) @@ -204,21 +209,21 @@ static void decode_lai(const uint8_t *data, struct osmo_location_area_id *decode gsm48_decode_lai2(&lai, decoded); } -/* Helper function for gsm0808_enc_speech_codec() - * and gsm0808_enc_speech_codec_list() */ -static uint8_t enc_speech_codec(struct msgb *msg, - const struct gsm0808_speech_codec *sc) +/* Helper function for gsm0808_enc_speech_codec[_list](). + * Returns number of bytes appended; negative on error. */ +static int enc_speech_codec(struct msgb *msg, + const struct gsm0808_speech_codec *sc) { /* See also 3GPP TS 48.008 3.2.2.103 Speech Codec List */ uint8_t header = 0; uint8_t *old_tail; - bool type_extended = false; + bool type_extended; /* Note: Extended codec types are codec types that require 8 instead * of 4 bit to fully specify the selected codec. In the following, * we check if we work with an extended type or not. We also check * if the codec type is valid at all. */ - switch(sc->type) { + switch (sc->type) { case GSM0808_SCT_FR1: case GSM0808_SCT_FR2: case GSM0808_SCT_FR3: @@ -235,8 +240,7 @@ static uint8_t enc_speech_codec(struct msgb *msg, break; default: /* Invalid codec type specified */ - OSMO_ASSERT(false); - break; + return -EINVAL; } old_tail = msg->tail; @@ -251,11 +255,10 @@ static uint8_t enc_speech_codec(struct msgb *msg, header |= (1 << 4); if (type_extended) { - header |= 0x0f; + header |= GSM0808_SCT_EXT; msgb_put_u8(msg, header); msgb_put_u8(msg, sc->type); } else { - OSMO_ASSERT(sc->type < 0x0f); header |= sc->type; msgb_put_u8(msg, header); } @@ -273,11 +276,13 @@ static uint8_t enc_speech_codec(struct msgb *msg, case GSM0808_SCT_FR5: case GSM0808_SCT_HR4: case GSM0808_SCT_CSD: - OSMO_ASSERT((sc->cfg & 0xff00) == 0); + if (sc->cfg >> 8) + return -EINVAL; msgb_put_u8(msg, (uint8_t) sc->cfg & 0xff); break; default: - OSMO_ASSERT(sc->cfg == 0); + if (sc->cfg != 0) + return -EINVAL; break; } @@ -287,27 +292,38 @@ static uint8_t enc_speech_codec(struct msgb *msg, /*! Encode TS 08.08 Speech Codec IE * \param[out] msg Message Buffer to which IE will be appended * \param[in] sc Speech Codec to be encoded into IE - * \returns number of bytes appended to \a msg */ -uint8_t gsm0808_enc_speech_codec(struct msgb *msg, - const struct gsm0808_speech_codec *sc) + * \returns number of bytes appended to \a msg; negative on error */ +int gsm0808_enc_speech_codec2(struct msgb *msg, + const struct gsm0808_speech_codec *sc) { /*! See also 3GPP TS 48.008 3.2.2.103 Speech Codec List */ - uint8_t *old_tail; uint8_t *tlv_len; - - OSMO_ASSERT(msg); - OSMO_ASSERT(sc); + int rc; msgb_put_u8(msg, GSM0808_IE_SPEECH_CODEC); tlv_len = msgb_put(msg, 1); - old_tail = msg->tail; - enc_speech_codec(msg, sc); + rc = enc_speech_codec(msg, sc); + if (rc < 0) + return rc; - *tlv_len = (uint8_t) (msg->tail - old_tail); + *tlv_len = rc; return *tlv_len + 2; } +/*! Deprecated: gsm0808_enc_speech_codec2() wrapper for backwards compatibility. + * Mimics the old behavior: crash on missing and/or invalid input. */ +uint8_t gsm0808_enc_speech_codec(struct msgb *msg, + const struct gsm0808_speech_codec *sc) +{ + int rc; + + rc = gsm0808_enc_speech_codec2(msg, sc); + OSMO_ASSERT(rc > 0); + + return rc; +} + /*! Decode TS 08.08 Speech Codec IE * \param[out] sc Caller-allocated memory for Speech Codec * \param[in] elem IE value to be decoded @@ -320,7 +336,6 @@ int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc, uint8_t header; const uint8_t *old_elem = elem; - OSMO_ASSERT(sc); if (!elem) return -EINVAL; if (len == 0) @@ -332,7 +347,7 @@ int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc, /* An extended codec type needs at least two fields, * bail if the input data length is not sufficient. */ - if ((header & 0x0F) == 0x0F && len < 2) + if ((header & 0x0F) == GSM0808_SCT_EXT && len < 2) return -EINVAL; elem++; @@ -347,7 +362,7 @@ int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc, if (header & (1 << 4)) sc->tf = true; - if ((header & 0x0F) != 0x0F) { + if ((header & 0x0F) != GSM0808_SCT_EXT) { sc->type = (header & 0x0F); } else { sc->type = *elem; @@ -392,35 +407,46 @@ int gsm0808_dec_speech_codec(struct gsm0808_speech_codec *sc, /*! Encode TS 08.08 Speech Codec list * \param[out] msg Message Buffer to which IE is to be appended * \param[in] scl Speech Codec List to be encoded into IE - * \returns number of bytes added to \a msg */ -uint8_t gsm0808_enc_speech_codec_list(struct msgb *msg, - const struct gsm0808_speech_codec_list *scl) + * \returns number of bytes added to \a msg; negative on error */ +int gsm0808_enc_speech_codec_list2(struct msgb *msg, + const struct gsm0808_speech_codec_list *scl) { /*! See also 3GPP TS 48.008 3.2.2.103 Speech Codec List */ uint8_t *old_tail; uint8_t *tlv_len; unsigned int i; - uint8_t rc; unsigned int bytes_used = 0; - OSMO_ASSERT(msg); - OSMO_ASSERT(scl); - msgb_put_u8(msg, GSM0808_IE_SPEECH_CODEC_LIST); tlv_len = msgb_put(msg, 1); old_tail = msg->tail; for (i = 0; i < scl->len; i++) { - rc = enc_speech_codec(msg, &scl->codec[i]); - OSMO_ASSERT(rc >= 1); + int rc = enc_speech_codec(msg, &scl->codec[i]); + if (rc < 1) + return rc; bytes_used += rc; - OSMO_ASSERT(bytes_used <= 255); + if (bytes_used > 0xff) + return -EOVERFLOW; } *tlv_len = (uint8_t) (msg->tail - old_tail); return *tlv_len + 2; } +/*! Deprecated: gsm0808_enc_speech_codec_list2() wrapper for backwards compatibility. + * Mimics the old behavior: crash on missing and/or invalid input. */ +uint8_t gsm0808_enc_speech_codec_list(struct msgb *msg, + const struct gsm0808_speech_codec_list *scl) +{ + int rc; + + rc = gsm0808_enc_speech_codec_list2(msg, scl); + OSMO_ASSERT(rc > 0); + + return rc; +} + /*! Decode TS 08.08 Speech Codec list IE * \param[out] scl Caller-provided memory to store codec list * \param[in] elem IE value to be decoded @@ -435,7 +461,6 @@ int gsm0808_dec_speech_codec_list(struct gsm0808_speech_codec_list *scl, int rc; uint8_t decoded = 0; - OSMO_ASSERT(scl); if (!elem) return -EINVAL; @@ -472,16 +497,8 @@ uint8_t gsm0808_enc_channel_type(struct msgb *msg, uint8_t *old_tail; uint8_t *tlv_len; - OSMO_ASSERT(msg); - OSMO_ASSERT(ct); OSMO_ASSERT(ct->perm_spch_len <= CHANNEL_TYPE_ELEMENT_MAXLEN - 2); - /* FIXME: Implement encoding support for Data - * and Speech + CTM Text Telephony */ - if ((ct->ch_indctr & 0x0f) != GSM0808_CHAN_SPEECH - && (ct->ch_indctr & 0x0f) != GSM0808_CHAN_SIGN) - OSMO_ASSERT(false); - msgb_put_u8(msg, GSM0808_IE_CHANNEL_TYPE); tlv_len = msgb_put(msg, 1); old_tail = msg->tail; @@ -489,12 +506,45 @@ uint8_t gsm0808_enc_channel_type(struct msgb *msg, msgb_put_u8(msg, ct->ch_indctr & 0x0f); msgb_put_u8(msg, ct->ch_rate_type); - for (i = 0; i < ct->perm_spch_len; i++) { - byte = ct->perm_spch[i]; + switch (ct->ch_indctr) { + case GSM0808_CHAN_DATA: + byte = ct->data_rate; + + if (!ct->data_transparent) + byte |= 0x40; /* Set T/NT */ + + if (ct->data_rate_allowed_is_set) { + OSMO_ASSERT(!ct->data_transparent); + byte |= 0x80; /* Set ext */ + msgb_put_u8(msg, byte); - if (i < ct->perm_spch_len - 1) - byte |= 0x80; + byte = ct->data_rate_allowed; + if (ct->data_asym_pref_is_set) { + byte |= 0x80; /* Set ext */ + msgb_put_u8(msg, byte); + + /* Set asymmetry indication, rest is spare */ + byte = ct->data_asym_pref << 5; + } + } msgb_put_u8(msg, byte); + break; + case GSM0808_CHAN_SPEECH: + case GSM0808_CHAN_SPEECH_CTM_TEXT_TELEPHONY: + for (i = 0; i < ct->perm_spch_len; i++) { + byte = ct->perm_spch[i]; + + if (i < ct->perm_spch_len - 1) + byte |= 0x80; + msgb_put_u8(msg, byte); + } + break; + case GSM0808_CHAN_SIGN: + /* Octet 5 is spare */ + msgb_put_u8(msg, 0); + break; + default: + OSMO_ASSERT(false); } *tlv_len = (uint8_t) (msg->tail - old_tail); @@ -514,7 +564,6 @@ int gsm0808_dec_channel_type(struct gsm0808_channel_type *ct, uint8_t byte; const uint8_t *old_elem = elem; - OSMO_ASSERT(ct); if (!elem) return -EINVAL; if (len < 3 || len > 11) @@ -524,17 +573,60 @@ int gsm0808_dec_channel_type(struct gsm0808_channel_type *ct, ct->ch_indctr = (*elem) & 0x0f; elem++; - ct->ch_rate_type = (*elem) & 0x0f; + ct->ch_rate_type = *elem; elem++; - for (i = 0; i < ARRAY_SIZE(ct->perm_spch); i++) { + switch (ct->ch_indctr) { + case GSM0808_CHAN_DATA: byte = *elem; elem++; - ct->perm_spch[i] = byte & 0x7f; - if ((byte & 0x80) == 0x00) - break; + ct->data_transparent = !(byte & 0x40); /* T/NT */ + ct->data_rate = byte & 0x3f; + + /* Optional extension for non-transparent service */ + if (byte & 0x80) { + if (ct->data_transparent) + return -EINVAL; + if (elem - old_elem >= len) + return -EOVERFLOW; + byte = *elem; + elem++; + + ct->data_rate_allowed_is_set = true; + ct->data_rate_allowed = byte & 0x7f; + + /* Optional extension */ + if (byte & 0x80) { + if (elem - old_elem >= len) + return -EOVERFLOW; + byte = *elem; + elem++; + + ct->data_asym_pref_is_set = true; + ct->data_asym_pref = byte & 0x60 >> 5; + } + } + break; + case GSM0808_CHAN_SPEECH: + case GSM0808_CHAN_SPEECH_CTM_TEXT_TELEPHONY: + for (i = 0; i < ARRAY_SIZE(ct->perm_spch); i++) { + if (elem - old_elem >= len) + return -EOVERFLOW; + + byte = *elem; + elem++; + ct->perm_spch[i] = byte & 0x7f; + if ((byte & 0x80) == 0x00) + break; + } + ct->perm_spch_len = i + 1; + break; + case GSM0808_CHAN_SIGN: + /* Octet 5 is spare */ + break; + default: + return -ENOTSUP; } - ct->perm_spch_len = i + 1; return (int)(elem - old_elem); } @@ -703,8 +795,6 @@ uint8_t gsm0808_enc_encrypt_info(struct msgb *msg, uint8_t *old_tail; uint8_t *tlv_len; - OSMO_ASSERT(msg); - OSMO_ASSERT(ei); OSMO_ASSERT(ei->key_len <= ARRAY_SIZE(ei->key)); OSMO_ASSERT(ei->perm_algo_len <= ENCRY_INFO_PERM_ALGO_MAXLEN); @@ -721,6 +811,8 @@ uint8_t gsm0808_enc_encrypt_info(struct msgb *msg, } msgb_put_u8(msg, perm_algo); + /* FIXME: 48.008 3.2.2.10 Encryption Information says: + * "When present, the key shall be 8 octets long." */ ptr = msgb_put(msg, ei->key_len); memcpy(ptr, ei->key, ei->key_len); @@ -741,7 +833,6 @@ int gsm0808_dec_encrypt_info(struct gsm0808_encrypt_info *ei, unsigned int perm_algo_len = 0; const uint8_t *old_elem = elem; - OSMO_ASSERT(ei); if (!elem) return -EINVAL; if (len == 0) @@ -760,6 +851,8 @@ int gsm0808_dec_encrypt_info(struct gsm0808_encrypt_info *ei, } ei->perm_algo_len = perm_algo_len; + /* FIXME: 48.008 3.2.2.10 Encryption Information says: + * "When present, the key shall be 8 octets long." */ ei->key_len = len - 1; memcpy(ei->key, elem, ei->key_len); elem+=ei->key_len; @@ -767,6 +860,80 @@ int gsm0808_dec_encrypt_info(struct gsm0808_encrypt_info *ei, return (int)(elem - old_elem); } +/*! Encode TS 48.008 Kc128 IE. + * \param[out] msg Message Buffer to which IE is to be appended. + * \param[in] kc128 Pointer to 16 bytes of Kc128 key data. + * \returns number of bytes appended to msg */ +int gsm0808_enc_kc128(struct msgb *msg, const uint8_t *kc128) +{ + uint8_t *start = msg->tail; + msgb_tv_fixed_put(msg, GSM0808_IE_KC_128, 16, kc128); + return msg->tail - start; +} + +/*! Decode TS 48.008 Kc128 IE. + * \param[out] kc128 Target buffer for received Kc128 key, 16 bytes long. + * \param[in] elem IE value to be decoded (without IE discriminator). + * \param[in] len Length of elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_kc128(uint8_t *kc128, const uint8_t *elem, uint8_t len) +{ + if (len != 16) + return -EINVAL; + memcpy(kc128, elem, 16); + return len; +} + +/* Store individual Cell Identifier information in a CGI, without clearing the remaining ones. + * This is useful to supplement one CGI with information from more than one Cell Identifier, + * which in turn is useful to match Cell Identifiers of differing kinds to each other. + * Before first invocation, clear the *dst struct externally, this function does only write those members + * that are present in parameter u. + */ +static void cell_id_to_cgi(struct osmo_cell_global_id *dst, + enum CELL_IDENT discr, const union gsm0808_cell_id_u *u) +{ + switch (discr) { + case CELL_IDENT_WHOLE_GLOBAL: + *dst = u->global; + return; + + case CELL_IDENT_WHOLE_GLOBAL_PS: + dst->lai = u->global_ps.rai.lac; + dst->cell_identity = u->global_ps.cell_identity; + return; + + case CELL_IDENT_LAC_AND_CI: + dst->lai.lac = u->lac_and_ci.lac; + dst->cell_identity = u->lac_and_ci.ci; + return; + + case CELL_IDENT_CI: + dst->cell_identity = u->ci; + return; + + case CELL_IDENT_LAI_AND_LAC: + dst->lai = u->lai_and_lac; + return; + + case CELL_IDENT_LAC: + dst->lai.lac = u->lac; + return; + + case CELL_IDENT_SAI: + dst->lai = u->sai.lai; + return; + + case CELL_IDENT_NO_CELL: + case CELL_IDENT_BSS: + case CELL_IDENT_UTRAN_PLMN_LAC_RNC: + case CELL_IDENT_UTRAN_RNC: + case CELL_IDENT_UTRAN_LAC_RNC: + /* No values to set. */ + return; + } +} + /* Return the size of the value part of a cell identifier of given type */ int gsm0808_cell_id_size(enum CELL_IDENT discr) { @@ -784,6 +951,10 @@ int gsm0808_cell_id_size(enum CELL_IDENT discr) case CELL_IDENT_BSS: case CELL_IDENT_NO_CELL: return 0; + case CELL_IDENT_SAI: + return 7; + case CELL_IDENT_WHOLE_GLOBAL_PS: + return 8; default: return -EINVAL; } @@ -828,6 +999,20 @@ int gsm0808_decode_cell_id_u(union gsm0808_cell_id_u *out, enum CELL_IDENT discr case CELL_IDENT_NO_CELL: /* Does not have any list items */ break; + case CELL_IDENT_SAI: + if (len < 7) + return -EINVAL; + decode_lai(buf, &out->sai.lai); + out->sai.sac = osmo_load16be(buf + sizeof(struct gsm48_loc_area_id)); + break; + case CELL_IDENT_WHOLE_GLOBAL_PS: + /* 3GPP TS 48.018 sec 11.3.9 "Cell Identifier" */ + if (len < 8) + return -EINVAL; + decode_lai(buf, (struct osmo_location_area_id *)&out->global_ps.rai); /* rai contains lai + non-decoded rac */ + out->global_ps.rai.rac = *(buf + sizeof(struct gsm48_loc_area_id)); + out->global_ps.cell_identity = osmo_load16be(buf + sizeof(struct gsm48_loc_area_id) + 1); + break; default: /* Remaining cell identification types are not implemented. */ return -EINVAL; @@ -869,6 +1054,26 @@ void gsm0808_msgb_put_cell_id_u(struct msgb *msg, enum CELL_IDENT id_discr, cons case CELL_IDENT_NO_CELL: /* Does not have any list items */ break; + + case CELL_IDENT_SAI: { + const struct osmo_service_area_id *id = &u->sai; + struct gsm48_loc_area_id lai; + gsm48_generate_lai2(&lai, &id->lai); + memcpy(msgb_put(msg, sizeof(lai)), &lai, sizeof(lai)); + msgb_put_u16(msg, id->sac); + break; + } + + case CELL_IDENT_WHOLE_GLOBAL_PS: { + /* 3GPP TS 48.018 sec 11.3.9 "Cell Identifier" */ + const struct osmo_cell_global_id_ps *id = &u->global_ps; + struct gsm48_loc_area_id lai; + gsm48_generate_lai2(&lai, &id->rai.lac); + memcpy(msgb_put(msg, sizeof(lai)), &lai, sizeof(lai)); + memcpy(msgb_put(msg, 1), &id->rai.rac, 1); + msgb_put_u16(msg, id->cell_identity); + break; + } default: /* Support for other identifier list types is not implemented. */ OSMO_ASSERT(false); @@ -885,19 +1090,31 @@ uint8_t gsm0808_enc_cell_id_list2(struct msgb *msg, uint8_t *old_tail; uint8_t *tlv_len; unsigned int i; - - OSMO_ASSERT(msg); - OSMO_ASSERT(cil); + uint8_t id_discr; msgb_put_u8(msg, GSM0808_IE_CELL_IDENTIFIER_LIST); tlv_len = msgb_put(msg, 1); old_tail = msg->tail; - msgb_put_u8(msg, cil->id_discr & 0x0f); + /* CGI-PS is an osmocom-specific type. In here we don't care about the + * PS part, only the CS one. */ + if (cil->id_discr == CELL_IDENT_WHOLE_GLOBAL_PS) + id_discr = CELL_IDENT_WHOLE_GLOBAL; + else + id_discr = cil->id_discr & 0x0f; + + msgb_put_u8(msg, id_discr); OSMO_ASSERT(cil->id_list_len <= GSM0808_CELL_ID_LIST2_MAXLEN); - for (i = 0; i < cil->id_list_len; i++) - gsm0808_msgb_put_cell_id_u(msg, cil->id_discr, &cil->id_list[i]); + for (i = 0; i < cil->id_list_len; i++) { + if (cil->id_discr == CELL_IDENT_WHOLE_GLOBAL_PS) { + union gsm0808_cell_id_u u; + cell_id_to_cgi(&u.global, cil->id_discr, &cil->id_list[i]); + gsm0808_msgb_put_cell_id_u(msg, CELL_IDENT_WHOLE_GLOBAL, &u); + } else { + gsm0808_msgb_put_cell_id_u(msg, cil->id_discr, &cil->id_list[i]); + } + } *tlv_len = (uint8_t) (msg->tail - old_tail); return *tlv_len + 2; @@ -916,9 +1133,6 @@ uint8_t gsm0808_enc_cell_id_list(struct msgb *msg, uint8_t *tlv_len; unsigned int i; - OSMO_ASSERT(msg); - OSMO_ASSERT(cil); - msgb_put_u8(msg, GSM0808_IE_CELL_IDENTIFIER_LIST); tlv_len = msgb_put(msg, 1); old_tail = msg->tail; @@ -1057,6 +1271,31 @@ static int parse_cell_id_lac_list(struct gsm0808_cell_id_list2 *cil, const uint8 return i; } +static int parse_cell_id_sai_list(struct gsm0808_cell_id_list2 *cil, const uint8_t *data, size_t remain, size_t *consumed) +{ + struct osmo_service_area_id *id; + uint16_t *sac_be; + size_t lai_offset; + int i = 0; + const size_t elemlen = sizeof(struct gsm48_loc_area_id) + sizeof(*sac_be); + + *consumed = 0; + while (remain >= elemlen) { + if (i >= GSM0808_CELL_ID_LIST2_MAXLEN) + return -ENOSPC; + id = &cil->id_list[i].sai; + lai_offset = i * elemlen; + decode_lai(&data[lai_offset], &id->lai); + sac_be = (uint16_t *)(&data[lai_offset + sizeof(struct gsm48_loc_area_id)]); + id->sac = osmo_load16be(sac_be); + *consumed += elemlen; + remain -= elemlen; + i++; + } + + return i; +} + /*! Decode Cell Identifier List IE * \param[out] cil Caller-provided memory to store Cell ID list * \param[in] elem IE value to be decoded @@ -1069,7 +1308,6 @@ int gsm0808_dec_cell_id_list2(struct gsm0808_cell_id_list2 *cil, size_t bytes_elem = 0; int list_len = 0; - OSMO_ASSERT(cil); if (!elem) return -EINVAL; if (len == 0) @@ -1101,6 +1339,12 @@ int gsm0808_dec_cell_id_list2(struct gsm0808_cell_id_list2 *cil, case CELL_IDENT_NO_CELL: /* Does not have any list items */ break; + case CELL_IDENT_SAI: + list_len = parse_cell_id_sai_list(cil, elem, len, &bytes_elem); + break; + case CELL_IDENT_UTRAN_PLMN_LAC_RNC: + case CELL_IDENT_UTRAN_RNC: + case CELL_IDENT_UTRAN_LAC_RNC: default: /* Remaining cell identification types are not implemented. */ return -EINVAL; @@ -1131,7 +1375,6 @@ int gsm0808_dec_cell_id_list(struct gsm0808_cell_id_list *cil, const uint8_t *old_elem = elem; unsigned int item_count = 0; - OSMO_ASSERT(cil); if (!elem) return -EINVAL; if (len == 0) @@ -1289,9 +1532,6 @@ uint8_t gsm0808_enc_cell_id(struct msgb *msg, const struct gsm0808_cell_id *ci) .id_list_len = 1, }; - OSMO_ASSERT(msg); - OSMO_ASSERT(ci); - ie_tag = msg->tail; rc = gsm0808_enc_cell_id_list2(msg, &cil); @@ -1498,7 +1738,7 @@ int gsm48_mr_cfg_from_gsm0808_sc_cfg(struct gsm48_multi_rate_conf *cfg, /* Rate 5,15k can never be selected (see table) */ cfg->m5_15 = 0; - if (s15_s0 & GSM0808_SC_CFG_AMR_4_75_5_90_7_40_12_20 & 0xff) { + if (s15_s0 & GSM0808_SC_CFG_AMR_4_75_5_90_7_40_12_20) { /* Table Table 7.11.3.1.3-2 lists one mode that selects 4 * rates at once (Config-NB-Code = 1). The rates selected * are known to be compatible between GERAN and UTRAN, since @@ -1565,18 +1805,7 @@ int gsm48_mr_cfg_from_gsm0808_sc_cfg(struct gsm48_multi_rate_conf *cfg, int gsm0808_get_cipher_reject_cause(const struct tlv_parsed *tp) { - const uint8_t *buf = TLVP_VAL_MINLEN(tp, GSM0808_IE_CAUSE, 1); - - if (!buf) - return -EBADMSG; - - if (TLVP_LEN(tp, GSM0808_IE_CAUSE) > 1) { - if (!gsm0808_cause_ext(buf[0])) - return -EINVAL; - return buf[1]; - } - - return buf[0]; + return gsm0808_get_cause(tp); } /*! Print a human readable name of the cell identifier to the char buffer. @@ -1603,6 +1832,10 @@ int gsm0808_cell_id_u_name(char *buf, size_t buflen, return snprintf(buf, buflen, "%s", osmo_lai_name(&u->lai_and_lac)); case CELL_IDENT_WHOLE_GLOBAL: return snprintf(buf, buflen, "%s", osmo_cgi_name(&u->global)); + case CELL_IDENT_WHOLE_GLOBAL_PS: + return snprintf(buf, buflen, "%s", osmo_cgi_ps_name(&u->global_ps)); + case CELL_IDENT_SAI: + return snprintf(buf, buflen, "%s", osmo_sai_name(&u->sai)); default: /* For CELL_IDENT_BSS and CELL_IDENT_NO_CELL, just print the discriminator. * Same for kinds we have no string representation of yet. */ @@ -1610,47 +1843,6 @@ int gsm0808_cell_id_u_name(char *buf, size_t buflen, } } -/* Store individual Cell Identifier information in a CGI, without clearing the remaining ones. - * This is useful to supplement one CGI with information from more than one Cell Identifier, - * which in turn is useful to match Cell Identifiers of differing kinds to each other. - * Before first invocation, clear the *dst struct externally, this function does only write those members - * that are present in parameter u. - */ -static void cell_id_to_cgi(struct osmo_cell_global_id *dst, - enum CELL_IDENT discr, const union gsm0808_cell_id_u *u) -{ - switch (discr) { - case CELL_IDENT_WHOLE_GLOBAL: - *dst = u->global; - return; - - case CELL_IDENT_LAC_AND_CI: - dst->lai.lac = u->lac_and_ci.lac; - dst->cell_identity = u->lac_and_ci.ci; - return; - - case CELL_IDENT_CI: - dst->cell_identity = u->ci; - return; - - case CELL_IDENT_LAI_AND_LAC: - dst->lai = u->lai_and_lac; - return; - - case CELL_IDENT_LAC: - dst->lai.lac = u->lac; - return; - - case CELL_IDENT_NO_CELL: - case CELL_IDENT_BSS: - case CELL_IDENT_UTRAN_PLMN_LAC_RNC: - case CELL_IDENT_UTRAN_RNC: - case CELL_IDENT_UTRAN_LAC_RNC: - /* No values to set. */ - return; - } -} - /*! Return true if the common information between the two Cell Identifiers match. * For example, if a LAC+CI is compared to LAC, return true if the LAC are the same. * Note that CELL_IDENT_NO_CELL will always return false. @@ -1773,6 +1965,14 @@ void gsm0808_cell_id_from_cgi(struct gsm0808_cell_id *cid, enum CELL_IDENT id_di cid->id.global = *cgi; return; + case CELL_IDENT_WHOLE_GLOBAL_PS: + cid->id.global_ps = (struct osmo_cell_global_id_ps){ + .rai = { + .lac = cgi->lai, + }, + .cell_identity = cgi->cell_identity, + }; + return; case CELL_IDENT_LAC_AND_CI: cid->id.lac_and_ci = (struct osmo_lac_and_ci_id){ .lac = cgi->lai.lac, @@ -1792,6 +1992,12 @@ void gsm0808_cell_id_from_cgi(struct gsm0808_cell_id *cid, enum CELL_IDENT id_di cid->id.lac = cgi->lai.lac; return; + case CELL_IDENT_SAI: + cid->id.sai = (struct osmo_service_area_id){ + .lai = cgi->lai, + }; + return; + case CELL_IDENT_NO_CELL: case CELL_IDENT_BSS: case CELL_IDENT_UTRAN_PLMN_LAC_RNC: @@ -1816,6 +2022,11 @@ int gsm0808_cell_id_to_cgi(struct osmo_cell_global_id *cgi, const struct gsm0808 *cgi = cid->id.global; return OSMO_CGI_PART_PLMN | OSMO_CGI_PART_LAC | OSMO_CGI_PART_CI; + case CELL_IDENT_WHOLE_GLOBAL_PS: + cgi->lai = cid->id.global_ps.rai.lac; + cgi->cell_identity = cid->id.global_ps.cell_identity; + return OSMO_CGI_PART_PLMN | OSMO_CGI_PART_LAC | OSMO_CGI_PART_CI; + case CELL_IDENT_LAC_AND_CI: cgi->lai.lac = cid->id.lac_and_ci.lac; cgi->cell_identity = cid->id.lac_and_ci.ci; @@ -1833,6 +2044,10 @@ int gsm0808_cell_id_to_cgi(struct osmo_cell_global_id *cgi, const struct gsm0808 cgi->lai.lac = cid->id.lac; return OSMO_CGI_PART_LAC; + case CELL_IDENT_SAI: + cgi->lai = cid->id.sai.lai; + return OSMO_CGI_PART_PLMN | OSMO_CGI_PART_LAC; + case CELL_IDENT_NO_CELL: case CELL_IDENT_BSS: case CELL_IDENT_UTRAN_PLMN_LAC_RNC: @@ -1855,28 +2070,17 @@ const struct value_string gsm0808_cell_id_discr_names[] = { { CELL_IDENT_UTRAN_PLMN_LAC_RNC, "UTRAN-PLMN-LAC-RNC" }, { CELL_IDENT_UTRAN_RNC, "UTRAN-RNC" }, { CELL_IDENT_UTRAN_LAC_RNC, "UTRAN-LAC-RNC" }, + { CELL_IDENT_SAI, "SAI" }, + { CELL_IDENT_WHOLE_GLOBAL_PS, "CGI-PS"}, { 0, NULL } }; -#define APPEND_THING(func, args...) do { \ - int remain = buflen - (pos - buf); \ - int l = func(pos, remain, ##args); \ - if (l < 0 || l > remain) \ - pos = buf + buflen; \ - else \ - pos += l; \ - if (l > 0) \ - total_len += l; \ - } while(0) -#define APPEND_STR(fmt, args...) APPEND_THING(snprintf, fmt, ##args) -#define APPEND_CELL_ID_U(DISCR, U) APPEND_THING(gsm0808_cell_id_u_name, DISCR, U) - char *gsm0808_cell_id_name_buf(char *buf, size_t buflen, const struct gsm0808_cell_id *cid) { - char *pos = buf; - int total_len = 0; - APPEND_STR("%s:", gsm0808_cell_id_discr_name(cid->id_discr)); - APPEND_CELL_ID_U(cid->id_discr, &cid->id); + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + + OSMO_STRBUF_PRINTF(sb, "%s:", gsm0808_cell_id_discr_name(cid->id_discr)); + OSMO_STRBUF_APPEND(sb, gsm0808_cell_id_u_name, cid->id_discr, &cid->id); return buf; } @@ -1923,30 +2127,31 @@ char *gsm0808_cell_id_name_c(const void *ctx, const struct gsm0808_cell_id *cid) */ int gsm0808_cell_id_list_name_buf(char *buf, size_t buflen, const struct gsm0808_cell_id_list2 *cil) { - char *pos = buf; - int total_len = 0; - int i; + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; - APPEND_STR("%s[%u]", gsm0808_cell_id_discr_name(cil->id_discr), cil->id_list_len); + OSMO_STRBUF_PRINTF(sb, "%s[%u]", + gsm0808_cell_id_discr_name(cil->id_discr), + cil->id_list_len); switch (cil->id_discr) { case CELL_IDENT_BSS: case CELL_IDENT_NO_CELL: - return total_len; + return sb.chars_needed; default: break; } - APPEND_STR(":{"); + OSMO_STRBUF_PRINTF(sb, ":{"); - for (i = 0; i < cil->id_list_len; i++) { + for (unsigned int i = 0; i < cil->id_list_len; i++) { if (i) - APPEND_STR(", "); - APPEND_CELL_ID_U(cil->id_discr, &cil->id_list[i]); + OSMO_STRBUF_PRINTF(sb, ", "); + OSMO_STRBUF_APPEND(sb, gsm0808_cell_id_u_name, + cil->id_discr, &cil->id_list[i]); } - APPEND_STR("}"); - return total_len; + OSMO_STRBUF_PRINTF(sb, "}"); + return sb.chars_needed; } /*! Return a human-readable representation of \a cil in a static buffer. @@ -1968,9 +2173,6 @@ char *gsm0808_cell_id_list_name_c(const void *ctx, const struct gsm0808_cell_id_ return buf; } -#undef APPEND_STR -#undef APPEND_CELL_ID_U - char *gsm0808_channel_type_name_buf(char *buf, size_t buf_len, const struct gsm0808_channel_type *ct) { snprintf(buf, buf_len, "ch_indctr=0x%x ch_rate_type=0x%x perm_spch=%s", @@ -1993,4 +2195,367 @@ char *gsm0808_channel_type_name_c(const void *ctx, const struct gsm0808_channel_ return gsm0808_channel_type_name_buf(buf, 128, ct); } +/*! Encode Group Call Reference IE (3GPP TS 48.008 3.2.2.55). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] gc Group Call Reference to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_group_callref(struct msgb *msg, const struct gsm0808_group_callref *gc) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + + OSMO_ASSERT(msg); + OSMO_ASSERT(gc); + + msgb_put_u8(msg, GSM0808_IE_GROUP_CALL_REFERENCE); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + memcpy(msgb_put(msg, sizeof(*gc)), gc, sizeof(*gc)); + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode Group Call Reference IE (3GPP TS 48.008 3.2.2.55). + * \param[out] gc Group Call Reference structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_group_callref(struct gsm0808_group_callref *gc, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(gc); + OSMO_ASSERT(elem); + + if (len != sizeof(*gc)) + return -EINVAL; + + memcpy(gc, elem, sizeof(*gc)); + + return len; +} + +/*! Encode Priority IE (3GPP TS 48.008 3.2.2.18). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] pri Priority to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_priority(struct msgb *msg, const struct gsm0808_priority *pri) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + + OSMO_ASSERT(msg); + OSMO_ASSERT(pri); + + msgb_put_u8(msg, GSM0808_IE_PRIORITY); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + memcpy(msgb_put(msg, sizeof(*pri)), pri, sizeof(*pri)); + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode Priority IE (3GPP TS 48.008 3.2.2.18). + * \param[out] pri Priority structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_priority(struct gsm0808_priority *pri, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(pri); + OSMO_ASSERT(elem); + + if (len != sizeof(*pri)) + return -EINVAL; + + memcpy(pri, elem, sizeof(*pri)); + + return len; +} + +/*! Encode VGCS Feature Flags IE (3GPP TS 48.008 3.2.2.88). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] ff VGCS Feature Flags to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_vgcs_feature_flags(struct msgb *msg, const struct gsm0808_vgcs_feature_flags *ff) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + + OSMO_ASSERT(msg); + OSMO_ASSERT(ff); + + msgb_put_u8(msg, GSM0808_IE_VGCS_FEATURE_FLAGS); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + memcpy(msgb_put(msg, sizeof(*ff)), ff, sizeof(*ff)); + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode VGCS Feature Flags IE (3GPP TS 48.008 3.2.2.88). + * \param[out] ff VGCS Feature Flags structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_vgcs_feature_flags(struct gsm0808_vgcs_feature_flags *ff, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(ff); + OSMO_ASSERT(elem); + + if (len != sizeof(*ff)) + return -EINVAL; + + memcpy(ff, elem, sizeof(*ff)); + + return len; +} + +/*! Encode Data Identity IE (3GPP TS 48.008 3.2.2.99). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] di Data Identity to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_data_identity(struct msgb *msg, const struct gsm0808_data_identity *ti) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + + OSMO_ASSERT(msg); + OSMO_ASSERT(ti); + + msgb_put_u8(msg, GSM0808_IE_DATA_IDENTITY); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + memcpy(msgb_put(msg, sizeof(*ti)), ti, sizeof(*ti)); + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode Data Identity IE (3GPP TS 48.008 3.2.2.99). + * \param[out] di Data Identity structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_data_identity(struct gsm0808_data_identity *ti, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(ti); + OSMO_ASSERT(elem); + + if (len < sizeof(*ti)) + return -EINVAL; + + memcpy(ti, elem, sizeof(*ti)); + + return len; +} + +/*! Encode MSISDN IE (3GPP TS 48.008 3.2.2.101). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] msisdn MSISDN to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_msisdn(struct msgb *msg, const char *msisdn) +{ + uint8_t *tlv_len; + uint8_t bcd_lv[11]; + int rc; + + OSMO_ASSERT(msg); + OSMO_ASSERT(msisdn); + + rc = gsm48_encode_bcd_number(bcd_lv, sizeof(bcd_lv), 0, msisdn); + if (rc < 1) + return 0; + + msgb_put_u8(msg, GSM0808_IE_MSISDN); + tlv_len = msgb_put(msg, rc); + + memcpy(tlv_len, bcd_lv, rc); + + *tlv_len = rc - 1; + return *tlv_len + 2; +} + +/*! Decode MSISDN IE (3GPP TS 48.008 3.2.2.101). + * \param[out] msisdn MSISDN structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_msisdn(char *msisdn, const char *elem, uint8_t len) +{ + OSMO_ASSERT(msisdn); + OSMO_ASSERT(elem); + + if (len < 1) + return -EINVAL; + + gsm48_decode_bcd_number(msisdn, MSISDN_MAXLEN + 1, (uint8_t *)(elem - 1), 0); + + return len; +} + +/*! Encode Talker Identity IE (3GPP TS 48.008 3.2.2.91). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] ti Talker Identity to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_talker_identity(struct msgb *msg, const struct gsm0808_talker_identity *ti) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + uint8_t *ptr; + unsigned int bytes; + + OSMO_ASSERT(msg); + OSMO_ASSERT(ti); + + msgb_put_u8(msg, GSM0808_IE_TALKER_IDENTITY); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + bytes = (ti->id_bits + 7) >> 3; + ptr = msgb_put(msg, 1 + bytes); + ptr[0] = -ti->id_bits & 0x7; + memcpy(ptr + 1, ti->talker_id, bytes); + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode Talker Identity IE (3GPP TS 48.008 3.2.2.91). + * \param[out] ti Talker Identity structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_talker_identity(struct gsm0808_talker_identity *ti, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(ti); + OSMO_ASSERT(elem); + + if (len < 2) + return -EINVAL; + unsigned int bytes; + + bytes = len - 1; + if (bytes > TALKER_IDENTITY_MAXLEN) + return -EINVAL; + ti->id_bits = (bytes << 3) - (elem[0] & 0x7); + memcpy(ti->talker_id, elem + 1, bytes); + + return len; +} + +/*! Encode Assignment Requirements IE (3GPP TS 48.008 3.2.2.52). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] ar Assignment Requirement to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_assign_req(struct msgb *msg, const enum gsm0808_assignment_requirement ar) +{ + uint8_t *ptr; + + OSMO_ASSERT(msg); + + msgb_put_u8(msg, GSM0808_IE_ASSIGNMENT_REQUIREMENT); + + ptr = msgb_put(msg, 1); + ptr[0] = ar; + + return 2; +} + +/*! Decode Assignment Requirements IE (3GPP TS 48.008 3.2.2.52). + * \param[out] ar Assignment Requirements enum to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_assign_req(enum gsm0808_assignment_requirement *ar, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(ar); + OSMO_ASSERT(elem); + + if (len != 1) + return -EINVAL; + + *ar = elem[0]; + + return 1; +} + +/*! Encode Cell Identifier List Segment IE (3GPP TS 48.008 3.2.2.27a). + * \param[out] msg Message Buffer to which IE is to be appended + * \param[in] ie_type Type of IE to use (5 different lists are specified.) + * \param[in] ci Cell Identifier List Segment to be encoded + * \returns number of bytes appended to \a msg */ +uint8_t gsm0808_enc_cell_id_list_segment(struct msgb *msg, uint8_t ie_type, + const struct gsm0808_cell_id_list_segment *ci) +{ + uint8_t *old_tail; + uint8_t *tlv_len; + int rc; + + OSMO_ASSERT(msg); + OSMO_ASSERT(ci); + + msgb_put_u8(msg, ie_type); + tlv_len = msgb_put(msg, 1); + old_tail = msg->tail; + + msgb_put_u8(msg, (ci->seq_last << 4) | ci->seq_number); + + rc = gsm0808_enc_cell_id_list2(msg, &ci->cil); + if (rc <= 0) + return rc; + + *tlv_len = (uint8_t) (msg->tail - old_tail); + return *tlv_len + 2; +} + +/*! Decode Cell Identifier List Segment IE (3GPP TS 48.008 3.2.2.27a). + * \param[out] ci Cell Identifier List Segment structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_cell_id_list_segment(struct gsm0808_cell_id_list_segment *ci, const uint8_t *elem, uint8_t len) +{ + int rc; + + OSMO_ASSERT(ci); + OSMO_ASSERT(elem); + + if (len < 1) + return -EINVAL; + + ci->seq_last = elem[0] >> 4; + ci->seq_number = elem[0] & 0x0f; + + rc = gsm0808_dec_cell_id_list2(&ci->cil, elem + 1, len - 1); + if (rc < 0) + return rc; + + return len; +} + +/*! Decode Call Identifier IE (3GPP TS 48.008 3.2.2.105). + * \param[out] ci Call Identifier structure to store data + * \param[in] elem IE value to be decoded. + * \param[in] len Length of \a elem in bytes. + * \returns number of bytes parsed; negative on error */ +int gsm0808_dec_call_id(uint32_t *ci, const uint8_t *elem, uint8_t len) +{ + OSMO_ASSERT(ci); + OSMO_ASSERT(elem); + + if (len != 4) + return -EINVAL; + + /* The Call Identifier is stored as little endian. */ + *ci = osmo_load32le(elem); + + return 4; +} + /*! @} */ diff --git a/src/gsm/gsm23003.c b/src/gsm/gsm23003.c index e20afcbc..1eed41fe 100644 --- a/src/gsm/gsm23003.c +++ b/src/gsm/gsm23003.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <ctype.h> @@ -247,6 +243,43 @@ char *osmo_lai_name_c(const void *ctx, const struct osmo_location_area_id *lai) return osmo_lai_name_buf(buf, 32, lai); } +/*! Return MCC-MNC-LAC-RAC as string, in caller-provided output buffer. + * \param[out] buf caller-allocated output buffer + * \param[in] buf_len size of buf in bytes + * \param[in] rai RAI to encode, the rac member is ignored. + * \returns buf + */ +char *osmo_rai_name2_buf(char *buf, size_t buf_len, const struct osmo_routing_area_id *rai) +{ + char plmn[16]; + snprintf(buf, buf_len, "%s-%u-%u", osmo_plmn_name_buf(plmn, sizeof(plmn), + &rai->lac.plmn), rai->lac.lac, rai->rac); + return buf; +} + +/*! Return MCC-MNC-LAC-RAC as string, in a static buffer. + * \param[in] rai RAI to encode, the rac member is ignored. + * \returns Static string buffer. + */ +const char *osmo_rai_name2(const struct osmo_routing_area_id *rai) +{ + static __thread char buf[32]; + return osmo_rai_name2_buf(buf, sizeof(buf), rai); +} + +/*! Return MCC-MNC-LAC-RAC as string, in a talloc-allocated output buffer. + * \param[in] ctx talloc context from which to allocate output buffer + * \param[in] rai RAI to encode, the rac member is ignored. + * \returns string representation of lai in dynamically allocated buffer. + */ +char *osmo_rai_name2_c(const void *ctx, const struct osmo_routing_area_id *rai) +{ + char *buf = talloc_size(ctx, 32); + if (!buf) + return NULL; + return osmo_rai_name2_buf(buf, 32, rai); +} + /*! Return MCC-MNC-LAC-CI as string, in caller-provided output buffer. * \param[out] buf caller-allocated output buffer * \param[in] buf_len size of buf in bytes @@ -291,6 +324,95 @@ char *osmo_cgi_name_c(const void *ctx, const struct osmo_cell_global_id *cgi) return osmo_cgi_name_buf(buf, 32, cgi); } +/*! Return MCC-MNC-LAC-RAC-CI as string, in caller-provided output buffer. + * \param[out] buf caller-allocated output buffer + * \param[in] buf_len size of buf in bytes + * \param[in] cgi_ps CGI-PS to encode. + * \returns buf + */ +char *osmo_cgi_ps_name_buf(char *buf, size_t buf_len, const struct osmo_cell_global_id_ps *cgi_ps) +{ + snprintf(buf, buf_len, "%s-%u", osmo_rai_name2(&cgi_ps->rai), cgi_ps->cell_identity); + return buf; +} + +/*! Return MCC-MNC-LAC-RAC-CI as string, in a static buffer. + * \param[in] cgi_ps CGI-PS to encode. + * \returns Static string buffer. + */ +const char *osmo_cgi_ps_name(const struct osmo_cell_global_id_ps *cgi_ps) +{ + static __thread char buf[32]; + return osmo_cgi_ps_name_buf(buf, sizeof(buf), cgi_ps); +} + +/*! Same as osmo_cgi_ps_name(), but uses a different static buffer. + * Useful for printing two distinct CGI-PSs in the same printf format. + * \param[in] cgi CGI-PS to encode. + * \returns Static string buffer. + */ +const char *osmo_cgi_ps_name2(const struct osmo_cell_global_id_ps *cgi_ps) +{ + static __thread char buf[32]; + return osmo_cgi_ps_name_buf(buf, sizeof(buf), cgi_ps); +} + +/*! Return MCC-MNC-LAC-RAC-CI as string, in a talloc-allocated output buffer. + * \param[in] ctx talloc context from which to allocate output buffer + * \param[in] cgi_ps CGI-PS to encode. + * \returns string representation of CGI in dynamically-allocated buffer. + */ +char *osmo_cgi_ps_name_c(const void *ctx, const struct osmo_cell_global_id_ps *cgi_ps) +{ + char *buf = talloc_size(ctx, 32); + return osmo_cgi_ps_name_buf(buf, 32, cgi_ps); +} + +/*! Return MCC-MNC-LAC-SAC as string, in caller-provided output buffer. + * \param[out] buf caller-allocated output buffer + * \param[in] buf_len size of buf in bytes + * \param[in] sai SAI to encode. + * \returns buf + */ +char *osmo_sai_name_buf(char *buf, size_t buf_len, const struct osmo_service_area_id *sai) +{ + snprintf(buf, buf_len, "%s-%u", osmo_lai_name(&sai->lai), sai->sac); + return buf; +} + +/*! Return MCC-MNC-LAC-SAC as string, in a static buffer. + * \param[in] sai SAI to encode. + * \returns Static string buffer. + */ +const char *osmo_sai_name(const struct osmo_service_area_id *sai) +{ + static __thread char buf[32]; + return osmo_sai_name_buf(buf, sizeof(buf), sai); +} + +/*! Same as osmo_cgi_name(), but uses a different static buffer. + * Useful for printing two distinct CGIs in the same printf format. + * \param[in] sai SAI to encode. + * \returns Static string buffer. + */ +const char *osmo_sai_name2(const struct osmo_service_area_id *sai) +{ + static __thread char buf[32]; + return osmo_sai_name_buf(buf, sizeof(buf), sai); +} + +/*! Return MCC-MNC-LAC-SAC as string, in a talloc-allocated output buffer. + * \param[in] ctx talloc context from which to allocate output buffer + * \param[in] sai SAI to encode. + * \returns string representation of CGI in dynamically-allocated buffer. + */ +char *osmo_sai_name_c(const void *ctx, const struct osmo_service_area_id *sai) +{ + char *buf = talloc_size(ctx, 32); + return osmo_sai_name_buf(buf, 32, sai); +} + + static void to_bcd(uint8_t *bcd, uint16_t val) { bcd[2] = val % 10; @@ -368,14 +490,12 @@ void osmo_plmn_to_bcd(uint8_t *bcd_dst, const struct osmo_plmn_id *plmn) } } -/* Convert given 3-byte BCD buffer to integers and write results to *mcc and - * *mnc. The first three BCD digits result in the MCC and the remaining ones in - * the MNC. Return mnc_3_digits as false if the MNC's most significant digit is encoded as 0xF, true - * otherwise; i.e. true if MNC > 99 or if it is represented with leading zeros instead of 0xF. +/* Convert given 3-byte BCD buffer to integers and write results to plmn->mcc and plmn->mnc. The first three BCD digits + * result in the MCC and the remaining ones in the MNC. Set plmn->mnc_3_digits as false if the MNC's most significant + * digit is encoded as 0xF, true otherwise; i.e. true if MNC > 99 or if it is represented with leading zeros instead of + * 0xF. * \param[in] bcd_src 3-byte BCD buffer containing MCC+MNC representations. - * \param[out] mcc MCC result buffer, or NULL. - * \param[out] mnc MNC result buffer, or NULL. - * \param[out] mnc_3_digits Result buffer for 3-digit flag, or NULL. + * \param[out] plmn user provided memory to store the result. */ void osmo_plmn_from_bcd(const uint8_t *bcd_src, struct osmo_plmn_id *plmn) { @@ -406,22 +526,23 @@ void osmo_plmn_from_bcd(const uint8_t *bcd_src, struct osmo_plmn_id *plmn) */ int osmo_mnc_from_str(const char *mnc_str, uint16_t *mnc, bool *mnc_3_digits) { - long int _mnc = 0; + int _mnc = 0; bool _mnc_3_digits = false; - char *endptr; int rc = 0; if (!mnc_str || !isdigit((unsigned char)mnc_str[0]) || strlen(mnc_str) > 3) return -EINVAL; - errno = 0; - _mnc = strtol(mnc_str, &endptr, 10); - if (errno) - rc = -errno; - else if (*endptr) + rc = osmo_str_to_int(&_mnc, mnc_str, 10, 0, 999); + /* Heed the API definition to return -EINVAL in case of surplus chars */ + if (rc == -E2BIG) return -EINVAL; - if (_mnc < 0 || _mnc > 999) - return -ERANGE; + /* Heed the API definition to always return negative errno */ + if (rc > 0) + return -rc; + if (rc < 0) + return rc; + _mnc_3_digits = strlen(mnc_str) > 2; if (mnc) @@ -484,6 +605,23 @@ int osmo_lai_cmp(const struct osmo_location_area_id *a, const struct osmo_locati return 0; } +/* Compare two RAI. + * The order of comparison is MCC, MNC, LAC, RAC. See also osmo_lai_cmp(). + * \param a[in] "Left" side RAI. + * \param b[in] "Right" side RAI. + * \returns 0 if the RAI are equal, -1 if a < b, 1 if a > b. */ +int osmo_rai_cmp(const struct osmo_routing_area_id *a, const struct osmo_routing_area_id *b) +{ + int rc = osmo_lai_cmp(&a->lac, &b->lac); + if (rc) + return rc; + if (a->rac < b->rac) + return -1; + if (a->rac > b->rac) + return 1; + return 0; +} + /* Compare two CGI. * The order of comparison is MCC, MNC, LAC, CI. See also osmo_lai_cmp(). * \param a[in] "Left" side CGI. @@ -501,6 +639,23 @@ int osmo_cgi_cmp(const struct osmo_cell_global_id *a, const struct osmo_cell_glo return 0; } +/* Compare two CGI-PS. + * The order of comparison is MCC, MNC, LAC, RAC, CI. See also osmo_rai_cmp(). + * \param a[in] "Left" side CGI-PS. + * \param b[in] "Right" side CGI-PS. + * \returns 0 if the CGI are equal, -1 if a < b, 1 if a > b. */ +int osmo_cgi_ps_cmp(const struct osmo_cell_global_id_ps *a, const struct osmo_cell_global_id_ps *b) +{ + int rc = osmo_rai_cmp(&a->rai, &b->rai); + if (rc) + return rc; + if (a->cell_identity < b->cell_identity) + return -1; + if (a->cell_identity > b->cell_identity) + return 1; + return 0; +} + /*! Generate TS 23.003 Section 19.2 Home Network Realm/Domain (text form) * \param out[out] caller-provided output buffer, at least 33 bytes long * \param plmn[in] Osmocom representation of PLMN ID (MCC + MNC) diff --git a/src/gsm/gsm23236.c b/src/gsm/gsm23236.c new file mode 100644 index 00000000..4a83ec82 --- /dev/null +++ b/src/gsm/gsm23236.c @@ -0,0 +1,542 @@ +/*! \file gsm23236.c + * Utility function implementations related to 3GPP TS 23.236 */ +/* + * (C) 2020 sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Author: Neels Hofmeyr <nhofmeyr@sysmocom.de> + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <errno.h> +#include <stdlib.h> + +#include <osmocom/core/utils.h> +#include <osmocom/gsm/gsm23236.h> + +/*! Validate that the given NRI is valid for a given nri_bitlen range. + * \param[in] nri_v NRI value to validate. + * \param[in] nri_bitlen Valid NRI range in nr of bits used; if nri_bitlen > OSMO_NRI_BITLEN_MAX, nri_v is only + * checked to not be marked invalid. + * \returns 0 if valid, <0 if the NRI is <0, >0 if the NRI surpasses the range. + */ +int osmo_nri_v_validate(int16_t nri_v, uint8_t nri_bitlen) +{ + if (nri_v < 0) + return -1; + if (nri_bitlen < OSMO_NRI_BITLEN_MIN) + return 1; + if (nri_bitlen < OSMO_NRI_BITLEN_MAX && (nri_v >> nri_bitlen)) + return 1; + return 0; +} + +/*! Match NRI value against a list NRI ranges. */ +static bool nri_v_matches_range(const struct osmo_nri_range *range, int16_t nri_v) +{ + return range && nri_v >= range->first && nri_v <= range->last; +} + +/*! Return true if the ranges overlap, i.e. one or more NRI values appear in both ranges. */ +static bool nri_range_overlaps_range(const struct osmo_nri_range *a, const struct osmo_nri_range *b) +{ + return nri_v_matches_range(b, a->first) || nri_v_matches_range(b, a->last) + || nri_v_matches_range(a, b->first) || nri_v_matches_range(a, b->last); +} + +/*! Return true if the ranges overlap or are directly adjacent to each other. */ +static bool nri_range_touches(const struct osmo_nri_range *a, const struct osmo_nri_range *b) +{ + /* The first > last check may seem redundant, but ensures integer overflow safety. */ + return nri_range_overlaps_range(a, b) + || (a->first > b->last && a->first == b->last + 1) + || (b->first > a->last && b->first == a->last + 1); +} + +/*! Grow target range to also span range 'add'. Only useful for touching ranges, since all values between the two ranges + * are also included. */ +static void nri_range_extend(struct osmo_nri_range *target, const struct osmo_nri_range *add) +{ + target->first = OSMO_MIN(target->first, add->first); + target->last = OSMO_MAX(target->last, add->last); +} + +/*! Return true when the given NRI value appears in the list of NRI ranges. + * \param[in] nri_v NRI value to look for. + * \param[in] nri_ranges List NRI ranges. + * \returns true iff nri_v appears anywhere in nri_ranges. + */ +bool osmo_nri_v_matches_ranges(int16_t nri_v, const struct osmo_nri_ranges *nri_ranges) +{ + struct osmo_nri_range *range; + if (!nri_ranges) + return false; + llist_for_each_entry(range, &nri_ranges->entries, entry) { + if (nri_v_matches_range(range, nri_v)) + return true; + } + return false; +} + +/*! Modulo and shift the given NRI value so that it becomes a value present in a list of NRI ranges. + * Only range values within nri_bitlen are used. + * \param[inout] nri_v The NRI value to limit, e.g. random bits or an increment counter value. + * \param[in] nri_ranges List of NRI ranges indicating valid NRI values, where no entries may overlap in range values, + * and all entries must be valid (first <= last). + * \returns 0 on success, negative on error. + */ +int osmo_nri_v_limit_by_ranges(int16_t *nri_v, const struct osmo_nri_ranges *nri_ranges, uint32_t nri_bitlen) +{ + struct osmo_nri_range *range; + uint32_t total_values = 0; + int16_t v = *nri_v; + int16_t range_max = (((int16_t)1) << nri_bitlen) - 1; + + if (v < 0 || !nri_ranges) + return -1; + + /* Sum up total amount of range values */ + llist_for_each_entry(range, &nri_ranges->entries, entry) { + if (osmo_nri_range_validate(range, 255)) + return -1; + if (range->first > range_max) + continue; + total_values += OSMO_MIN(range_max, range->last) - range->first + 1; + } + + /* Modulo the given NRI value by that, and pick that nth value from the given ranges. + * (nri_ranges is pretty much guaranteed to be sorted and range_max checks thus would no longer be needed, but + * just check them anyway.) */ + v %= total_values; + llist_for_each_entry(range, &nri_ranges->entries, entry) { + uint32_t len; + if (range->first > range_max) + continue; + len = OSMO_MIN(range_max, range->last) - range->first + 1; + if (v < len) { + *nri_v = range->first + v; + return 0; + } + v -= len; + } + + /* Nothing found -- there are no entires or my math is off. */ + return -1; +} + +/*! Retrieve the Network Resource Indicator bits from a TMSI or p-TMSI. + * Useful for MSC pooling as described by 3GPP TS 23.236. + * \param[out] nri_v Write the extracted NRI value to this location (if non-NULL). If 0 is returned, it is guaranteed + * that nri_v >= 0. On non-zero return code, nri_v == -1. + * \param[in] tmsi TMSI value containing NRI bits. + * \param[in] nri_bitlen Length of the NRI value in number of bits, + * OSMO_NRI_BITLEN_MIN <= nri_bitlen <= * OSMO_NRI_BITLEN_MAX. + * \return 0 on success, negative on error (i.e. if nri_bitlen is not in the valid range). + */ +int osmo_tmsi_nri_v_get(int16_t *nri_v, uint32_t tmsi, uint8_t nri_bitlen) +{ + uint8_t lowest_bit; + if (nri_v) + *nri_v = -1; + if (nri_bitlen < OSMO_NRI_BITLEN_MIN || nri_bitlen > OSMO_NRI_BITLEN_MAX) + return -1; + /* If not interested in the NRI value, exit here. */ + if (!nri_v) + return 0; + /* According to 3GPP TS 23.236, the most significant bit of the NRI is always bit 23. + * (So this is not a temporary placeholder 23 we sometimes like to use, it is an actually specified 23!) */ + lowest_bit = 23 - (nri_bitlen - 1); + /* ????xxxxxx??????? -> 0000000????xxxxxx tmsi >> lowest_bit + * -> xxxxxx & (bitmask that is nri_bitlen bits wide) + */ + *nri_v = (tmsi >> lowest_bit) & ((((uint32_t)1) << nri_bitlen) - 1); + return 0; +} + +/*! Write Network Resource Indicator bits into a TMSI or p-TMSI. + * Overwrite the NRI bits with a given NRI value in a TMSI or p-TMSI. + * Useful for MSC pooling as described by 3GPP TS 23.236. + * \param[inout] tmsi A base TMSI or p-TMSI to replace the NRI value in, result is written back to this location. + * \param[in] nri_v The NRI value to place in the tmsi. + * \param[in] nri_bitlen Length of the NRI value in number of bits, + * OSMO_NRI_BITLEN_MIN <= nri_bitlen <= * OSMO_NRI_BITLEN_MAX. + * \return 0 on success, negative on error (i.e. if nri_bitlen is not in the valid range or if tmsi is NULL). + */ +int osmo_tmsi_nri_v_set(uint32_t *tmsi, int16_t nri_v, uint8_t nri_bitlen) +{ + uint8_t lowest_bit; + uint32_t v_mask; + if (nri_bitlen < OSMO_NRI_BITLEN_MIN || nri_bitlen > OSMO_NRI_BITLEN_MAX) + return -1; + if (nri_v < 0) + return -1; + if (!tmsi) + return -1; + lowest_bit = 23 - (nri_bitlen - 1); + v_mask = ((((uint32_t)1) << nri_bitlen) - 1) << lowest_bit; + *tmsi = ((*tmsi) & ~v_mask) | ((((uint32_t)nri_v) << lowest_bit) & v_mask); + return 0; +} + +/*! Apply osmo_nri_v_limit_by_ranges() in-place on the NRI value included in a TMSI. + * Extract the NRI value from the TMSI, limit that to be part of the ranges given in 'nri_ranges', and place the + * resulting NRI value back in the TMSI. + * \param[inout] tmsi TMSI value of which to modify the NRI bits, e.g. fresh randomized bits. + * \param[in] nri_ranges List of NRI ranges indicating valid NRI values, where no entries may overlap in range values, + * and all entries must be valid (first <= last). + * \param[in] nri_bitlen Valid NRI range in nr of bits used. + * \returns 0 on success, negative on error. + */ +int osmo_tmsi_nri_v_limit_by_ranges(uint32_t *tmsi, const struct osmo_nri_ranges *nri_ranges, uint8_t nri_bitlen) +{ + int rc; + int16_t nri_v; + rc = osmo_tmsi_nri_v_get(&nri_v, *tmsi, nri_bitlen); + if (rc) + return rc; + rc = osmo_nri_v_limit_by_ranges(&nri_v, nri_ranges, nri_bitlen); + if (rc) + return rc; + return osmo_tmsi_nri_v_set(tmsi, nri_v, nri_bitlen); +} + +/*! Validate that the given NRI range is valid for a given nri_bitlen range. + * \param[in] nri_range NRI value range to validate. + * \param[in] nri_bitlen Valid NRI range in nr of bits used. If nri_bitlen > OSMO_NRI_BITLEN_MAX, the NRI range is only + * validated to be first <= last and non-negative, not checked to fit a bit length range, + * \returns 0 if valid, -1 or 1 if range->first is invalid, -2 or 2 if range->last is invalid, -3 if first > last. + */ +int osmo_nri_range_validate(const struct osmo_nri_range *range, uint8_t nri_bitlen) +{ + int rc; + rc = osmo_nri_v_validate(range->first, nri_bitlen); + if (rc) + return rc; + rc = osmo_nri_v_validate(range->last, nri_bitlen); + if (rc) + return 2 * rc; + if (range->first > range->last) + return -3; + return 0; +} + +/*! Return true when the given NRI range has at least one NRI value that appears in a list of other NRI ranges. + * \param[in] range NRI range to look for. + * \param[in] nri_ranges List NRI ranges. + * \returns true iff any NRI value from 'range' appears anywhere in nri_ranges. + */ +bool osmo_nri_range_overlaps_ranges(const struct osmo_nri_range *range, const struct osmo_nri_ranges *nri_ranges) +{ + struct osmo_nri_range *i; + if (!nri_ranges) + return false; + llist_for_each_entry(i, &nri_ranges->entries, entry) { + if (nri_range_overlaps_range(i, range)) + return true; + } + return false; +} + +/*! Allocate an empty struct osmo_nri_ranges (list of struct osmo_nri_range). + * \param ctx Talloc context to allocate from. + * \return allocated empty list. + */ +struct osmo_nri_ranges *osmo_nri_ranges_alloc(void *ctx) +{ + struct osmo_nri_ranges *nri_ranges; + nri_ranges = talloc_zero(ctx, struct osmo_nri_ranges); + OSMO_ASSERT(nri_ranges); + INIT_LLIST_HEAD(&nri_ranges->entries); + return nri_ranges; +} + +/*! Free a struct osmo_nri_ranges. + * \param nri_ranges The list to discard. + */ +void osmo_nri_ranges_free(struct osmo_nri_ranges *nri_ranges) +{ + if (nri_ranges) + talloc_free(nri_ranges); +} + +/*! Insert a new struct osmo_nri_range in an osmo_nri_ranges list, so that it remains sorted by 'first' values. */ +static void nri_ranges_add_entry_sorted(struct osmo_nri_ranges *nri_ranges, struct osmo_nri_range *add) +{ + struct osmo_nri_range *r; + struct llist_head *at_pos; + OSMO_ASSERT(nri_ranges); + at_pos = nri_ranges->entries.prev; + llist_for_each_entry(r, &nri_ranges->entries, entry) { + if (r->first <= add->first) + continue; + at_pos = r->entry.prev; + break; + } + llist_add(&add->entry, at_pos); +} + +/*! Add a range of NRI values to a list of nri_range structs. + * Intelligently add and/or combine the entries in a list of NRI ranges to also include the NRI range given in 'add'. + * The list remains sorted by 'first' values. + * \param[inout] nri_ranges List of talloc allocated struct osmo_nri_range entries to add the new range to. + * \param[in] add NRI range to add to 'nri_ranges'. + * \returns 0 on success, negative on error (if the range in 'add' is invalid). + */ +int osmo_nri_ranges_add(struct osmo_nri_ranges *nri_ranges, const struct osmo_nri_range *add) +{ + struct osmo_nri_range *range; + struct osmo_nri_range *range_next; + struct osmo_nri_range *target = NULL; + + if (osmo_nri_range_validate(add, 255)) + return -1; + if (!nri_ranges) + return -1; + + /* Is there an entry overlapping this range? */ + llist_for_each_entry(range, &nri_ranges->entries, entry) { + if (!nri_range_touches(range, add)) + continue; + target = range; + } + + if (!target) { + /* No overlaps with existing ranges, create a new one. */ + target = talloc_zero(nri_ranges, struct osmo_nri_range); + OSMO_ASSERT(target); + *target = *add; + nri_ranges_add_entry_sorted(nri_ranges, target); + return 0; + } + + /* Overlap found, join into existing entry */ + nri_range_extend(target, add); + + /* Remove redundant entries */ + llist_for_each_entry_safe(range, range_next, &nri_ranges->entries, entry) { + if (range == target) + continue; + if (!nri_range_touches(target, range)) + continue; + nri_range_extend(target, range); + llist_del(&range->entry); + talloc_free(range); + } + return 0; +} + +/*! Remove a range of NRI values from a list of nri_range structs. + * Intelligently drop and/or cut or split the entries in a list of NRI ranges to no longer include the NRI range given + * in 'del'. Note that after this, the list may have more entries than before, if a range was split into two smaller + * ranges. + * \param[inout] nri_ranges List of talloc allocated struct osmo_nri_range entries to remove values from. + * \param[in] del NRI range to remove from 'nri_ranges'. + * \returns 0 on success, negative on error (if the range in 'del' is invalid). + */ +int osmo_nri_ranges_del(struct osmo_nri_ranges *nri_ranges, const struct osmo_nri_range *del) +{ + struct osmo_nri_range *range; + struct osmo_nri_range *range_next; + + if (osmo_nri_range_validate(del, 255)) + return -1; + if (!nri_ranges) + return -1; + + llist_for_each_entry_safe(range, range_next, &nri_ranges->entries, entry) { + bool head; + bool tail; + if (!nri_range_overlaps_range(range, del)) + continue; + + head = nri_v_matches_range(range, del->first) && (del->first > range->first); + tail = nri_v_matches_range(range, del->last) && (del->last < range->last); + + if (head && tail) { + /* Range cut in two */ + struct osmo_nri_range *new_tail; + + /* Add a new entry for the tail section */ + new_tail = talloc_zero(nri_ranges, struct osmo_nri_range); + OSMO_ASSERT(new_tail); + *new_tail = (struct osmo_nri_range){ + .first = del->last + 1, + .last = range->last, + }; + llist_add(&new_tail->entry, &range->entry); + + /* Existing entry becomes the head section */ + range->last = del->first - 1; + } else if (head) { + /* Range reduced, a head remains */ + range->last = del->first - 1; + } else if (tail) { + /* Range reduced, a tail remains */ + range->first = del->last + 1; + } else { + /* nothing remains */ + llist_del(&range->entry); + talloc_free(range); + } + } + return 0; +} + +/*! Compose a human readable representation of a list of NRI ranges in a buffer, like "23..42,123..142". + * \param[out] buf Target buffer. + * \param[in] buflen sizeof(buf). + * \param[in] nri_ranges List NRI ranges. + * \returns strlen() of string that would be written if the buffer is large enough, like snprintf(). + */ +int osmo_nri_ranges_to_str_buf(char *buf, size_t buflen, const struct osmo_nri_ranges *nri_ranges) +{ + struct osmo_nri_range *range; + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + bool first = true; + if (!nri_ranges || llist_empty(&nri_ranges->entries)) { + OSMO_STRBUF_PRINTF(sb, "empty"); + return sb.chars_needed; + } + llist_for_each_entry(range, &nri_ranges->entries, entry) { + OSMO_STRBUF_PRINTF(sb, "%s%d..%d", first ? "" : ",", range->first, range->last); + } + return sb.chars_needed; +} + +/*! Compose a human readable representation of a list of NRI ranges in a talloc buffer, like "23..42,123..142". + * \param[in] ctx Talloc context. + * \param[in] nri_ranges List of NRI ranges. + * \returns a talloc allocated string. + */ +char *osmo_nri_ranges_to_str_c(void *ctx, const struct osmo_nri_ranges *nri_ranges) +{ + OSMO_NAME_C_IMPL(ctx, 16, "ERROR", osmo_nri_ranges_to_str_buf, nri_ranges); +} + +/*! Parse a string to an NRI value, allowing both decimal and hexadecimal formats; useful for VTY config + * implementations. + * \param[out] dst Write the resulting NRI value to this location. + * \param[in] str Decimal "511" or hex "0x1ff" string to parse. + * \returns 0 on success, negative on error. + */ +static int osmo_nri_parse(int16_t *dst, const char *str) +{ + int val; + int base = 10; + if (osmo_str_startswith(str, "0x")) + base = 16; + if (osmo_str_to_int(&val, str, base, 0, INT16_MAX)) + return -1; + *dst = (int16_t)val; + return 0; +} + +/*! Parse string arguments to a struct osmo_nri_range; useful for VTY config implementations. + * Validate and parse 'first' and optional 'last' string arguments into struct osmo_nri_range values. + * The strings may be in decimal format ("511") or hexadecimal with leading "0x" ("0x1ff"). + * If only one of 'first'/'last' is provided, the resulting range will have only that value (first == last). + * \param[out] nri_range Target for parsed values. + * \param[in] first_str Decimal or hex string, representing the first value in the range, or NULL if omitted. + * \param[in] last_str Decimal or hex string, representing the last value in the range, or NULL if omitted. + * \returns 0 on success, negative on error. + */ +static int osmo_nri_parse_range(struct osmo_nri_range *nri_range, const char *first_str, const char *last_str) +{ + if (!nri_range) + return -1; + if (!first_str) { + first_str = last_str; + last_str = NULL; + if (!first_str) + return -1; + } + if (osmo_nri_parse(&nri_range->first, first_str)) + return -1; + nri_range->last = nri_range->first; + if (last_str) { + if (osmo_nri_parse(&nri_range->last, last_str)) + return -1; + } + return 0; +} + +/*! VTY implementation for adding an NRI range to a list of ranges. + * Parse one or, if present, two argv arguments, which must be numbers representing the first and last value to add to + * the list of NRI ranges, in decimal format ("511") or hexadecimal with leading "0x" ("0x1ff"). If the range values + * surpass the nri_bitlen, return a warning in 'message', but still add the values to the list. + * \param[out] message Returned string constant to alert the user with, or NULL if all is well. + * \param[out] added_range If not NULL, write the range parsing result to this location. + * \param[in] nri_ranges List NRI ranges to add to. + * \param[in] argc Argument count. + * \param[in] argv Argument list. + * \param[in] nri_bitlen Valid NRI range in nr of bits used. + * \returns 0 on success, -1 on error, 1 for a warning (if adding was successful but the added range surpasses + * nri_bitlen). + */ +int osmo_nri_ranges_vty_add(const char **message, struct osmo_nri_range *added_range, + struct osmo_nri_ranges *nri_ranges, int argc, const char **argv, uint8_t nri_bitlen) +{ + struct osmo_nri_range add_range; + if (osmo_nri_parse_range(&add_range, argv[0], argc > 1 ? argv[1] : NULL)) { + *message = "Error: cannot parse NRI range"; + return -1; + } + + if (added_range) + *added_range = add_range; + + if (osmo_nri_ranges_add(nri_ranges, &add_range)) { + *message = "Error: failed to add NRI range"; + return -1; + } + + if (nri_bitlen <= OSMO_NRI_BITLEN_MAX && osmo_nri_range_validate(&add_range, nri_bitlen)) { + *message = "Warning: NRI range surpasses current NRI bitlen"; + return 1; + } + + *message = NULL; + return 0; +} + +/*! VTY implementation for removing an NRI range from a list of ranges. + * Parse one or, if present, two argv arguments, which must be numbers representing the first and last value to remove + * from the list of NRI ranges, in decimal format ("511") or hexadecimal with leading "0x" ("0x1ff"). + * \param[out] message Returned string constant to alert the user with, or NULL if all is well. + * \param[out] removed_range If not NULL, write the range parsing result to this location. + * \param[in] nri_ranges List of NRI ranges to remove from. + * \param[in] argc Argument count. + * \param[in] argv Argument list. + * \returns 0 on success, -1 on error, 1 for a warning. + */ +int osmo_nri_ranges_vty_del(const char **message, struct osmo_nri_range *removed_range, + struct osmo_nri_ranges *nri_ranges, int argc, const char **argv) +{ + struct osmo_nri_range del_range; + if (osmo_nri_parse_range(&del_range, argv[0], argc > 1 ? argv[1] : NULL)) { + *message = "Error: cannot parse NRI range"; + return -1; + } + + if (removed_range) + *removed_range = del_range; + + if (osmo_nri_ranges_del(nri_ranges, &del_range)) { + *message = "Error: failed to remove NRI range"; + return -1; + } + + *message = NULL; + return 0; +} diff --git a/src/gsm/gsm29118.c b/src/gsm/gsm29118.c index 2c02b2fd..0ca5b52e 100644 --- a/src/gsm/gsm29118.c +++ b/src/gsm/gsm29118.c @@ -14,10 +14,6 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include <osmocom/core/utils.h> diff --git a/src/gsm/gsm29205.c b/src/gsm/gsm29205.c index 6ceb8ee4..8fed0206 100644 --- a/src/gsm/gsm29205.c +++ b/src/gsm/gsm29205.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include "config.h" diff --git a/src/gsm/gsm44021.c b/src/gsm/gsm44021.c new file mode 100644 index 00000000..5e9d5ab8 --- /dev/null +++ b/src/gsm/gsm44021.c @@ -0,0 +1,303 @@ +/************************************************************************* + * GSM CSD modified V.110 frame decoding/encoding (ubits <-> struct with D/S/X/E bits) + *************************************************************************/ + +/* (C) 2022 by Harald Welte <laforge@osmocom.org> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <errno.h> +#include <osmocom/core/bits.h> +#include <osmocom/isdn/v110.h> + +/*! Decode a 60-bit GSM 12kbit/s CSD frame present as 60 ubits into a struct osmo_v110_decoded_frame. + * \param[out] caller-allocated output data structure, filled by this function + * \param[in] ra_bits One V.110 frame as 60 unpacked bits. */ +int osmo_csd_12k_6k_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits) +{ + /* 3GPP TS 44.021 Section 8.1.2 / 8.1.3 + D1 D2 D3 D4 D5 D6 S1 + D7 D8 D9 D10 D11 D12 X + D13 D14 D15 D16 D17 D18 S3 + D19 D20 D21 D22 D23 D24 S4 + E4 E5 E6 E7 D25 D26 D27 + D28 D29 D30 S6 D31 D32 D33 + D34 D35 D36 X D37 D38 D39 + D40 D41 D42 S8 D43 D44 D45 + D46 D47 D48 S9 */ + + if (n_bits < 60) + return -EINVAL; + + /* X1 .. X2 */ + fr->x_bits[0] = ra_bits[1 * 7 + 6]; + fr->x_bits[1] = ra_bits[6 * 7 + 3]; + + /* S1, S3, S4, S6, S8, S9 */ + fr->s_bits[0] = ra_bits[0 * 7 + 6]; + fr->s_bits[2] = ra_bits[2 * 7 + 6]; + fr->s_bits[3] = ra_bits[3 * 7 + 6]; + fr->s_bits[5] = ra_bits[5 * 7 + 3]; + fr->s_bits[7] = ra_bits[7 * 7 + 3]; + fr->s_bits[8] = ra_bits[8 * 7 + 3]; + + /* E1 .. E3 must be set by out-of-band knowledge! */ + + /* E4 .. E7 */ + memcpy(fr->e_bits+3, ra_bits + 4 * 7 + 0, 4); + + /* D-bits */ + memcpy(fr->d_bits + 0 * 6 + 0, ra_bits + 0 * 7 + 0, 6); + memcpy(fr->d_bits + 1 * 6 + 0, ra_bits + 1 * 7 + 0, 6); + memcpy(fr->d_bits + 2 * 6 + 0, ra_bits + 2 * 7 + 0, 6); + memcpy(fr->d_bits + 3 * 6 + 0, ra_bits + 3 * 7 + 0, 6); + memcpy(fr->d_bits + 4 * 6 + 0, ra_bits + 4 * 7 + 4, 3); + memcpy(fr->d_bits + 4 * 6 + 3, ra_bits + 5 * 7 + 0, 3); + memcpy(fr->d_bits + 5 * 6 + 0, ra_bits + 5 * 7 + 4, 3); + memcpy(fr->d_bits + 5 * 6 + 3, ra_bits + 6 * 7 + 0, 3); + memcpy(fr->d_bits + 6 * 6 + 0, ra_bits + 6 * 7 + 4, 3); + memcpy(fr->d_bits + 6 * 6 + 3, ra_bits + 7 * 7 + 0, 3); + memcpy(fr->d_bits + 7 * 6 + 0, ra_bits + 7 * 7 + 4, 3); + memcpy(fr->d_bits + 7 * 6 + 3, ra_bits + 8 * 7 + 0, 3); + + return 0; +} + +int osmo_csd_12k_6k_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr) +{ + if (ra_bits_size < 60) + return -EINVAL; + + /* X1 .. X2 */ + ra_bits[1 * 7 + 6] = fr->x_bits[0]; + ra_bits[6 * 7 + 3] = fr->x_bits[1]; + + /* S1, S3, S4, S6, S8, S9 */ + ra_bits[0 * 7 + 6] = fr->s_bits[0]; + ra_bits[2 * 7 + 6] = fr->s_bits[2]; + ra_bits[3 * 7 + 6] = fr->s_bits[3]; + ra_bits[5 * 7 + 3] = fr->s_bits[5]; + ra_bits[7 * 7 + 3] = fr->s_bits[7]; + ra_bits[8 * 7 + 3] = fr->s_bits[8]; + + /* E1 .. E3 are dropped */ + + /* E4 .. E7 */ + memcpy(ra_bits + 4 * 7 + 0, fr->e_bits+3, 4); + + /* D-bits */ + memcpy(ra_bits + 0 * 7 + 0, fr->d_bits + 0 * 6 + 0, 6); + memcpy(ra_bits + 1 * 7 + 0, fr->d_bits + 1 * 6 + 0, 6); + memcpy(ra_bits + 2 * 7 + 0, fr->d_bits + 2 * 6 + 0, 6); + memcpy(ra_bits + 3 * 7 + 0, fr->d_bits + 3 * 6 + 0, 6); + memcpy(ra_bits + 4 * 7 + 4, fr->d_bits + 4 * 6 + 0, 3); + memcpy(ra_bits + 5 * 7 + 0, fr->d_bits + 4 * 6 + 3, 3); + memcpy(ra_bits + 5 * 7 + 4, fr->d_bits + 5 * 6 + 0, 3); + memcpy(ra_bits + 6 * 7 + 0, fr->d_bits + 5 * 6 + 3, 3); + memcpy(ra_bits + 6 * 7 + 4, fr->d_bits + 6 * 6 + 0, 3); + memcpy(ra_bits + 7 * 7 + 0, fr->d_bits + 6 * 6 + 3, 3); + memcpy(ra_bits + 7 * 7 + 4, fr->d_bits + 7 * 6 + 0, 3); + memcpy(ra_bits + 8 * 7 + 0, fr->d_bits + 7 * 6 + 3, 3); + + return 60; +} + +/*! Decode a 36-bit GSM 3k6kbit/s CSD frame present as 36 ubits into a struct osmo_v110_decoded_frame. + * \param[out] caller-allocated output data structure, filled by this function + * \param[in] ra_bits One V.110 frame as 36 unpacked bits. */ +int osmo_csd_3k6_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits) +{ + + /* 3GPP TS 44.021 Section 8.1.4 + D1 D2 D3 S1 D4 D5 D6 X + D7 D8 D9 S3 D10 D11 D12 S4 + E4 E5 E6 E7 D13 D14 D15 S6 + D16 D17 D18 X D19 D20 D21 S8 + D22 D23 D24 S9 + */ + + if (n_bits < 36) + return -EINVAL; + + /* X1 .. X2 */ + fr->x_bits[0] = ra_bits[0 * 8 + 7]; + fr->x_bits[1] = ra_bits[3 * 8 + 3]; + + /* S1, S3, S4, S6, S8, S9 */ + fr->s_bits[0] = ra_bits[0 * 8 + 3]; + fr->s_bits[2] = ra_bits[1 * 8 + 3]; + fr->s_bits[3] = ra_bits[1 * 8 + 7]; + fr->s_bits[5] = ra_bits[2 * 8 + 7]; + fr->s_bits[7] = ra_bits[3 * 8 + 7]; + fr->s_bits[8] = ra_bits[4 * 8 + 3]; + + /* E1 .. E3 must be set by out-of-band knowledge! */ + + /* E4 .. E7 */ + memcpy(fr->e_bits+3, ra_bits + 2 * 8 + 0, 4); + + /* D-bits */ + unsigned int d_idx = 0; + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 0]; /* D1 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 0]; /* D1 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 1]; /* D2 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 1]; /* D2 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 2]; /* D3 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 2]; /* D3 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 4]; /* D4 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 4]; /* D4 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 5]; /* D5 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 5]; /* D5 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 6]; /* D6 */ + fr->d_bits[d_idx++] = ra_bits[0 * 8 + 6]; /* D6 */ + + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 0]; /* D7 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 0]; /* D7 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 1]; /* D8 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 1]; /* D8 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 2]; /* D9 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 2]; /* D9 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 4]; /* D10 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 4]; /* D10 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 5]; /* D11 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 5]; /* D11 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 6]; /* D12 */ + fr->d_bits[d_idx++] = ra_bits[1 * 8 + 6]; /* D12 */ + + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 4]; /* D13 */ + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 4]; /* D13 */ + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 5]; /* D14 */ + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 5]; /* D14 */ + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 6]; /* D15 */ + fr->d_bits[d_idx++] = ra_bits[2 * 8 + 6]; /* D15 */ + + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 0]; /* D16 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 0]; /* D16 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 1]; /* D17 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 1]; /* D17 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 2]; /* D18 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 2]; /* D18 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 4]; /* D19 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 4]; /* D19 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 5]; /* D20 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 5]; /* D20 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 6]; /* D21 */ + fr->d_bits[d_idx++] = ra_bits[3 * 8 + 6]; /* D21 */ + + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 0]; /* D22 */ + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 0]; /* D22 */ + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 1]; /* D23 */ + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 1]; /* D23 */ + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 2]; /* D24 */ + fr->d_bits[d_idx++] = ra_bits[4 * 8 + 2]; /* D24 */ + + OSMO_ASSERT(d_idx == 48); + + return 0; +} + +int osmo_csd_3k6_encode_frame(ubit_t *ra_bits, size_t ra_bits_size, const struct osmo_v110_decoded_frame *fr) +{ + if (ra_bits_size < 36) + return -EINVAL; + + /* X1 .. X2 */ + ra_bits[0 * 8 + 7] = fr->x_bits[0]; + ra_bits[3 * 8 + 3] = fr->x_bits[1]; + + /* S1, S3, S4, S6, S8, S9 */ + ra_bits[0 * 8 + 3] = fr->s_bits[0]; + ra_bits[1 * 8 + 3] = fr->s_bits[2]; + ra_bits[1 * 8 + 7] = fr->s_bits[3]; + ra_bits[2 * 8 + 7] = fr->s_bits[5]; + ra_bits[3 * 8 + 7] = fr->s_bits[7]; + ra_bits[4 * 8 + 3] = fr->s_bits[8]; + + /* E1 .. E3 are ignored */ + + /* E4 .. E7 */ + memcpy(ra_bits + 2 * 8 + 0, fr->e_bits+3, 4); + + /* D-bits */ + unsigned int d_idx = 0; + ra_bits[0 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[0 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[0 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[0 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[0 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[0 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2; + + ra_bits[1 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[1 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[1 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[1 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[1 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[1 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2; + + ra_bits[2 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[2 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[2 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2; + + ra_bits[3 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[3 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[3 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[3 * 8 + 4] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[3 * 8 + 5] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[3 * 8 + 6] = fr->d_bits[d_idx]; d_idx += 2; + + ra_bits[4 * 8 + 0] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[4 * 8 + 1] = fr->d_bits[d_idx]; d_idx += 2; + ra_bits[4 * 8 + 2] = fr->d_bits[d_idx]; d_idx += 2; + + OSMO_ASSERT(d_idx == 48); + + return 36; +} + +/*! Print a encoded "CSD modififed V.110" frame in the same table-like structure as the spec. + * \param outf output FILE stream to which to dump + * \param[in] fr unpacked bits to dump + * \param[in] in_len length of unpacked bits available at fr. */ +void osmo_csd_ubit_dump(FILE *outf, const ubit_t *fr, size_t in_len) +{ + switch (in_len) { + case 60: + for (unsigned int septet = 0; septet < 9; septet++) { + if (septet < 8) { + fprintf(outf, "%d\t%d\t%d\t%d\t%d\t%d\t%d\n", fr[septet * 7 + 0], + fr[septet * 7 + 1], fr[septet * 7 + 2], fr[septet * 7 + 3], + fr[septet * 7 + 4], fr[septet * 7 + 5], fr[septet*7 + 6]); + } else { + fprintf(outf, "%d\t%d\t%d\t%d\n", fr[septet * 7 + 0], + fr[septet * 7 + 1], fr[septet * 7 + 2], fr[septet * 7 + 3]); + } + } + break; + case 36: + for (unsigned int octet = 0; octet < 5; octet++) { + if (octet < 4) { + fprintf(outf, "%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", + fr[octet * 8 + 0], fr[octet * 8 + 1], fr[octet * 8 + 2], + fr[octet * 8 + 3], fr[octet * 8 + 4], fr[octet * 8 + 5], + fr[octet * 8 + 6], fr[octet * 8 + 7]); + } else { + fprintf(outf, "%d\t%d\t%d\t%d\n", fr[octet * 8 + 0], + fr[octet * 8 + 1], fr[octet * 8 + 2], fr[octet * 8 + 3]); + } + } + break; + default: + fprintf(outf, "invalid input data length: %zu\n", in_len); + } +} diff --git a/src/gsm/gsm44068.c b/src/gsm/gsm44068.c new file mode 100644 index 00000000..2556b128 --- /dev/null +++ b/src/gsm/gsm44068.c @@ -0,0 +1,121 @@ +/* Group Call Control (GCC) is an ETSI/3GPP standard protocol used between + * MS (Mobile Station) and MSC (Mobile Switchting Center) in 2G/GSM-R network. + * It is specified in 3GPP TS 44.068. + * + * (C) 2023 by Sysmocom s.f.m.c. GmbH + * All Rights Reserved + * + * Author: Andreas Eversberg + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * SPDX-License-Identifier: GPL-2.0+ + */ + +#include <stddef.h> +#include <osmocom/gsm/tlv.h> +#include <osmocom/gsm/protocol/gsm_44_068.h> + +/*********************************************************************** + * Protocol Definitions + ***********************************************************************/ + +const struct value_string osmo_gsm44068_msg_type_names[] = { + { OSMO_GSM44068_MSGT_IMMEDIATE_SETUP, "IMMEDIATE SETUP" }, + { OSMO_GSM44068_MSGT_SETUP, "SETUP" }, + { OSMO_GSM44068_MSGT_CONNECT, "CONNECT" }, + { OSMO_GSM44068_MSGT_TERMINATION, "TERMINATION" }, + { OSMO_GSM44068_MSGT_TERMINATION_REQUEST, "TERMINATION REQUEST" }, + { OSMO_GSM44068_MSGT_TERMINATION_REJECT, "TERMINATION REJECT" }, + { OSMO_GSM44068_MSGT_STATUS, "STATUS" }, + { OSMO_GSM44068_MSGT_GET_STATUS, "GET STATUS" }, + { OSMO_GSM44068_MSGT_SET_PARAMETER, "SET PARAMETER" }, + { OSMO_GSM44068_MSGT_IMMEDIATE_SETUP_2, "IMMEDIATE SETUP 2" }, + { 0, NULL } +}; + +const struct value_string osmo_gsm44068_priority_level_names[] = { + { OSMO_GSM44068_PRIO_LEVEL_4, "priority level 4" }, + { OSMO_GSM44068_PRIO_LEVEL_3, "priority level 3" }, + { OSMO_GSM44068_PRIO_LEVEL_2, "priority level 2" }, + { OSMO_GSM44068_PRIO_LEVEL_1, "priority level 1" }, + { OSMO_GSM44068_PRIO_LEVEL_0, "priority level 0" }, + { OSMO_GSM44068_PRIO_LEVEL_B, "priority level B" }, + { OSMO_GSM44068_PRIO_LEVEL_A, "priority level A" }, + { 0, NULL } +}; + +const struct value_string osmo_gsm44068_cause_names[] = { + { OSMO_GSM44068_CAUSE_ILLEGAL_MS, "Illegal MS" }, + { OSMO_GSM44068_CAUSE_IMEI_NOT_ACCEPTED, "IMEI not accepted" }, + { OSMO_GSM44068_CAUSE_ILLEGAL_ME, "Illegal ME" }, + { OSMO_GSM44068_CAUSE_SERVICE_NOT_AUTHORIZED, "Service not authorized" }, + { OSMO_GSM44068_CAUSE_APP_NOT_SUPPORTED_ON_PROTO, "Application not supported on the protocol" }, + { OSMO_GSM44068_CAUSE_RR_CONNECTION_ABORTED, "RR connection aborted" }, + { OSMO_GSM44068_CAUSE_NORMAL_CALL_CLEARING, "Normal call clearing" }, + { OSMO_GSM44068_CAUSE_NETWORK_FAILURE, "Network failure" }, + { OSMO_GSM44068_CAUSE_BUSY, "Busy" }, + { OSMO_GSM44068_CAUSE_CONGESTION, "Congestion" }, + { OSMO_GSM44068_CAUSE_USER_NOT_ORIGINATOR, "User not originator of call" }, + { OSMO_GSM44068_CAUSE_NET_WANTS_TO_MAINTAIN_CALL, "Network wants to maintain call" }, + { OSMO_GSM44068_CAUSE_RESPONSE_TO_GET_STATUS, "Response to GET STATUS" }, + { OSMO_GSM44068_CAUSE_SERVICE_OPTION_NOT_SUBSCR, "Service option not supported" }, + { OSMO_GSM44068_CAUSE_REQUESTED_SERVICE_NOT_SUB, "Requested service option not subscribed" }, + { OSMO_GSM44068_CAUSE_SERVICE_OPTION_OOO, "Service option temporarily out of order" }, + { OSMO_GSM44068_CAUSE_CALL_CANNOT_BE_IDENTIFIED, "Call cannot be identified" }, + { OSMO_GSM44068_CAUSE_RETRY_UPON_ENTRY_NEW_CALL, "retry upon entry into a new cell" }, + { OSMO_GSM44068_CAUSE_INVALID_TRANSACTION_ID, "Invalid transaction identifier value" }, + { OSMO_GSM44068_CAUSE_SEMANTICALLY_INCORRECT_MSG, "Semantically incorrect message" }, + { OSMO_GSM44068_CAUSE_INVALID_MANDATORY_INFO, "Invalid mandatory information" }, + { OSMO_GSM44068_CAUSE_MESSAGE_TYPE_NON_EXISTENT, "Message type non-existent or not implemented" }, + { OSMO_GSM44068_CAUSE_MESSAGE_TYPE_NOT_COMPAT, "Message type not compatible with the protocol state" }, + { OSMO_GSM44068_CAUSE_IE_NON_EXISTENT, "Information element non-existent or not implemented" }, + { OSMO_GSM44068_CAUSE_IE_NOT_COMPAT, "Message type not compatible with the protocol state" }, + { OSMO_GSM44068_CAUSE_PROTOCOL_ERROR, "Protocol error, unspecified" }, + { 0, NULL } +}; + +const struct value_string osmo_gsm44068_call_state_names[] = { + { OSMO_GSM44068_CSTATE_U0, "U0" }, + { OSMO_GSM44068_CSTATE_U1, "U1" }, + { OSMO_GSM44068_CSTATE_U2sl_U2, "U2sl/U2" }, + { OSMO_GSM44068_CSTATE_U3, "U3" }, + { OSMO_GSM44068_CSTATE_U4, "U4" }, + { OSMO_GSM44068_CSTATE_U5, "U5" }, + { OSMO_GSM44068_CSTATE_U0p, "U0.p" }, + { OSMO_GSM44068_CSTATE_U2wr_U6, "U2wr/U6" }, + { OSMO_GSM44068_CSTATE_U2r, "U2r" }, + { OSMO_GSM44068_CSTATE_U2ws, "U2ws" }, + { OSMO_GSM44068_CSTATE_U2sr, "U2sr" }, + { OSMO_GSM44068_CSTATE_U2nc, "U2nc" }, + { 0, NULL } +}; + +const struct value_string osmo_gsm44068_talker_priority_names[] = { + { OSMO_GSM44068_PRIO_NORMAL, "Normal" }, + { OSMO_GSM44068_PRIO_PRIVILEGED, "Privileged" }, + { OSMO_GSM44068_PRIO_EMERGENCY, "Emergency" }, + { 0, NULL } +}; + +const struct tlv_definition osmo_gsm44068_att_tlvdef = { + .def = { + [OSMO_GSM44068_IEI_MOBILE_IDENTITY] = { TLV_TYPE_TLV }, + [OSMO_GSM44068_IEI_USER_USER] = { TLV_TYPE_TLV }, + [OSMO_GSM44068_IEI_CALL_STATE] = { TLV_TYPE_SINGLE_TV }, + [OSMO_GSM44068_IEI_STATE_ATTRIBUTES] = { TLV_TYPE_SINGLE_TV }, + [OSMO_GSM44068_IEI_TALKER_PRIORITY] = { TLV_TYPE_SINGLE_TV }, + [OSMO_GSM44068_IEI_SMS_INDICATIONS] = { TLV_TYPE_SINGLE_TV }, + }, +}; diff --git a/src/gsm/gsm48.c b/src/gsm/gsm48.c index 17b0829d..64b17658 100644 --- a/src/gsm/gsm48.c +++ b/src/gsm/gsm48.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> @@ -45,6 +41,8 @@ #include <osmocom/gsm/protocol/gsm_04_80.h> #include <osmocom/gsm/protocol/gsm_08_58.h> #include <osmocom/gsm/protocol/gsm_04_08_gprs.h> +#include <osmocom/gsm/protocol/gsm_23_003.h> +#include <osmocom/gsm/protocol/gsm_44_068.h> /*! \addtogroup gsm0408 * @{ @@ -123,6 +121,7 @@ const struct tlv_definition gsm48_rr_att_tlvdef = { [GSM48_IE_CHDES_2_AFTER] = { TLV_TYPE_FIXED, 3 }, [GSM48_IE_MODE_SEC_CH] = { TLV_TYPE_TV }, [GSM48_IE_F_CH_SEQ_AFTER] = { TLV_TYPE_FIXED, 9 }, + [GSM48_IE_EXTENDED_TSC_SET] = { TLV_TYPE_TV }, [GSM48_IE_MA_AFTER] = { TLV_TYPE_TLV }, [GSM48_IE_BA_RANGE] = { TLV_TYPE_TLV }, [GSM48_IE_GROUP_CHDES] = { TLV_TYPE_TLV }, @@ -131,10 +130,10 @@ const struct tlv_definition gsm48_rr_att_tlvdef = { [GSM48_IE_REALTIME_DIFF] = { TLV_TYPE_TLV }, [GSM48_IE_START_TIME] = { TLV_TYPE_FIXED, 2 }, [GSM48_IE_TIMING_ADVANCE] = { TLV_TYPE_TV }, - [GSM48_IE_GROUP_CIP_SEQ] = { TLV_TYPE_SINGLE_TV }, - [GSM48_IE_CIP_MODE_SET] = { TLV_TYPE_SINGLE_TV }, - [GSM48_IE_GPRS_RESUMPT] = { TLV_TYPE_SINGLE_TV }, - [GSM48_IE_SYNC_IND] = { TLV_TYPE_SINGLE_TV }, + [GSM48_IE_GROUP_CIP_SEQ_HO] = { TLV_TYPE_SINGLE_TV }, + [GSM48_IE_CIP_MODE_SET_HO] = { TLV_TYPE_SINGLE_TV }, + [GSM48_IE_GPRS_RESUMPT_HO] = { TLV_TYPE_SINGLE_TV }, + [GSM48_IE_SYNC_IND_HO] = { TLV_TYPE_SINGLE_TV }, }, }; @@ -150,7 +149,7 @@ const struct tlv_definition gsm48_mm_att_tlvdef = { [GSM48_IE_NET_DST] = { TLV_TYPE_TLV }, [GSM48_IE_LOCATION_AREA] = { TLV_TYPE_FIXED, 5 }, - [GSM48_IE_PRIORITY_LEV] = { TLV_TYPE_SINGLE_TV }, + [GSM48_IE_PRIORITY_LEV_HO] = { TLV_TYPE_SINGLE_TV }, [GSM48_IE_FOLLOW_ON_PROC] = { TLV_TYPE_T }, [GSM48_IE_CTS_PERMISSION] = { TLV_TYPE_T }, }, @@ -163,13 +162,16 @@ static const struct value_string rr_cause_names[] = { { GSM48_RR_CAUSE_ABNORMAL_TIMER, "Abnormal release, timer expired" }, { GSM48_RR_CAUSE_ABNORMAL_NOACT, "Abnormal release, no activity on radio path" }, { GSM48_RR_CAUSE_PREMPTIVE_REL, "Preemptive release" }, + { GSM48_RR_CAUSE_UTRAN_CFG_UNK, "UTRAN configuration unknown" }, { GSM48_RR_CAUSE_HNDOVER_IMP, "Handover impossible, timing advance out of range" }, { GSM48_RR_CAUSE_CHAN_MODE_UNACCT, "Channel mode unacceptable" }, { GSM48_RR_CAUSE_FREQ_NOT_IMPL, "Frequency not implemented" }, + { GSM48_RR_CAUSE_LEAVE_GROUP_CA, "Originator or talker leaving group call area" }, + { GSM48_RR_CAUSE_LOW_LEVEL_FAIL, "Lower layer failure" }, { GSM48_RR_CAUSE_CALL_CLEARED, "Call already cleared" }, { GSM48_RR_CAUSE_SEMANT_INCORR, "Semantically incorrect message" }, { GSM48_RR_CAUSE_INVALID_MAND_INF, "Invalid mandatory information" }, - { GSM48_RR_CAUSE_MSG_TYPE_N, "Message type non-existant or not implemented" }, + { GSM48_RR_CAUSE_MSG_TYPE_N, "Message type non-existent or not implemented" }, { GSM48_RR_CAUSE_MSG_TYPE_N_COMPAT, "Message type not compatible with protocol state" }, { GSM48_RR_CAUSE_COND_IE_ERROR, "Conditional IE error" }, { GSM48_RR_CAUSE_NO_CELL_ALLOC_A, "No cell allocation available" }, @@ -418,19 +420,102 @@ const char *gsm48_rr_msg_name(uint8_t msgtype) return get_value_string(rr_msg_names, msgtype); } +/* 3GPP TS 44.018 Table 10.4.2 */ +static const struct value_string rr_msg_type_short_names[] = { + { GSM48_MT_RR_SH_SI10, "System Information Type 10" }, + { GSM48_MT_RR_SH_FACCH, "Notification/FACCH" }, + { GSM48_MT_RR_SH_UL_FREE, "Uplink Free" }, + { GSM48_MT_RR_SH_MEAS_REP, "Enhanced Measurement Report (uplink)" }, + { GSM48_MT_RR_SH_MEAS_INFO, "Measurement Information (downlink)" }, + { GSM48_MT_RR_SH_VGCS_RECON, "VBS/VGCS Reconfigure" }, + { GSM48_MT_RR_SH_VGCS_RECON2, "VBS/VGCS Reconfigure2" }, + { GSM48_MT_RR_SH_VGCS_INFO, "VGCS Additional Information" }, + { GSM48_MT_RR_SH_VGCS_SMS, "VGCS SMS Information" }, + { GSM48_MT_RR_SH_SI10bis, "System Information Type 10bis" }, + { GSM48_MT_RR_SH_SI10ter, "System Information Type 10ter" }, + { GSM48_MT_RR_SH_VGCS_NEIGH, "VGCS Neighbour Cell Information" }, + { GSM48_MT_RR_SH_APP_DATA, "Notify Application Data" }, + { 0, NULL } +}; + +/*! return string representation of RR Message Type using the RR short protocol discriminator */ +const char *gsm48_rr_short_pd_msg_name(uint8_t msgtype) +{ + return get_value_string(rr_msg_type_short_names, msgtype); +} const struct value_string gsm48_chan_mode_names[] = { { GSM48_CMODE_SIGN, "SIGNALLING" }, { GSM48_CMODE_SPEECH_V1, "SPEECH_V1" }, { GSM48_CMODE_SPEECH_EFR, "SPEECH_EFR" }, { GSM48_CMODE_SPEECH_AMR, "SPEECH_AMR" }, + { GSM48_CMODE_SPEECH_V4, "SPEECH_V4" }, + { GSM48_CMODE_SPEECH_V5, "SPEECH_V5" }, + { GSM48_CMODE_SPEECH_V6, "SPEECH_V6" }, + + { GSM48_CMODE_DATA_43k5_14k5, "DATA_43k5_14k5" }, + { GSM48_CMODE_DATA_29k0_14k5, "DATA_29k0_14k5" }, + { GSM48_CMODE_DATA_43k5_29k0, "DATA_43k5_29k0" }, + { GSM48_CMODE_DATA_14k5_43k5, "DATA_14k5_43k5" }, + { GSM48_CMODE_DATA_14k5_29k0, "DATA_14k5_29k0" }, + { GSM48_CMODE_DATA_29k0_43k5, "DATA_29k0_43k5" }, + + { GSM48_CMODE_DATA_43k5, "DATA_43k5" }, + { GSM48_CMODE_DATA_32k0, "DATA_32k0" }, + { GSM48_CMODE_DATA_29k0, "DATA_29k0" }, { GSM48_CMODE_DATA_14k5, "DATA_14k5" }, { GSM48_CMODE_DATA_12k0, "DATA_12k0" }, { GSM48_CMODE_DATA_6k0, "DATA_6k0" }, { GSM48_CMODE_DATA_3k6, "DATA_3k6" }, + + { GSM48_CMODE_SPEECH_V1_VAMOS, "SPEECH_V1_VAMOS" }, + { GSM48_CMODE_SPEECH_V2_VAMOS, "SPEECH_V2_VAMOS" }, + { GSM48_CMODE_SPEECH_V3_VAMOS, "SPEECH_V3_VAMOS" }, + { GSM48_CMODE_SPEECH_V5_VAMOS, "SPEECH_V5_VAMOS" }, { 0, NULL }, }; +/*! Translate GSM48_CMODE_SPEECH_* to its corresponding GSM48_CMODE_SPEECH_*_VAMOS mode. + * If the mode has no equivalent VAMOS mode, return a negative value. + */ +enum gsm48_chan_mode gsm48_chan_mode_to_vamos(enum gsm48_chan_mode mode) +{ + switch (mode) { + case GSM48_CMODE_SPEECH_V1: + case GSM48_CMODE_SPEECH_V1_VAMOS: + return GSM48_CMODE_SPEECH_V1_VAMOS; + case GSM48_CMODE_SPEECH_EFR: + case GSM48_CMODE_SPEECH_V2_VAMOS: + return GSM48_CMODE_SPEECH_V2_VAMOS; + case GSM48_CMODE_SPEECH_AMR: + case GSM48_CMODE_SPEECH_V3_VAMOS: + return GSM48_CMODE_SPEECH_V3_VAMOS; + case GSM48_CMODE_SPEECH_V5_VAMOS: + return GSM48_CMODE_SPEECH_V5_VAMOS; + default: + return -1; + } +} + +/*! Translate GSM48_CMODE_SPEECH_*_VAMOS to its corresponding GSM48_CMODE_SPEECH_* non-vamos mode. + * If the mode is not a VAMOS mode, return the unchanged mode. + */ +enum gsm48_chan_mode gsm48_chan_mode_to_non_vamos(enum gsm48_chan_mode mode) +{ + switch (mode) { + case GSM48_CMODE_SPEECH_V1_VAMOS: + return GSM48_CMODE_SPEECH_V1; + case GSM48_CMODE_SPEECH_V2_VAMOS: + return GSM48_CMODE_SPEECH_EFR; + case GSM48_CMODE_SPEECH_V3_VAMOS: + return GSM48_CMODE_SPEECH_AMR; + case GSM48_CMODE_SPEECH_V5_VAMOS: + return GSM48_CMODE_SPEECH_V5; + default: + return mode; + } +} + const struct value_string gsm_chan_t_names[] = { { GSM_LCHAN_NONE, "NONE" }, { GSM_LCHAN_SDCCH, "SDCCH" }, @@ -458,7 +543,8 @@ const char *gsm48_mi_type_name(uint8_t mi) return get_value_string(mi_type_names, mi); } -/*! Return a human readable representation of a Mobile Identity in caller-provided buffer. +/*! Deprecated, see osmo_mobile_identity instead. + * Return a human readable representation of a Mobile Identity in caller-provided buffer. * \param[out] buf caller-provided output buffer * \param[in] buf_len size of buf in bytes * \param[in] mi Mobile Identity buffer containing 3GPP TS 04.08 style MI type and data. @@ -479,9 +565,10 @@ char *osmo_mi_name_buf(char *buf, size_t buf_len, const uint8_t *mi, uint8_t mi_ if (mi_len == GSM48_TMSI_LEN && mi[0] == (0xf0 | GSM_MI_TYPE_TMSI)) { tmsi = osmo_load32be(&mi[1]); snprintf(buf, buf_len, "TMSI-0x%08" PRIX32, tmsi); - return buf; + } else { + snprintf(buf, buf_len, "TMSI-invalid"); } - return "TMSI-invalid"; + return buf; case GSM_MI_TYPE_IMSI: case GSM_MI_TYPE_IMEI: @@ -491,11 +578,13 @@ char *osmo_mi_name_buf(char *buf, size_t buf_len, const uint8_t *mi, uint8_t mi_ return buf; default: - return "unknown"; + snprintf(buf, buf_len, "unknown"); + return buf; } } -/*! Return a human readable representation of a Mobile Identity in static buffer. +/*! Deprecated, see osmo_mobile_identity instead. + * Return a human readable representation of a Mobile Identity in static buffer. * \param[in] mi Mobile Identity buffer containing 3GPP TS 04.08 style MI type and data. * \param[in] mi_len Length of mi. * \return A string like "IMSI-1234567", "TMSI-0x1234ABCD" or "unknown", "TMSI-invalid"... @@ -506,7 +595,8 @@ const char *osmo_mi_name(const uint8_t *mi, uint8_t mi_len) return osmo_mi_name_buf(mi_name, sizeof(mi_name), mi, mi_len); } -/*! Return a human readable representation of a Mobile Identity in dynamically-allocated buffer. +/*! Deprecated, see osmo_mobile_identity instead. + * Return a human readable representation of a Mobile Identity in dynamically-allocated buffer. * \param[in] ctx talloc context from which to allocate output buffer * \param[in] mi Mobile Identity buffer containing 3GPP TS 04.08 style MI type and data. * \param[in] mi_len Length of mi. @@ -522,6 +612,478 @@ char *osmo_mi_name_c(const void *ctx, const uint8_t *mi, uint8_t mi_len) return osmo_mi_name_buf(mi_name, buf_len, mi, mi_len); } +/*! Extract Mobile Identity from encoded bytes (3GPP TS 24.008 10.5.1.4). + * + * On failure (negative return value), mi->type == GSM_MI_TYPE_NONE, mi->string[] is all-zero and mi->tmsi == + * GSM_RESERVED_TMSI. + * + * On success, mi->type reflects the decoded Mobile Identity type (GSM_MI_TYPE_IMSI, GSM_MI_TYPE_TMSI, GSM_MI_TYPE_IMEI + * or GSM_MI_TYPE_IMEISV). + * + * On success, mi->string always contains a human readable representation of the Mobile Identity digits: IMSI, IMEI and + * IMEISV as digits like "12345678", and TMSI as "0x" and 8 hexadecimal digits like "0x1234abcd". + * + * mi->tmsi contains the uint32_t TMSI value iff the extracted Mobile Identity was a TMSI, or GSM_RESERVED_TMSI + * otherwise. + * + * \param[out] mi Return buffer for decoded Mobile Identity. + * \param[in] mi_data The encoded Mobile Identity octets. + * \param[in] mi_len Number of octets in mi_data. + * \param[in] allow_hex If false, hexadecimal digits (>9) result in an error return value. + * \returns 0 on success, negative on error: -EBADMSG = invalid length indication or invalid data, + * -EINVAL = unknown Mobile Identity type. + */ +int osmo_mobile_identity_decode(struct osmo_mobile_identity *mi, const uint8_t *mi_data, uint8_t mi_len, + bool allow_hex) +{ + int rc; + int nibbles_len; + char *str = NULL; /* initialize to avoid uninitialized false warnings on some gcc versions (11.1.0) */ + size_t str_size = 0; /* initialize to avoid uninitialized false warnings on some gcc versions (11.1.0) */ + + if (!mi_data || mi_len < 1) { + rc = -EBADMSG; + goto return_error; + } + + nibbles_len = (mi_len - 1) * 2 + ((mi_data[0] & GSM_MI_ODD) ? 1 : 0); + + *mi = (struct osmo_mobile_identity){ + .type = mi_data[0] & GSM_MI_TYPE_MASK, + }; + + /* First do length checks */ + switch (mi->type) { + case GSM_MI_TYPE_TMSI: + mi->tmsi = GSM_RESERVED_TMSI; + if (nibbles_len != (GSM23003_TMSI_NUM_BYTES * 2)) { + rc = -EBADMSG; + goto return_error; + } + break; + + case GSM_MI_TYPE_IMSI: + if (nibbles_len < GSM23003_IMSI_MIN_DIGITS || nibbles_len > GSM23003_IMSI_MAX_DIGITS) { + rc = -EBADMSG; + goto return_error; + } + str = mi->imsi; + str_size = sizeof(mi->imsi); + break; + + case GSM_MI_TYPE_IMEI: + if (nibbles_len != GSM23003_IMEI_NUM_DIGITS && nibbles_len != GSM23003_IMEI_NUM_DIGITS_NO_CHK) { + rc = -EBADMSG; + goto return_error; + } + str = mi->imei; + str_size = sizeof(mi->imei); + break; + + case GSM_MI_TYPE_IMEISV: + if (nibbles_len != GSM23003_IMEISV_NUM_DIGITS) { + rc = -EBADMSG; + goto return_error; + } + str = mi->imeisv; + str_size = sizeof(mi->imeisv); + break; + + default: + rc = -EINVAL; + goto return_error; + } + + /* Decode BCD digits */ + switch (mi->type) { + case GSM_MI_TYPE_TMSI: + /* MI is a 32bit integer TMSI. Length has been checked above. */ + if ((mi_data[0] & 0xf0) != 0xf0) { + /* A TMSI always has the first nibble == 0xf */ + rc = -EBADMSG; + goto return_error; + } + mi->tmsi = osmo_load32be(&mi_data[1]); + return 0; + + case GSM_MI_TYPE_IMSI: + case GSM_MI_TYPE_IMEI: + case GSM_MI_TYPE_IMEISV: + /* If the length is even, the last nibble (higher nibble of last octet) must be 0xf */ + if (!(mi_data[0] & GSM_MI_ODD) + && ((mi_data[mi_len - 1] & 0xf0) != 0xf0)) { + rc = -EBADMSG; + goto return_error; + } + rc = osmo_bcd2str(str, str_size, mi_data, 1, 1 + nibbles_len, allow_hex); + /* check mi->str printing rc */ + if (rc < 1 || rc >= str_size) { + rc = -EBADMSG; + goto return_error; + } + return 0; + + default: + /* Already handled above, but as future bug paranoia: */ + rc = -EINVAL; + goto return_error; + } + +return_error: + *mi = (struct osmo_mobile_identity){ + .type = GSM_MI_TYPE_NONE, + }; + return rc; +} + +/*! Return the number of encoded Mobile Identity octets, without actually encoding. + * Useful to write tag-length header before encoding the MI. + * \param[in] mi Mobile Identity. + * \param[out] mi_digits If not NULL, store the number of nibbles of used MI data (i.e. strlen(mi->string) or 8 for a TMSI). + * \return octets that osmo_mobile_identity_encode_msgb() will write for this mi. + */ +int osmo_mobile_identity_encoded_len(const struct osmo_mobile_identity *mi, int *mi_digits) +{ + int mi_nibbles; + if (!mi) + return -EINVAL; + switch (mi->type) { + case GSM_MI_TYPE_TMSI: + mi_nibbles = GSM23003_TMSI_NUM_BYTES * 2; + break; + case GSM_MI_TYPE_IMSI: + mi_nibbles = strlen(mi->imsi); + if (mi_nibbles < GSM23003_IMSI_MIN_DIGITS + || mi_nibbles > GSM23003_IMSI_MAX_DIGITS) + return -EINVAL; + break; + case GSM_MI_TYPE_IMEI: + mi_nibbles = strlen(mi->imei); + if (mi_nibbles < GSM23003_IMEI_NUM_DIGITS_NO_CHK + || mi_nibbles > GSM23003_IMEI_NUM_DIGITS) + return -EINVAL; + break; + case GSM_MI_TYPE_IMEISV: + mi_nibbles = strlen(mi->imeisv); + if (mi_nibbles != GSM23003_IMEISV_NUM_DIGITS) + return -EINVAL; + break; + default: + return -ENOTSUP; + } + + if (mi_digits) + *mi_digits = mi_nibbles; + + /* one type nibble, plus the MI nibbles, plus a filler nibble to complete the last octet: + * mi_octets = ceil((float)(mi_nibbles + 1) / 2) + */ + return (mi_nibbles + 2) / 2; +} + +/*! Encode Mobile Identity from uint32_t (TMSI) or digits string (all others) (3GPP TS 24.008 10.5.1.4). + * + * \param[out] buf Return buffer for encoded Mobile Identity. + * \param[in] buflen sizeof(buf). + * \param[in] mi Mobile identity to encode. + * \param[in] allow_hex If false, hexadecimal digits (>9) result in an error return value. + * \returns Amount of bytes written to buf, or negative on error. + */ +int osmo_mobile_identity_encode_buf(uint8_t *buf, size_t buflen, const struct osmo_mobile_identity *mi, bool allow_hex) +{ + int rc; + int nibbles_len; + int mi_octets; + const char *mi_str; + + if (!buf || !buflen) + return -EIO; + + mi_octets = osmo_mobile_identity_encoded_len(mi, &nibbles_len); + if (mi_octets < 0) + return mi_octets; + if (mi_octets > buflen) + return -ENOSPC; + + buf[0] = (mi->type & GSM_MI_TYPE_MASK) | ((nibbles_len & 1) ? GSM_MI_ODD : 0); + + switch (mi->type) { + case GSM_MI_TYPE_TMSI: + buf[0] |= 0xf0; + osmo_store32be(mi->tmsi, &buf[1]); + return mi_octets; + + case GSM_MI_TYPE_IMSI: + mi_str = mi->imsi; + break; + case GSM_MI_TYPE_IMEI: + mi_str = mi->imei; + break; + case GSM_MI_TYPE_IMEISV: + mi_str = mi->imeisv; + break; + default: + return -ENOTSUP; + } + rc = osmo_str2bcd(buf, buflen, mi_str, 1, -1, allow_hex); + if (rc != mi_octets) + return -EINVAL; + return mi_octets; +} + +/*! Encode Mobile Identity type and BCD digits, appended to a msgb. + * Example to add a GSM48_IE_MOBILE_ID IEI with tag and length to a msgb: + * + * struct osmo_mobile_identity mi = { .type = GSM_MI_TYPE_IMSI }; + * OSMO_STRLCPY_ARRAY(mi.imsi, "1234567890123456"); + * uint8_t *l = msgb_tl_put(msg, GSM48_IE_MOBILE_ID); + * int rc = osmo_mobile_identity_encode_msgb(msg, &mi, false); + * if (rc < 0) + * goto error; + * *l = rc; + * + * Example to add a BSSGP_IE_IMSI with tag and variable-size length, where the + * length needs to be known at the time of writing the IE tag-length header: + * + * struct osmo_mobile_identity mi = { .type = GSM_MI_TYPE_IMSI, }; + * OSMO_STRLCPY_ARRAY(mi.imsi, pinfo->imsi); + * msgb_tvl_put(msg, BSSGP_IE_IMSI, osmo_mobile_identity_encoded_len(&mi, NULL)); + * if (osmo_mobile_identity_encode_msgb(msg, &mi, false) < 0) + * goto error; + */ +int osmo_mobile_identity_encode_msgb(struct msgb *msg, const struct osmo_mobile_identity *mi, bool allow_hex) +{ + int rc = osmo_mobile_identity_encode_buf(msg->tail, msgb_tailroom(msg), mi, allow_hex); + if (rc < 0) + return rc; + msgb_put(msg, rc); + return rc; +} + +/*! Extract Mobile Identity from a Complete Layer 3 message. + * + * Determine the Mobile Identity data and call osmo_mobile_identity_decode() to return a decoded struct + * osmo_mobile_identity. + * + * \param[out] mi Return buffer for decoded Mobile Identity. + * \param[in] l3_data The Complete Layer 3 message to extract from (LU, CM Service Req or Paging Resp). + * \param[in] l3_len Length of l3_data in bytes. + * \returns 0 on success, negative on error: return codes as defined in osmo_mobile_identity_decode(), or + * -ENOTSUP = not a Complete Layer 3 message, + */ +int osmo_mobile_identity_decode_from_l3_buf(struct osmo_mobile_identity *mi, const uint8_t *l3_data, size_t l3_len, + bool allow_hex) +{ + const struct gsm48_hdr *gh; + int8_t pdisc = 0; + uint8_t mtype = 0; + const struct gsm48_loc_upd_req *lu; + const uint8_t *cm2_buf; + uint8_t cm2_len; + const uint8_t *mi_start; + const struct gsm48_pag_resp *paging_response; + const uint8_t *mi_data; + uint8_t mi_len; + const struct gsm48_imsi_detach_ind *idi; + + *mi = (struct osmo_mobile_identity){ + .type = GSM_MI_TYPE_NONE, + .tmsi = GSM_RESERVED_TMSI, + }; + + if (l3_len < sizeof(*gh)) + return -EBADMSG; + + gh = (void *)l3_data; + pdisc = gsm48_hdr_pdisc(gh); + mtype = gsm48_hdr_msg_type(gh); + + switch (pdisc) { + case GSM48_PDISC_MM: + + switch (mtype) { + case GSM48_MT_MM_LOC_UPD_REQUEST: + /* First make sure that lu-> can be dereferenced */ + if (l3_len < sizeof(*gh) + sizeof(*lu)) + return -EBADMSG; + + /* Now we know there is enough msgb data to read a lu->mi_len, so also check that */ + lu = (struct gsm48_loc_upd_req*)gh->data; + if (l3_len < sizeof(*gh) + sizeof(*lu) + lu->mi_len) + return -EBADMSG; + mi_data = lu->mi; + mi_len = lu->mi_len; + goto got_mi; + + case GSM48_MT_MM_CM_SERV_REQ: + case GSM48_MT_MM_CM_REEST_REQ: + /* Unfortunately in Phase1 the Classmark2 length is variable, so we cannot + * just use gsm48_service_request struct, and need to parse it manually. */ + if (l3_len < sizeof(*gh) + 2) + return -EBADMSG; + + cm2_len = gh->data[1]; + cm2_buf = gh->data + 2; + goto got_cm2; + + case GSM48_MT_MM_IMSI_DETACH_IND: + if (l3_len < sizeof(*gh) + sizeof(*idi)) + return -EBADMSG; + idi = (struct gsm48_imsi_detach_ind*) gh->data; + mi_data = idi->mi; + mi_len = idi->mi_len; + goto got_mi; + + case GSM48_MT_MM_ID_RESP: + if (l3_len < sizeof(*gh) + 2) + return -EBADMSG; + mi_data = gh->data+1; + mi_len = gh->data[0]; + goto got_mi; + + default: + break; + } + break; + + case GSM48_PDISC_RR: + + switch (mtype) { + case GSM48_MT_RR_PAG_RESP: + if (l3_len < sizeof(*gh) + sizeof(*paging_response)) + return -EBADMSG; + paging_response = (struct gsm48_pag_resp*)gh->data; + cm2_len = paging_response->cm2_len; + cm2_buf = (uint8_t*)&paging_response->cm2; + goto got_cm2; + + case GSM48_MT_RR_TALKER_IND: + /* Check minimum size: Header + CM2 LV + minimum MI LV */ + if (l3_len < sizeof(*gh) + 4 + 2) + return -EBADMSG; + /* CM2 shall be always 3 bytes in length */ + if (gh->data[0] != 3) + return -EBADMSG; + cm2_len = gh->data[0]; + cm2_buf = gh->data + 1; + goto got_cm2; + + default: + break; + } + break; + } + + return -ENOTSUP; + +got_cm2: + /* MI (Mobile Identity) LV follows the Classmark2 */ + + /* There must be at least a mi_len byte after the CM2 */ + if (cm2_buf + cm2_len + 1 > l3_data + l3_len) + return -EBADMSG; + + mi_start = cm2_buf + cm2_len; + mi_len = mi_start[0]; + mi_data = mi_start + 1; + +got_mi: + /* mi_data points at the start of the Mobile Identity coding of mi_len bytes */ + if (mi_data + mi_len > l3_data + l3_len) + return -EBADMSG; + + return osmo_mobile_identity_decode(mi, mi_data, mi_len, allow_hex); +} + +/*! Extract Mobile Identity from a Complete Layer 3 message. + * + * Determine the Mobile Identity data and call osmo_mobile_identity_decode() to return a decoded struct + * osmo_mobile_identity. + * + * \param[out] mi Return buffer for decoded Mobile Identity. + * \param[in] msg The Complete Layer 3 message to extract from (LU, CM Service Req or Paging Resp). + * \returns 0 on success, negative on error: return codes as defined in osmo_mobile_identity_decode(), or + * -ENOTSUP = not a Complete Layer 3 message, + */ +int osmo_mobile_identity_decode_from_l3(struct osmo_mobile_identity *mi, struct msgb *msg, bool allow_hex) +{ + return osmo_mobile_identity_decode_from_l3_buf(mi, msgb_l3(msg), msgb_l3len(msg), allow_hex); +} + +/*! Return a human readable representation of a struct osmo_mobile_identity. + * Write a string like "IMSI-1234567", "TMSI-0x1234ABCD" or "NONE", "NULL". + * \param[out] buf String buffer to write to. + * \param[in] buflen sizeof(buf). + * \param[in] mi Decoded Mobile Identity data. + * \return the strlen() of the string written when buflen is sufficiently large, like snprintf(). + */ +int osmo_mobile_identity_to_str_buf(char *buf, size_t buflen, const struct osmo_mobile_identity *mi) +{ + struct osmo_strbuf sb = { .buf = buf, .len = buflen }; + if (!mi) + return snprintf(buf, buflen, "NULL"); + OSMO_STRBUF_PRINTF(sb, "%s", gsm48_mi_type_name(mi->type)); + switch (mi->type) { + case GSM_MI_TYPE_TMSI: + OSMO_STRBUF_PRINTF(sb, "-0x%08" PRIX32, mi->tmsi); + break; + case GSM_MI_TYPE_IMSI: + OSMO_STRBUF_PRINTF(sb, "-%s", mi->imsi); + break; + case GSM_MI_TYPE_IMEI: + OSMO_STRBUF_PRINTF(sb, "-%s", mi->imei); + break; + case GSM_MI_TYPE_IMEISV: + OSMO_STRBUF_PRINTF(sb, "-%s", mi->imeisv); + break; + default: + break; + } + return sb.chars_needed; +} + +/*! Like osmo_mobile_identity_to_str_buf(), but return the string in a talloc buffer. + * \param[in] ctx Talloc context to allocate from. + * \param[in] mi Decoded Mobile Identity data. + * \return a string like "IMSI-1234567", "TMSI-0x1234ABCD" or "NONE", "NULL". + */ +char *osmo_mobile_identity_to_str_c(void *ctx, const struct osmo_mobile_identity *mi) +{ + OSMO_NAME_C_IMPL(ctx, 32, "ERROR", osmo_mobile_identity_to_str_buf, mi) +} + +/*! Compare two osmo_mobile_identity structs, returning typical cmp() result. + * \param[in] a Left side osmo_mobile_identity. + * \param[in] b Right side osmo_mobile_identity. + * \returns 0 if both are equal, -1 if a < b, 1 if a > b. + */ +int osmo_mobile_identity_cmp(const struct osmo_mobile_identity *a, const struct osmo_mobile_identity *b) +{ + int cmp; + if (a == b) + return 0; + if (!a) + return -1; + if (!b) + return 1; + cmp = OSMO_CMP(a->type, b->type); + if (cmp) + return cmp; + switch (a->type) { + case GSM_MI_TYPE_TMSI: + return OSMO_CMP(a->tmsi, b->tmsi); + case GSM_MI_TYPE_IMSI: + return strncmp(a->imsi, b->imsi, sizeof(a->imsi)); + case GSM_MI_TYPE_IMEI: + return strncmp(a->imei, b->imei, sizeof(a->imei)); + case GSM_MI_TYPE_IMEISV: + return strncmp(a->imeisv, b->imeisv, sizeof(a->imeisv)); + default: + /* No known type, but both have the same type. */ + return 0; + } +} + /*! Checks is particular message is cipherable in A/Gb mode according to * 3GPP TS 24.008 § 4.7.1.2 * \param[in] hdr Message header @@ -674,7 +1236,8 @@ void gsm48_set_dtx(struct gsm48_cell_options *op, enum gsm48_dtx_mode full, } } -/*! Generate TS 04.08 Mobile ID from TMSI +/*! Deprecated, see osmo_mobile_identity instead. + * Generate TS 04.08 Mobile ID from TMSI * \param[out] buf Caller-provided output buffer (7 bytes) * \param[in] tmsi TMSI to be encoded * \returns number of byes encoded (always 7) */ @@ -690,7 +1253,8 @@ int gsm48_generate_mid_from_tmsi(uint8_t *buf, uint32_t tmsi) return 7; } -/*! Generate TS 24.008 §10.5.1.4 Mobile ID of BCD type from ASCII string +/*! Deprecated, see osmo_mobile_identity instead. + * Generate TS 24.008 §10.5.1.4 Mobile ID of BCD type from ASCII string * \param[out] buf Caller-provided output buffer of at least GSM48_MID_MAX_SIZE bytes * \param[in] id Identity to be encoded * \param[in] mi_type Type of identity (e.g. GSM_MI_TYPE_IMSI, IMEI, IMEISV) @@ -722,7 +1286,8 @@ uint8_t gsm48_generate_mid(uint8_t *buf, const char *id, uint8_t mi_type) return 2 + buf[1]; } -/*! Generate TS 04.08 Mobile ID from IMSI +/*! Deprecated, see osmo_mobile_identity instead. + * Generate TS 04.08 Mobile ID from IMSI * \param[out] buf Caller-provided output buffer * \param[in] imsi IMSI to be encoded * \returns number of bytes used in \a buf */ @@ -731,16 +1296,19 @@ int gsm48_generate_mid_from_imsi(uint8_t *buf, const char *imsi) return gsm48_generate_mid(buf, imsi, GSM_MI_TYPE_IMSI); } -/*! Convert TS 04.08 Mobile Identity (10.5.1.4) to string. +/*! Deprecated, see osmo_mobile_identity instead. + * Convert TS 04.08 Mobile Identity (10.5.1.4) to string. * This function does not validate the Mobile Identity digits, i.e. digits > 9 are returned as 'A'-'F'. * \param[out] string Caller-provided buffer for output * \param[in] str_len Length of \a string in bytes * \param[in] mi Mobile Identity to be stringified * \param[in] mi_len Length of \a mi in bytes - * \returns WARNING: the return value of this function is not well implemented. + * \returns Return <= 0 on error, > 0 on success. + * WARNING: the return value of this function is not well implemented. * Depending on the MI type and amount of output buffer, this may return * the nr of written bytes, or the written strlen(), or the snprintf() - * style strlen()-if-the-buffer-were-large-enough. */ + * style strlen()-if-the-buffer-were-large-enough. + */ int gsm48_mi_to_string(char *string, int str_len, const uint8_t *mi, int mi_len) { int rc; @@ -780,7 +1348,64 @@ int gsm48_mi_to_string(char *string, int str_len, const uint8_t *mi, int mi_len) return 1; } -/*! Parse TS 04.08 Routing Area Identifier +/*! Decode to struct osmo_routing_area_id from a 3GPP TS 24.008 § 10.5.5.15 Routing area identification. + * \param[out] dst Store the decoded result here. + * \param[in] ra_data The start of a Routing Area ID in encoded form, to be decoded. + * \param[in] ra_data_len Buffer size available to read from at *ra_data. + * \return the number of decoded bytes on success, or negative on error (if the input buffer is too small). + */ +int osmo_routing_area_id_decode(struct osmo_routing_area_id *dst, const uint8_t *ra_data, size_t ra_data_len) +{ + const struct gsm48_ra_id *ra_id; + if (ra_data_len < sizeof(*ra_id)) + return -ENOSPC; + + gsm48_decode_lai2((void *)ra_data, &dst->lac); + + ra_id = (void *)ra_data; + dst->rac = ra_id->rac; + + return sizeof(*ra_id); +} + +/*! Encode struct osmo_routing_area_id to a 3GPP TS 24.008 § 10.5.5.15 Routing area identification: write to a buffer. + * \param[out] buf Return buffer for encoded Mobile Identity. + * \param[in] buflen sizeof(buf). + * \param[in] src RA to encode. + * \return Amount of bytes written to buf, or negative on error. + */ +int osmo_routing_area_id_encode_buf(uint8_t *buf, size_t buflen, const struct osmo_routing_area_id *src) +{ + struct gsm48_ra_id *ra_id; + if (buflen < sizeof(*ra_id)) + return -ENOSPC; + + gsm48_generate_lai2((void *)buf, &src->lac); + + ra_id = (void *)buf; + ra_id->rac = src->rac; + + return sizeof(*ra_id); +} + +/*! Encode struct osmo_routing_area_id to a 3GPP TS 24.008 § 10.5.5.15 Routing area identification: append to msgb. + * To succeed, the msgb must have tailroom >= sizeof(struct gsm48_ra_id). + * \param[out] msg Append to this msgb. + * \param[in] src Encode this Routing Area ID. + * \return Number of bytes appended to msgb, or negative on error. + */ +int osmo_routing_area_id_encode_msgb(struct msgb *msg, const struct osmo_routing_area_id *src) +{ + int rc = osmo_routing_area_id_encode_buf(msg->tail, msgb_tailroom(msg), src); + if (rc <= 0) + return rc; + msgb_put(msg, rc); + return rc; +} + +/*! Parse TS 04.08 Routing Area Identifier. + * Preferably use osmo_routing_area_id_decode() instead: struct osmo_routing_area_id is better integrated with other API + * like osmo_plmn_cmp(). * \param[out] Caller-provided memory for decoded RA ID * \param[in] buf Input buffer pointing to RAI IE value */ void gsm48_parse_ra(struct gprs_ra_id *raid, const uint8_t *buf) @@ -838,6 +1463,25 @@ int gsm48_construct_ra(uint8_t *buf, const struct gprs_ra_id *raid) return 6; } +/*! Compare a TS 04.08 Routing Area Identifier + * \param[in] raid1 first Routing Area ID to compare. + * \param[in] raid2 second Routing Area ID to compare. + * \returns true if raid1 and raid2 match, false otherwise. */ +bool gsm48_ra_equal(const struct gprs_ra_id *raid1, const struct gprs_ra_id *raid2) +{ + if (raid1->mcc != raid2->mcc) + return false; + if (raid1->mnc != raid2->mnc) + return false; + if (raid1->mnc_3_digits != raid2->mnc_3_digits) + return false; + if (raid1->lac != raid2->lac) + return false; + if (raid1->rac != raid2->rac) + return false; + return true; +} + /*! Determine number of paging sub-channels * \param[in] chan_desc Control Channel Description * \returns number of paging sub-channels @@ -845,7 +1489,7 @@ int gsm48_construct_ra(uint8_t *buf, const struct gprs_ra_id *raid) * Uses From Table 10.5.33 of GSM 04.08 to determine the number of * paging sub-channels in the given control channel configuration */ -int gsm48_number_of_paging_subchannels(struct gsm48_control_channel_descr *chan_desc) +int gsm48_number_of_paging_subchannels(const struct gsm48_control_channel_descr *chan_desc) { unsigned int n_pag_blocks = gsm0502_get_n_pag_blocks(chan_desc); @@ -1125,6 +1769,10 @@ char *gsm48_pdisc_msgtype_name_buf(char *buf, size_t buf_len, uint8_t pdisc, uin case GSM48_PDISC_CC: msgt_names = gsm48_cc_msgtype_names; break; + case GSM48_PDISC_GROUP_CC: + case GSM48_PDISC_BCAST_CC: + msgt_names = osmo_gsm44068_msg_type_names; + break; case GSM48_PDISC_NC_SS: msgt_names = gsm48_nc_ss_msgtype_names; break; diff --git a/src/gsm/gsm48049.c b/src/gsm/gsm48049.c index 5e743563..3ab907c9 100644 --- a/src/gsm/gsm48049.c +++ b/src/gsm/gsm48049.c @@ -95,7 +95,7 @@ const struct tlv_definition cbsp_att_tlvdef = { [CBSP_IEI_RR_LOADING_LIST] = { TLV_TYPE_TL16V }, [CBSP_IEI_CAUSE] = { TLV_TYPE_TV }, [CBSP_IEI_DCS] = { TLV_TYPE_TV }, - [CBSP_IEI_RECOVERY_IND] { TLV_TYPE_TV }, + [CBSP_IEI_RECOVERY_IND] = { TLV_TYPE_TV }, [CBSP_IEI_MSG_ID] = { TLV_TYPE_FIXED, 2 }, [CBSP_IEI_EMERG_IND] = { TLV_TYPE_TV }, [CBSP_IEI_WARN_TYPE] = { TLV_TYPE_FIXED, 2 }, diff --git a/src/gsm/gsm48_arfcn_range_encode.c b/src/gsm/gsm48_arfcn_range_encode.c index 6423a9a8..afe552d8 100644 --- a/src/gsm/gsm48_arfcn_range_encode.c +++ b/src/gsm/gsm48_arfcn_range_encode.c @@ -166,6 +166,10 @@ int osmo_gsm48_range_enc_determine_range(const int *arfcns, const int size, int { int max = 0; + /* don't dereference arfcns[] array if size is 0 */ + if (size == 0) + return OSMO_GSM48_ARFCN_RANGE_128; + /* * Go for the easiest. And pick arfcns[0] == f0. */ diff --git a/src/gsm/gsm48_ie.c b/src/gsm/gsm48_ie.c index 31028ba4..b95609fe 100644 --- a/src/gsm/gsm48_ie.c +++ b/src/gsm/gsm48_ie.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -34,6 +30,7 @@ #include <osmocom/core/msgb.h> #include <osmocom/gsm/tlv.h> #include <osmocom/gsm/mncc.h> +#include <osmocom/core/bitvec.h> #include <osmocom/gsm/protocol/gsm_04_08.h> #include <osmocom/gsm/gsm48_ie.h> @@ -47,10 +44,11 @@ static const char bcd_num_digits[] = { }; /*! Like gsm48_decode_bcd_number2() but with less airtight bounds checking. - * \param[out] Caller-provided output buffer + * \param[out] output Caller-provided output buffer + * \param[in] output_len sizeof(output) * \param[in] bcd_lv Length-Value portion of to-be-decoded IE * \param[in] h_len Length of an optional heder between L and V portion - * \returns - in case of success; negative on error */ + * \returns 0 in case of success; negative on error */ int gsm48_decode_bcd_number(char *output, int output_len, const uint8_t *bcd_lv, int h_len) { @@ -142,7 +140,7 @@ static int asc_to_bcd(const char asc) * \param[in] max_len Maximum Length of \a bcd_lv * \param[in] h_len Length of an optional heder between L and V portion * \param[in] input phone number as 0-terminated ASCII - * \returns number of bytes used in \a bcd_lv + * \returns number of bytes used in \a bcd_lv; negative on error * * Depending on a context (e.g. called or calling party BCD number), the * optional header between L and V parts can contain TON (Type Of Number), @@ -182,8 +180,8 @@ int gsm48_encode_bcd_number(uint8_t *bcd_lv, uint8_t max_len, } /*! Decode TS 04.08 Bearer Capability IE (10.5.4.5) - * \param[out] Caller-provided memory for decoded output - * \[aram[in] LV portion of TS 04.08 Bearer Capability + * \param[out] bcap Caller-provided memory for decoded output + * \param[in] lv LV portion of TS 04.08 Bearer Capability * \returns 0 on success; negative on error */ int gsm48_decode_bearer_cap(struct gsm_mncc_bearer_cap *bcap, const uint8_t *lv) @@ -206,7 +204,24 @@ int gsm48_decode_bearer_cap(struct gsm_mncc_bearer_cap *bcap, case GSM_MNCC_BCAP_SPEECH: i = 1; s = 0; - while(!(lv[i] & 0x80)) { + if ((lv[1] & 0x80) != 0) { /* octet 3a is absent */ + switch (bcap->radio) { + case GSM48_BCAP_RRQ_FR_ONLY: + bcap->speech_ver[s++] = GSM48_BCAP_SV_FR; + break; + case GSM48_BCAP_RRQ_DUAL_HR: + bcap->speech_ver[s++] = GSM48_BCAP_SV_HR; + bcap->speech_ver[s++] = GSM48_BCAP_SV_FR; + break; + case GSM48_BCAP_RRQ_DUAL_FR: + bcap->speech_ver[s++] = GSM48_BCAP_SV_FR; + bcap->speech_ver[s++] = GSM48_BCAP_SV_HR; + break; + } + bcap->speech_ver[s] = -1; /* end of list */ + return 0; + } + while (!(lv[i] & 0x80)) { i++; /* octet 3a etc */ if (in_len < i) return 0; @@ -219,9 +234,10 @@ int gsm48_decode_bearer_cap(struct gsm_mncc_bearer_cap *bcap, } break; case GSM_MNCC_BCAP_UNR_DIG: + case GSM_MNCC_BCAP_AUDIO: case GSM_MNCC_BCAP_FAX_G3: i = 1; - while(!(lv[i] & 0x80)) { + while (!(lv[i] & 0x80)) { i++; /* octet 3a etc */ if (in_len < i) return 0; @@ -235,7 +251,7 @@ int gsm48_decode_bearer_cap(struct gsm_mncc_bearer_cap *bcap, return 0; bcap->data.rate_adaption = (lv[i] >> 3) & 3; bcap->data.sig_access = lv[i] & 7; - while(!(lv[i] & 0x80)) { + while (!(lv[i] & 0x80)) { i++; /* octet 5a etc */ if (in_len < i) return 0; @@ -320,10 +336,11 @@ int gsm48_encode_bearer_cap(struct msgb *msg, int lv_only, lv[i] |= 0x80; /* last IE of octet 3 etc */ break; case GSM48_BCAP_ITCAP_UNR_DIG_INF: + case GSM48_BCAP_ITCAP_3k1_AUDIO: case GSM48_BCAP_ITCAP_FAX_G3: lv[i++] |= 0x80; /* last IE of octet 3 etc */ /* octet 4 */ - lv[i++] = 0xb8; + lv[i++] = 0x88; /* octet 5 */ lv[i++] = 0x80 | ((bcap->data.rate_adaption & 3) << 3) | (bcap->data.sig_access & 7); @@ -337,7 +354,9 @@ int gsm48_encode_bearer_cap(struct msgb *msg, int lv_only, lv[i++] = (bcap->data.parity & 7) | ((bcap->data.interm_rate & 3) << 5); /* octet 6c */ - lv[i] = 0x80 | (bcap->data.modem_type & 0x1f); + lv[i] = 0x80 | + ((bcap->data.transp & 3) << 5) | + (bcap->data.modem_type & 0x1f); break; default: return -EINVAL; @@ -353,9 +372,9 @@ int gsm48_encode_bearer_cap(struct msgb *msg, int lv_only, } /*! Decode TS 04.08 Call Control Capabilities IE (10.5.4.5a) - * \param[out] Caller-provided memory for decoded CC capabilities + * \param[out] ccap Caller-provided memory for decoded CC capabilities * \param[in] lv Length-Value of IE - * \retursns 0 on success; negative on error */ + * \returns 0 on success; negative on error */ int gsm48_decode_cccap(struct gsm_mncc_cccap *ccap, const uint8_t *lv) { uint8_t in_len = lv[0]; @@ -439,7 +458,7 @@ int gsm48_encode_called(struct msgb *msg, } /*! Decode TS 04.08 Caller ID - * \param[out] called Caller-provided memory for decoded number + * \param[out] callerid Caller-provided memory for decoded number * \param[in] lv Length-Value portion of IE * \returns 0 on success; negative on error */ int gsm48_decode_callerid(struct gsm_mncc_number *callerid, @@ -811,7 +830,7 @@ int gsm48_decode_ssversion(struct gsm_mncc_ssversion *ssv, { uint8_t in_len = lv[0]; - if (in_len < 1 || in_len < sizeof(ssv->info)) + if (in_len < 1 || in_len > sizeof(ssv->info)) return -EINVAL; memcpy(ssv->info, lv + 1, in_len); @@ -866,8 +885,9 @@ static int32_t smod(int32_t n, int32_t m) * \param[in] cd Cell Channel Description IE * \param[in] len Length of \a cd in bytes * \returns 0 on success; negative on error */ -int gsm48_decode_freq_list(struct gsm_sysinfo_freq *f, uint8_t *cd, - uint8_t len, uint8_t mask, uint8_t frqt) +int gsm48_decode_freq_list(struct gsm_sysinfo_freq *f, + const uint8_t *cd, uint8_t len, + uint8_t mask, uint8_t frqt) { int i; @@ -1299,4 +1319,249 @@ int gsm48_decode_freq_list(struct gsm_sysinfo_freq *f, uint8_t *cd, return 0; } + +/*! Decode 3GPP TS 24.008 Mobile Station Classmark 3 (10.5.1.7). + * \param[out] classmark3_out user provided memory to store decoded classmark3. + * \param[in] classmark3 pointer to memory that contains the raw classmark bits. + * \param[in] classmark3_len length in bytes of the memory where classmark3 points to. + * \returns 0 on success; negative on error. */ +int gsm48_decode_classmark3(struct gsm48_classmark3 *classmark3_out, + const uint8_t *classmark3, size_t classmark3_len) +{ + struct bitvec bv; + uint8_t data[255]; + struct gsm48_classmark3 *cm3 = classmark3_out; + + /* if cm3 gets extended by spec, it will be truncated, but 255 bytes + * should be more than enough. */ + if (classmark3_len > sizeof(data)) + classmark3_len = sizeof(data); + + memset(&bv, 0, sizeof(bv)); + memset(data, 0, sizeof(data)); + memset(classmark3_out, 0, sizeof(*classmark3_out)); + + memcpy(data, classmark3, classmark3_len); + bv.data = (uint8_t*) data; + bv.data_len = sizeof(data); + + /* Parse bit vector, see also: 3GPP TS 24.008, section 10.5.1.7 */ + bitvec_get_uint(&bv, 1); + cm3->mult_band_supp = bitvec_get_uint(&bv, 3); + switch (cm3->mult_band_supp) { + case 0x00: + cm3->a5_bits = bitvec_get_uint(&bv, 4); + break; + case 0x05: + case 0x06: + cm3->a5_bits = bitvec_get_uint(&bv, 4); + cm3->assoc_radio_cap_2 = bitvec_get_uint(&bv, 4); + cm3->assoc_radio_cap_1 = bitvec_get_uint(&bv, 4); + break; + case 0x01: + case 0x02: + case 0x04: + cm3->a5_bits = bitvec_get_uint(&bv, 4); + bitvec_get_uint(&bv, 4); + cm3->assoc_radio_cap_1 = bitvec_get_uint(&bv, 4); + break; + default: + return -1; + } + + cm3->r_support.present = bitvec_get_uint(&bv, 1); + if (cm3->r_support.present) + cm3->r_support.r_gsm_assoc_radio_cap = bitvec_get_uint(&bv, 3); + + cm3->hscsd_mult_slot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->hscsd_mult_slot_cap.present) + cm3->hscsd_mult_slot_cap.mslot_class = bitvec_get_uint(&bv, 5); + + cm3->ucs2_treatment = bitvec_get_uint(&bv, 1); + cm3->extended_meas_cap = bitvec_get_uint(&bv, 1); + + cm3->ms_meas_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->ms_meas_cap.present) { + cm3->ms_meas_cap.sms_value = bitvec_get_uint(&bv, 4); + cm3->ms_meas_cap.sm_value = bitvec_get_uint(&bv, 4); + } + + cm3->ms_pos_method_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->ms_pos_method_cap.present) + cm3->ms_pos_method_cap.method = bitvec_get_uint(&bv, 5); + + cm3->ecsd_multislot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->ecsd_multislot_cap.present) + cm3->ecsd_multislot_cap.mslot_class = bitvec_get_uint(&bv, 5); + + cm3->psk8_struct.present = bitvec_get_uint(&bv, 1); + if (cm3->psk8_struct.present) { + cm3->psk8_struct.mod_cap = bitvec_get_uint(&bv, 1); + + cm3->psk8_struct.rf_pwr_cap_1.present = bitvec_get_uint(&bv, 1); + if (cm3->psk8_struct.rf_pwr_cap_1.present) { + cm3->psk8_struct.rf_pwr_cap_1.value = + bitvec_get_uint(&bv, 2); + } + + cm3->psk8_struct.rf_pwr_cap_2.present = bitvec_get_uint(&bv, 1); + if (cm3->psk8_struct.rf_pwr_cap_2.present) { + cm3->psk8_struct.rf_pwr_cap_2.value = + bitvec_get_uint(&bv, 2); + } + } + + cm3->gsm_400_bands_supp.present = bitvec_get_uint(&bv, 1); + if (cm3->gsm_400_bands_supp.present) { + cm3->gsm_400_bands_supp.value = bitvec_get_uint(&bv, 2); + if (cm3->gsm_400_bands_supp.value == 0x00) + return -1; + cm3->gsm_400_bands_supp.assoc_radio_cap = + bitvec_get_uint(&bv, 4); + } + + cm3->gsm_850_assoc_radio_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->gsm_850_assoc_radio_cap.present) + cm3->gsm_850_assoc_radio_cap.value = bitvec_get_uint(&bv, 4); + + cm3->gsm_1900_assoc_radio_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->gsm_1900_assoc_radio_cap.present) + cm3->gsm_1900_assoc_radio_cap.value = bitvec_get_uint(&bv, 4); + + cm3->umts_fdd_rat_cap = bitvec_get_uint(&bv, 1); + cm3->umts_tdd_rat_cap = bitvec_get_uint(&bv, 1); + cm3->cdma200_rat_cap = bitvec_get_uint(&bv, 1); + + cm3->dtm_gprs_multislot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->dtm_gprs_multislot_cap.present) { + cm3->dtm_gprs_multislot_cap.mslot_class = bitvec_get_uint(&bv, 2); + cm3->dtm_gprs_multislot_cap.single_slot_dtm = + bitvec_get_uint(&bv, 1); + cm3->dtm_gprs_multislot_cap.dtm_egprs_multislot_cap.present = + bitvec_get_uint(&bv, 1); + if (cm3->dtm_gprs_multislot_cap.dtm_egprs_multislot_cap.present) + cm3->dtm_gprs_multislot_cap.dtm_egprs_multislot_cap. + mslot_class = bitvec_get_uint(&bv, 2); + } + + /* Release 4 starts here. */ + cm3->single_band_supp.present = bitvec_get_uint(&bv, 1); + if (cm3->single_band_supp.present) + cm3->single_band_supp.value = bitvec_get_uint(&bv, 4); + + cm3->gsm_750_assoc_radio_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->gsm_750_assoc_radio_cap.present) + cm3->gsm_750_assoc_radio_cap.value = bitvec_get_uint(&bv, 4); + + cm3->umts_1_28_mcps_tdd_rat_cap = bitvec_get_uint(&bv, 1); + cm3->geran_feature_package = bitvec_get_uint(&bv, 1); + + cm3->extended_dtm_gprs_multislot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->extended_dtm_gprs_multislot_cap.present) { + cm3->extended_dtm_gprs_multislot_cap.mslot_class = + bitvec_get_uint(&bv, 2); + cm3->extended_dtm_gprs_multislot_cap. + extended_dtm_egprs_multislot_cap.present = + bitvec_get_uint(&bv, 1); + if (cm3->extended_dtm_gprs_multislot_cap. + extended_dtm_egprs_multislot_cap.present) + cm3->extended_dtm_gprs_multislot_cap. + extended_dtm_egprs_multislot_cap.mslot_class = + bitvec_get_uint(&bv, 2); + } + + /* Release 5 starts here */ + cm3->high_multislot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->high_multislot_cap.present) + cm3->high_multislot_cap.value = bitvec_get_uint(&bv, 2); + + /* This used to be the GERAN Iu mode support bit, but the newer spec + * releases say that it should not be used (always zero), however + * we will just ignore tha state of this bit. */ + bitvec_get_uint(&bv, 1); + + cm3->geran_feature_package_2 = bitvec_get_uint(&bv, 1); + cm3->gmsk_multislot_power_prof = bitvec_get_uint(&bv, 2); + cm3->psk8_multislot_power_prof = bitvec_get_uint(&bv, 2); + + /* Release 6 starts here */ + cm3->t_gsm_400_bands_supp.present = bitvec_get_uint(&bv, 1); + if (cm3->t_gsm_400_bands_supp.present) { + cm3->t_gsm_400_bands_supp.value = bitvec_get_uint(&bv, 2); + cm3->t_gsm_400_bands_supp.assoc_radio_cap = + bitvec_get_uint(&bv, 4); + } + + /* This used to be T-GSM 900 associated radio capability, but the + * newer spec releases say that this bit should not be used, but if + * it is used by some MS anyway we must assume that there is data + * we have to override. */ + if (bitvec_get_uint(&bv, 1)) + bitvec_get_uint(&bv, 4); + + cm3->dl_advanced_rx_perf = bitvec_get_uint(&bv, 2); + cm3->dtm_enhancements_cap = bitvec_get_uint(&bv, 1); + + cm3->dtm_gprs_high_multislot_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->dtm_gprs_high_multislot_cap.present) { + cm3->dtm_gprs_high_multislot_cap.mslot_class = + bitvec_get_uint(&bv, 3); + cm3->dtm_gprs_high_multislot_cap.offset_required = + bitvec_get_uint(&bv, 1); + cm3->dtm_gprs_high_multislot_cap.dtm_egprs_high_multislot_cap. + present = bitvec_get_uint(&bv, 1); + if (cm3->dtm_gprs_high_multislot_cap. + dtm_egprs_high_multislot_cap.present) + cm3->dtm_gprs_high_multislot_cap. + dtm_egprs_high_multislot_cap.mslot_class = + bitvec_get_uint(&bv, 3); + } + + cm3->repeated_acch_capability = bitvec_get_uint(&bv, 1); + + /* Release 7 starts here */ + cm3->gsm_710_assoc_radio_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->gsm_710_assoc_radio_cap.present) + cm3->gsm_710_assoc_radio_cap.value = bitvec_get_uint(&bv, 4); + + cm3->t_gsm_810_assoc_radio_cap.present = bitvec_get_uint(&bv, 1); + if (cm3->t_gsm_810_assoc_radio_cap.present) + cm3->t_gsm_810_assoc_radio_cap.value = bitvec_get_uint(&bv, 4); + + cm3->ciphering_mode_setting_cap = bitvec_get_uint(&bv, 1); + cm3->add_pos_cap = bitvec_get_uint(&bv, 1); + + /* Release 8 starts here */ + cm3->e_utra_fdd_supp = bitvec_get_uint(&bv, 1); + cm3->e_utra_tdd_supp = bitvec_get_uint(&bv, 1); + cm3->e_utra_meas_rep_supp = bitvec_get_uint(&bv, 1); + cm3->prio_resel_supp = bitvec_get_uint(&bv, 1); + + /* Release 9 starts here */ + cm3->utra_csg_cells_rep = bitvec_get_uint(&bv, 1); + + cm3->vamos_level = bitvec_get_uint(&bv, 2); + + /* Release 10 starts here */ + cm3->tighter_capability = bitvec_get_uint(&bv, 2); + cm3->sel_ciph_dl_sacch = bitvec_get_uint(&bv, 1); + + /* Release 11 starts here */ + cm3->cs_ps_srvcc_geran_utra = bitvec_get_uint(&bv, 2); + cm3->cs_ps_srvcc_geran_eutra = bitvec_get_uint(&bv, 2); + + cm3->geran_net_sharing = bitvec_get_uint(&bv, 1); + cm3->e_utra_wb_rsrq_meas_supp = bitvec_get_uint(&bv, 1); + + /* Release 12 starts here */ + cm3->er_band_support = bitvec_get_uint(&bv, 1); + cm3->utra_mult_band_ind_supp = bitvec_get_uint(&bv, 1); + cm3->e_utra_mult_band_ind_supp = bitvec_get_uint(&bv, 1); + cm3->extended_tsc_set_cap_supp = bitvec_get_uint(&bv, 1); + + /* Late addition of a release 11 feature */ + cm3->extended_earfcn_val_range = bitvec_get_uint(&bv, 1); + + return 0; +} /*! @} */ diff --git a/src/gsm/gsm48_rest_octets.c b/src/gsm/gsm48_rest_octets.c index 518572ed..57c2c99a 100644 --- a/src/gsm/gsm48_rest_octets.c +++ b/src/gsm/gsm48_rest_octets.c @@ -46,8 +46,9 @@ int osmo_gsm48_rest_octets_si1_encode(uint8_t *data, uint8_t *nch_pos, int is180 if (nch_pos) { bitvec_set_bit(&bv, H); bitvec_set_uint(&bv, *nch_pos, 5); - } else + } else { bitvec_set_bit(&bv, L); + } if (is1800_net) bitvec_set_bit(&bv, L); @@ -58,11 +59,88 @@ int osmo_gsm48_rest_octets_si1_encode(uint8_t *data, uint8_t *nch_pos, int is180 return bv.data_len; } +struct nch_pos { + uint8_t num_blocks; + uint8_t first_block; +}; + +/* 3GPP TS 44.010 Table 10.5.2.32.1b */ +static const struct nch_pos si1ro_nch_positions[] = { + [0x00] = {1, 0}, + [0x01] = {1, 1}, + [0x02] = {1, 2}, + [0x03] = {1, 3}, + [0x04] = {1, 4}, + [0x05] = {1, 5}, + [0x06] = {1, 6}, + + [0x07] = {2, 0}, + [0x08] = {2, 1}, + [0x09] = {2, 2}, + [0x0a] = {2, 3}, + [0x0b] = {2, 4}, + [0x0c] = {2, 5}, + + [0x0d] = {3, 0}, + [0x0e] = {3, 1}, + [0x0f] = {3, 2}, + [0x10] = {3, 3}, + [0x11] = {3, 4}, + + [0x12] = {4, 0}, + [0x13] = {4, 1}, + [0x14] = {4, 2}, + [0x15] = {4, 3}, + + [0x16] = {5, 0}, + [0x17] = {5, 1}, + [0x18] = {5, 2}, + + [0x19] = {6, 0}, + [0x1a] = {6, 1}, + + [0x1b] = {7, 0}, +}; + +/*! Decode the 5-bit 'NCH position' field within SI1 Rest Octets. + * \param[in] value 5-bit value from SI1 rest octets + * \param[out] num_blocks Number of CCCH used for NCH + * \param[out] first_block First CCCH block used for NCH + * \returns 0 on success; negative in case of error */ +int osmo_gsm48_si1ro_nch_pos_decode(uint8_t value, uint8_t *num_blocks, uint8_t *first_block) +{ + if (value >= ARRAY_SIZE(si1ro_nch_positions)) + return -EINVAL; + + *num_blocks = si1ro_nch_positions[value].num_blocks; + *first_block = si1ro_nch_positions[value].first_block; + + return 0; +} + +/*! Encode the 5-bit 'NCH position' field within SI1 Rest Octets. + * \param[in] num_blocks Number of CCCH used for NCH + * \param[in] first_block First CCCH block used for NCH + * \returns 5-bit value for SI1 rest octets on success; negative in case of error */ +int osmo_gsm48_si1ro_nch_pos_encode(uint8_t num_blocks, uint8_t first_block) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(si1ro_nch_positions); i++) { + if (si1ro_nch_positions[i].num_blocks == num_blocks && + si1ro_nch_positions[i].first_block == first_block) { + return i; + } + } + return -EINVAL; +} + /* Append Repeated E-UTRAN Neighbour Cell to bitvec: see 3GPP TS 44.018 Table 10.5.2.33b.1 */ static inline bool append_eutran_neib_cell(struct bitvec *bv, const struct osmo_earfcn_si2q *e, size_t *e_offset, uint8_t budget) { unsigned i, skip = 0; + size_t offset = *e_offset; int16_t rem = budget - 6; /* account for mandatory stop bit and THRESH_E-UTRAN_high */ uint8_t earfcn_budget; @@ -93,7 +171,7 @@ static inline bool append_eutran_neib_cell(struct bitvec *bv, const struct osmo_ /* now we can proceed with actually adding EARFCNs within adjusted budget limit */ for (i = 0; i < e->length; i++) { if (e->arfcn[i] != OSMO_EARFCN_INVALID) { - if (skip < *e_offset) { + if (skip < offset) { skip++; /* ignore EARFCNs added on previous calls */ } else { earfcn_budget = 17; /* compute budget per-EARFCN */ @@ -134,8 +212,9 @@ static inline bool append_eutran_neib_cell(struct bitvec *bv, const struct osmo_ /* E-UTRAN_PRIORITY: 3GPP TS 45.008*/ bitvec_set_bit(bv, 1); bitvec_set_uint(bv, e->prio, 3); - } else + } else { bitvec_set_bit(bv, 0); + } /* THRESH_E-UTRAN_high */ bitvec_set_uint(bv, e->thresh_hi, 5); @@ -144,15 +223,17 @@ static inline bool append_eutran_neib_cell(struct bitvec *bv, const struct osmo_ /* THRESH_E-UTRAN_low: */ bitvec_set_bit(bv, 1); bitvec_set_uint(bv, e->thresh_lo, 5); - } else + } else { bitvec_set_bit(bv, 0); + } if (e->qrxlm_valid) { /* E-UTRAN_QRXLEVMIN: */ bitvec_set_bit(bv, 1); bitvec_set_uint(bv, e->qrxlm, 5); - } else + } else { bitvec_set_bit(bv, 0); + } return true; } @@ -161,7 +242,7 @@ static inline void append_earfcn(struct bitvec *bv, const struct osmo_earfcn_si2 { bool appended; unsigned int old = bv->cur_bit; /* save current position to make rollback possible */ - int rem = budget - 25; + int rem = ((int)budget) - 40; if (rem <= 0) return; @@ -189,8 +270,29 @@ static inline void append_earfcn(struct bitvec *bv, const struct osmo_earfcn_si2 /* Priority and E-UTRAN Parameters Description */ bitvec_set_bit(bv, 1); - /* No Serving Cell Priority Parameters Descr. */ - bitvec_set_bit(bv, 0); + /* budget: 10 bits used above */ + + /* Serving Cell Priority Parameters Descr. is Present, + * see also: 3GPP TS 44.018, Table 10.5.2.33b.1 */ + bitvec_set_bit(bv, 1); + + /* GERAN_PRIORITY */ + bitvec_set_uint(bv, 0, 3); + + /* THRESH_Priority_Search */ + bitvec_set_uint(bv, 0, 4); + + /* THRESH_GSM_low */ + bitvec_set_uint(bv, 0, 4); + + /* H_PRIO */ + bitvec_set_uint(bv, 0, 2); + + /* T_Reselection */ + bitvec_set_uint(bv, 0, 2); + + /* budget: 26 bits used above */ + /* No 3G Priority Parameters Description */ bitvec_set_bit(bv, 0); /* E-UTRAN Parameters Description */ @@ -214,12 +316,16 @@ static inline void append_earfcn(struct bitvec *bv, const struct osmo_earfcn_si2 /* Repeated E-UTRAN Neighbour Cells */ bitvec_set_bit(bv, 1); + /* budget: 34 bits used above */ + appended = append_eutran_neib_cell(bv, e, e_offset, rem); if (!appended) { /* appending is impossible within current budget: rollback */ bv->cur_bit = old; return; } + /* budget: further 6 bits used below, totalling 40 bits */ + /* stop bit - end of Repeated E-UTRAN Neighbour Cells sequence: */ bitvec_set_bit(bv, 0); @@ -404,8 +510,9 @@ static inline void append_uarfcns(struct bitvec *bv, const uint16_t *uarfcn_list if (i < uarfcn_length) { cu = uarfcn_list[i]; st = i; - } else + } else { break; + } } /* stop bit - end of Repeated UTRAN FDD Neighbour Cells */ @@ -508,8 +615,9 @@ static void append_selection_params(struct bitvec *bv, bitvec_set_uint(bv, sp->cell_resel_off, 6); bitvec_set_uint(bv, sp->temp_offs, 3); bitvec_set_uint(bv, sp->penalty_time, 5); - } else + } else { bitvec_set_bit(bv, L); + } } /* Append power offset to bitvec */ @@ -519,8 +627,9 @@ static void append_power_offset(struct bitvec *bv, if (po->present) { bitvec_set_bit(bv, H); bitvec_set_uint(bv, po->power_offset, 2); - } else + } else { bitvec_set_bit(bv, L); + } } /* Append GPRS indicator to bitvec */ @@ -532,8 +641,9 @@ static void append_gprs_ind(struct bitvec *bv, bitvec_set_uint(bv, gi->ra_colour, 3); /* 0 == SI13 in BCCH Norm, 1 == SI13 sent on BCCH Ext */ bitvec_set_bit(bv, gi->si13_position); - } else + } else { bitvec_set_bit(bv, L); + } } /* Generate SI3 Rest Octests (Chapter 10.5.2.34 / Table 10.4.72) */ @@ -567,8 +677,9 @@ int osmo_gsm48_rest_octets_si3_encode(uint8_t *data, const struct osmo_gsm48_si_ if (si3->scheduling.present) { bitvec_set_bit(&bv, H); bitvec_set_uint(&bv, si3->scheduling.where, 3); - } else + } else { bitvec_set_bit(&bv, L); + } /* GPRS Indicator */ append_gprs_ind(&bv, &si3->gprs_ind); @@ -618,22 +729,25 @@ int osmo_gsm48_rest_octets_si4_encode(uint8_t *data, const struct osmo_gsm48_si_ if (si4->lsa_params.present) { bitvec_set_bit(&bv, H); append_lsa_params(&bv, &si4->lsa_params); - } else + } else { bitvec_set_bit(&bv, L); + } /* Cell Identity */ if (1) { bitvec_set_bit(&bv, H); bitvec_set_uint(&bv, si4->cell_id, 16); - } else + } else { bitvec_set_bit(&bv, L); + } /* LSA ID Information */ if (0) { bitvec_set_bit(&bv, H); /* FIXME */ - } else + } else { bitvec_set_bit(&bv, L); + } } else { /* L and break indicator */ bitvec_set_bit(&bv, L); @@ -671,25 +785,29 @@ int osmo_gsm48_rest_octets_si6_encode(uint8_t *data, const struct osmo_gsm48_si6 if (in->pch_nch_info.call_priority_present) { bitvec_set_bit(&bv, 1); bitvec_set_uint(&bv, in->pch_nch_info.call_priority, 3); - } else + } else { bitvec_set_bit(&bv, 0); + } bitvec_set_bit(&bv, !!in->pch_nch_info.nln_status_sacch); - } else + } else { bitvec_set_bit(&bv, L); + } if (in->vbs_vgcs_options.present) { bitvec_set_bit(&bv, H); bitvec_set_bit(&bv, !!in->vbs_vgcs_options.inband_notifications); bitvec_set_bit(&bv, !!in->vbs_vgcs_options.inband_pagings); - } else + } else { bitvec_set_bit(&bv, L); + } if (in->dtm_support.present) { bitvec_set_bit(&bv, H); bitvec_set_uint(&bv, in->dtm_support.rac, 8); bitvec_set_uint(&bv, in->dtm_support.max_lapdm, 3); - } else + } else { bitvec_set_bit(&bv, L); + } if (in->band_indicator_1900) bitvec_set_bit(&bv, H); @@ -699,13 +817,127 @@ int osmo_gsm48_rest_octets_si6_encode(uint8_t *data, const struct osmo_gsm48_si6 if (in->gprs_ms_txpwr_max_ccch.present) { bitvec_set_bit(&bv, H); bitvec_set_uint(&bv, in->gprs_ms_txpwr_max_ccch.max_txpwr, 5); - } else + } else { bitvec_set_bit(&bv, L); + } bitvec_spare_padding(&bv, (bv.data_len * 8) - 1); return bv.data_len; } + +static unsigned int decode_t3192(unsigned int t3192) +{ + /* See also 3GPP TS 44.060 + Table 12.24.2: GPRS Cell Options information element details */ + static const unsigned int decode_t3192_tbl[8] = {500, 1000, 1500, 0, 80, 120, 160, 200}; + OSMO_ASSERT(t3192 <= 7); + return decode_t3192_tbl[t3192]; +} + +static unsigned int decode_drx_timer(unsigned int drx) +{ + static const unsigned int decode_drx_timer_tbl[8] = {0, 1, 2, 4, 8, 16, 32, 64}; + OSMO_ASSERT(drx <= 7); + return decode_drx_timer_tbl[drx]; +} + +static int decode_gprs_cell_opt(struct osmo_gprs_cell_options *gco, struct bitvec *bv) +{ + gco->nmo = bitvec_get_uint(bv, 2); + gco->t3168 = (bitvec_get_uint(bv, 3) + 1) * 500; + gco->t3192 = decode_t3192(bitvec_get_uint(bv, 3)); + gco->drx_timer_max = decode_drx_timer(bitvec_get_uint(bv, 3)); + + /* ACCESS_BURST_TYPE: */ + bitvec_get_uint(bv, 1); + /* CONTROL_ACK_TYPE: */ + gco->ctrl_ack_type_use_block = bitvec_get_uint(bv, 1); + gco->bs_cv_max = bitvec_get_uint(bv, 4); + + if (bitvec_get_uint(bv, 1)) { + bitvec_get_uint(bv, 3); /* DEC */ + bitvec_get_uint(bv, 3); /* INC */ + bitvec_get_uint(bv, 3); /* MAX */ + } + + if (bitvec_get_uint(bv, 1)) { + int ext_len = bitvec_get_uint(bv, 6); + if (ext_len < 0) + return ext_len; + unsigned int cur_bit = bv->cur_bit; + /* Extension Information */ + /* R99 extension: */ + gco->ext_info.egprs_supported = bitvec_get_uint(bv, 1); + if (gco->ext_info.egprs_supported) { + gco->ext_info.use_egprs_p_ch_req = !bitvec_get_uint(bv, 1); + gco->ext_info.bep_period = bitvec_get_uint(bv, 4); + } + gco->ext_info.pfc_supported = bitvec_get_uint(bv, 1); + gco->ext_info.dtm_supported = bitvec_get_uint(bv, 1); + gco->ext_info.bss_paging_coordination = bitvec_get_uint(bv, 1); + /* REL-4 extension: */ + gco->ext_info.ccn_active = bitvec_get_uint(bv, 1); + bitvec_get_uint(bv, 1); /* NW_EXT_UTBF */ + bv->cur_bit = cur_bit + ext_len + 1; + } + return 0; +} + +static void decode_gprs_pwr_ctrl_pars(struct osmo_gprs_power_ctrl_pars *pcp, struct bitvec *bv) +{ + pcp->alpha = bitvec_get_uint(bv, 4); + pcp->t_avg_w = bitvec_get_uint(bv,5); + pcp->t_avg_t = bitvec_get_uint(bv, 5); + pcp->pc_meas_chan = bitvec_get_uint(bv, 1); + pcp->n_avg_i = bitvec_get_uint(bv, 4); +} + +/*! Decode SI13 Rest Octests (04.08 Chapter 10.5.2.37b). + * \param[out] si13 decoded SI13 rest octets + * \param[in] encoded SI13 rest octets + * \returns parsed bits on success, negative on error */ +int osmo_gsm48_rest_octets_si13_decode(struct osmo_gsm48_si13_info *si13, const uint8_t *data) +{ + struct osmo_gprs_cell_options *co = &si13->cell_opts; + struct osmo_gprs_power_ctrl_pars *pcp = &si13->pwr_ctrl_pars; + struct bitvec bv; + int rc; + + memset(&bv, 0, sizeof(bv)); + bv.data = (uint8_t *) data; + bv.data_len = 20; + + memset(si13, 0, sizeof(*si13)); + + + if (bitvec_get_bit_high(&bv) == H) { + si13->bcch_change_mark = bitvec_get_uint(&bv, 3); + si13->si_change_field = bitvec_get_uint(&bv, 4); + if (bitvec_get_uint(&bv, 1)) { + si13->bcch_change_mark = bitvec_get_uint(&bv, 2); + /* FIXME: implement parsing GPRS Mobile Allocation IE */ + return -ENOTSUP; + } + if (bitvec_get_uint(&bv, 1)) { + /* PBCCH present in cell */ + /* FIXME: parse not implemented */ + return -ENOTSUP; + } else { + /* PBCCH not present in cell */ + si13->rac = bitvec_get_uint(&bv, 8); + si13->spgc_ccch_sup = bitvec_get_uint(&bv, 1); + si13->prio_acc_thr = bitvec_get_uint(&bv, 3); + si13->net_ctrl_ord = bitvec_get_uint(&bv, 2); + if ((rc = decode_gprs_cell_opt(co, &bv)) < 0) + return rc; + + decode_gprs_pwr_ctrl_pars(pcp, &bv); + } + } + return bv.cur_bit; +} + /* GPRS Mobile Allocation as per TS 04.60 Chapter 12.10a: < GPRS Mobile Allocation IE > ::= < HSN : bit (6) > @@ -730,8 +962,9 @@ static int append_gprs_mobile_alloc(struct bitvec *bv) /* We want to use a RFL number list */ bitvec_set_bit(bv, 1); /* FIXME: RFL number list */ - } else + } else { bitvec_set_bit(bv, 0); + } if (0) { /* We want to use a MA_BITMAP */ @@ -743,8 +976,9 @@ static int append_gprs_mobile_alloc(struct bitvec *bv) /* We want to provide an ARFCN index list */ bitvec_set_bit(bv, 1); /* FIXME */ - } else + } else { bitvec_set_bit(bv, 0); + } } return 0; } @@ -862,24 +1096,20 @@ static int append_gprs_cell_opt(struct bitvec *bv, } else { /* extension information */ bitvec_set_bit(bv, 1); + /* R99 extension: */ if (!gco->ext_info.egprs_supported) { /* 6bit length of extension */ - bitvec_set_uint(bv, (1 + 3)-1, 6); + bitvec_set_uint(bv, (1 + 5)-1, 6); /* EGPRS supported in the cell */ bitvec_set_bit(bv, 0); } else { /* 6bit length of extension */ - bitvec_set_uint(bv, (1 + 5 + 3)-1, 6); + bitvec_set_uint(bv, (1 + 5 + 5)-1, 6); /* EGPRS supported in the cell */ bitvec_set_bit(bv, 1); - /* 1bit EGPRS PACKET CHANNEL REQUEST */ - if (gco->supports_egprs_11bit_rach == 0) { - bitvec_set_bit(bv, - gco->ext_info.use_egprs_p_ch_req); - } else { - bitvec_set_bit(bv, 0); - } + /* 1bit EGPRS PACKET CHANNEL REQUEST (inverted logic) */ + bitvec_set_bit(bv, !gco->ext_info.use_egprs_p_ch_req); /* 4bit BEP PERIOD */ bitvec_set_uint(bv, gco->ext_info.bep_period, 4); @@ -887,6 +1117,10 @@ static int append_gprs_cell_opt(struct bitvec *bv, bitvec_set_bit(bv, gco->ext_info.pfc_supported); bitvec_set_bit(bv, gco->ext_info.dtm_supported); bitvec_set_bit(bv, gco->ext_info.bss_paging_coordination); + + /* REL-4 extension: */ + bitvec_set_bit(bv, gco->ext_info.ccn_active); + bitvec_set_bit(bv, 0); /* NW_EXT_UTBF disabled */ } return 0; @@ -973,15 +1207,17 @@ void osmo_gsm48_rest_octets_si3_decode(struct osmo_gsm48_si_ro_info *si3, const sp->cell_resel_off = bitvec_get_uint(&bv, 6); sp->temp_offs = bitvec_get_uint(&bv, 3); sp->penalty_time = bitvec_get_uint(&bv, 5); - } else + } else { sp->present = 0; + } /* Optional Power Offset */ if (bitvec_get_bit_high(&bv) == H) { po->present = 1; po->power_offset = bitvec_get_uint(&bv, 2); - } else + } else { po->present = 0; + } /* System Information 2ter Indicator */ if (bitvec_get_bit_high(&bv) == H) @@ -999,26 +1235,71 @@ void osmo_gsm48_rest_octets_si3_decode(struct osmo_gsm48_si_ro_info *si3, const if (bitvec_get_bit_high(&bv) == H) { si3->scheduling.present = 1; si3->scheduling.where = bitvec_get_uint(&bv, 3); - } else + } else { si3->scheduling.present = 0; + } /* GPRS Indicator */ if (bitvec_get_bit_high(&bv) == H) { gi->present = 1; gi->ra_colour = bitvec_get_uint(&bv, 3); gi->si13_position = bitvec_get_uint(&bv, 1); - } else + } else { gi->present = 0; + } /* 3G Early Classmark Sending Restriction. If H, then controlled by * early_cm_ctrl above */ if (bitvec_get_bit_high(&bv) == H) - si3->early_cm_restrict_3g = 1; - else si3->early_cm_restrict_3g = 0; + else + si3->early_cm_restrict_3g = 1; if (bitvec_get_bit_high(&bv) == H) si3->si2quater_indicator = 1; else si3->si2quater_indicator = 0; } + + +void osmo_gsm48_rest_octets_si4_decode(struct osmo_gsm48_si_ro_info *si4, const uint8_t *data, int len) +{ + struct osmo_gsm48_si_selection_params *sp = &si4->selection_params; + struct osmo_gsm48_si_power_offset *po = &si4->power_offset; + struct osmo_gsm48_si3_gprs_ind *gi = &si4->gprs_ind; + struct bitvec bv; + + memset(&bv, 0, sizeof(bv)); + bv.data = (uint8_t *) data; + bv.data_len = len; + + memset(si4, 0, sizeof(*si4)); + + /* Optional Selection Parameters */ + if (bitvec_get_bit_high(&bv) == H) { + sp->present = 1; + sp->cbq = bitvec_get_uint(&bv, 1); + sp->cell_resel_off = bitvec_get_uint(&bv, 6); + sp->temp_offs = bitvec_get_uint(&bv, 3); + sp->penalty_time = bitvec_get_uint(&bv, 5); + } else { + sp->present = 0; + } + + /* Optional Power Offset */ + if (bitvec_get_bit_high(&bv) == H) { + po->present = 1; + po->power_offset = bitvec_get_uint(&bv, 2); + } else { + po->present = 0; + } + + /* GPRS Indicator */ + if (bitvec_get_bit_high(&bv) == H) { + gi->present = 1; + gi->ra_colour = bitvec_get_uint(&bv, 3); + gi->si13_position = bitvec_get_uint(&bv, 1); + } else { + gi->present = 0; + } +} diff --git a/src/gsm/gsm_04_08_gprs.c b/src/gsm/gsm_04_08_gprs.c index 608fa8c1..80325939 100644 --- a/src/gsm/gsm_04_08_gprs.c +++ b/src/gsm/gsm_04_08_gprs.c @@ -65,7 +65,7 @@ const struct value_string gsm48_gmm_cause_names_[] = { { GMM_CAUSE_SEM_INCORR_MSG, "Semantically incorrect message" }, { GMM_CAUSE_INV_MAND_INFO, "Invalid mandatory information" }, { GMM_CAUSE_MSGT_NOTEXIST_NOTIMPL, - "Message type non-existant or not implemented" }, + "Message type non-existent or not implemented" }, { GMM_CAUSE_MSGT_INCOMP_P_STATE, "Message type not compatible with protocol state" }, { GMM_CAUSE_IE_NOTEXIST_NOTIMPL, @@ -105,7 +105,7 @@ const struct value_string gsm48_gsm_cause_names_[] = { { GSM_CAUSE_SEM_INCORR_MSG, "Semantically incorrect message" }, { GSM_CAUSE_INV_MAND_INFO, "Invalid mandatory information" }, { GSM_CAUSE_MSGT_NOTEXIST_NOTIMPL, - "Message type non-existant or not implemented" }, + "Message type non-existent or not implemented" }, { GSM_CAUSE_MSGT_INCOMP_P_STATE, "Message type not compatible with protocol state" }, { GSM_CAUSE_IE_NOTEXIST_NOTIMPL, diff --git a/src/gsm/gsm_utils.c b/src/gsm/gsm_utils.c index ae77a9dc..bb403392 100644 --- a/src/gsm/gsm_utils.c +++ b/src/gsm/gsm_utils.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \mainpage libosmogsm Documentation @@ -84,6 +80,7 @@ #include <osmocom/gsm/gsm_utils.h> #include <osmocom/gsm/meas_rep.h> #include <osmocom/gsm/protocol/gsm_04_08.h> +#include <osmocom/gsm/gsm0502.h> #include <stdlib.h> #include <stdint.h> @@ -96,7 +93,7 @@ #include <time.h> #include <unistd.h> -#include "../../config.h" +#include "config.h" #if (!EMBEDDED) /* FIXME: this can be removed once we bump glibc requirements to 2.25: */ @@ -324,17 +321,18 @@ int gsm_septet_encode(uint8_t *result, const char *data) * \param[in] septet_len Length of \a rdata * \param[in] padding padding bits at start * \returns number of bytes used in \a result */ -int gsm_septets2octets(uint8_t *result, const uint8_t *rdata, uint8_t septet_len, uint8_t padding) +int gsm_septet_pack(uint8_t *result, const uint8_t *rdata, size_t septet_len, uint8_t padding) { int i = 0, z = 0; uint8_t cb, nb; int shift = 0; - uint8_t *data = calloc(septet_len + 1, sizeof(uint8_t)); + uint8_t *data = malloc(septet_len + 1); if (padding) { shift = 7 - padding; /* the first zero is needed for padding */ memcpy(data + 1, rdata, septet_len); + data[0] = 0x00; septet_len++; } else memcpy(data, rdata, septet_len); @@ -369,6 +367,12 @@ int gsm_septets2octets(uint8_t *result, const uint8_t *rdata, uint8_t septet_len return z; } +/*! Backwards compatibility wrapper for gsm_septets_pack(), deprecated. */ +int gsm_septets2octets(uint8_t *result, const uint8_t *rdata, uint8_t septet_len, uint8_t padding) +{ + return gsm_septet_pack(result, rdata, septet_len, padding); +} + /*! GSM 7-bit alphabet TS 03.38 6.2.1 Character packing * \param[out] result Caller-provided output buffer * \param[in] n Maximum length of \a result in bytes @@ -382,7 +386,7 @@ int gsm_7bit_encode_n(uint8_t *result, size_t n, const char *data, int *octets) size_t max_septets = n * 8 / 7; /* prepare for the worst case, every character expanding to two bytes */ - uint8_t *rdata = calloc(strlen(data) * 2, sizeof(uint8_t)); + uint8_t *rdata = malloc(strlen(data) * 2); y = gsm_septet_encode(rdata, data); if (y > max_septets) { @@ -393,7 +397,7 @@ int gsm_7bit_encode_n(uint8_t *result, size_t n, const char *data, int *octets) y = max_septets; } - o = gsm_septets2octets(result, rdata, y, 0); + o = gsm_septet_pack(result, rdata, y, 0); if (octets) *octets = o; @@ -487,7 +491,7 @@ int osmo_get_rand_id(uint8_t *out, size_t len) * \param[out] buf Pre-allocated bufer for storing IE * \returns Number of bytes filled in buf */ -size_t gsm0858_rsl_ul_meas_enc(struct gsm_meas_rep_unidir *mru, bool dtxd_used, +size_t gsm0858_rsl_ul_meas_enc(const struct gsm_meas_rep_unidir *mru, bool dtxd_used, uint8_t *buf) { buf[0] = dtxd_used ? (1 << 6) : 0; @@ -882,10 +886,21 @@ char *gsm_fn_as_gsmtime_str(uint32_t fn) /*! Encode decoded \ref gsm_time to Frame Number * \param[in] time GSM Time in decoded structure * \returns GSM Frame Number */ -uint32_t gsm_gsmtime2fn(struct gsm_time *time) +uint32_t gsm_gsmtime2fn(const struct gsm_time *time) { - /* TS 05.02 Chapter 4.3.3 TDMA frame number */ - return (51 * ((time->t3 - time->t2 + 26) % 26) + time->t3 + (26 * 51 * time->t1)); + uint32_t fn; + + /* See also: + * 3GPP TS 44.018, section 10.5.2.38, 3GPP TS 45.002 section 4.3.3, and + * 3GPP TS 48.058, section 9.3.8 */ + fn = 51 * OSMO_MOD_FLR((time->t3-time->t2), 26) + time->t3 + 51 * 26 * time->t1; + + /* Note: Corrupted input values may cause a resulting frame number + * larger then the maximum permitted value of GSM_MAX_FN. Even though + * the caller is expected to check the input values beforehand we must + * make sure that the result cannot exceed the value range of a valid + * GSM frame number. */ + return fn % GSM_MAX_FN; } char *osmo_dump_gsmtime_buf(char *buf, size_t buf_len, const struct gsm_time *tm) @@ -910,6 +925,45 @@ char *osmo_dump_gsmtime_c(const void *ctx, const struct gsm_time *tm) return osmo_dump_gsmtime_buf(buf, 64, tm); } +#define GSM_RFN_THRESHOLD (GSM_RFN_MODULUS / 2) +uint32_t gsm_rfn2fn(uint16_t rfn, uint32_t curr_fn) +{ + uint32_t curr_rfn; + uint32_t fn_rounded; + const uint32_t rfn32 = rfn; /* used as 32bit for calculations */ + + /* Ensure that all following calculations are performed with the + * relative frame number */ + OSMO_ASSERT(rfn32 < GSM_RFN_MODULUS); + + /* Compute an internal relative frame number from the full internal + frame number */ + curr_rfn = gsm_fn2rfn(curr_fn); + + /* Compute a "rounded" version of the internal frame number, which + * exactly fits in the RFN_MODULUS raster */ + fn_rounded = GSM_TDMA_FN_SUB(curr_fn, curr_rfn); + + /* If the delta between the internal and the external relative frame + * number exceeds a certain limit, we need to assume that the incoming + * request belongs to a the previous rfn period. To correct this, + * we roll back the rounded frame number by one RFN_MODULUS */ + if (GSM_TDMA_FN_DIFF(rfn32, curr_rfn) > GSM_RFN_THRESHOLD) { + /* Race condition between rfn and curr_fn detected: rfn belongs + to the previous RFN_MODULUS cycle, wrapping... */ + if (fn_rounded < GSM_RFN_MODULUS) { + /* Corner case detected: wrapping crosses GSM_MAX_FN border */ + fn_rounded = GSM_TDMA_FN_SUB(GSM_MAX_FN, (GSM_TDMA_FN_SUB(GSM_RFN_MODULUS, fn_rounded))); + } else { + fn_rounded = GSM_TDMA_FN_SUB(fn_rounded, GSM_RFN_MODULUS); + } + } + + /* The real frame number is the sum of the rounded frame number and the + * relative framenumber computed via RACH */ + return GSM_TDMA_FN_SUM(fn_rounded, rfn32); +} + /*! append range1024 encoded data to bit vector * \param[out] bv Caller-provided output bit-vector * \param[in] r Input Range1024 sructure */ diff --git a/src/gsm/gsup.c b/src/gsm/gsup.c index 2f9d85d8..4f0a1b5f 100644 --- a/src/gsm/gsup.c +++ b/src/gsm/gsup.c @@ -101,7 +101,11 @@ const struct value_string osmo_gsup_message_type_names[] = { OSMO_VALUE_STRING(OSMO_GSUP_MSGT_E_CLOSE), OSMO_VALUE_STRING(OSMO_GSUP_MSGT_E_ABORT), - OSMO_VALUE_STRING(OSMO_GSUP_MSGT_E_ROUTING_ERROR), + OSMO_VALUE_STRING(OSMO_GSUP_MSGT_ROUTING_ERROR), + + OSMO_VALUE_STRING(OSMO_GSUP_MSGT_EPDG_TUNNEL_REQUEST), + OSMO_VALUE_STRING(OSMO_GSUP_MSGT_EPDG_TUNNEL_RESULT), + OSMO_VALUE_STRING(OSMO_GSUP_MSGT_EPDG_TUNNEL_ERROR), { 0, NULL } }; @@ -122,6 +126,62 @@ int osmo_gsup_get_err_msg_type(enum osmo_gsup_message_type type_in) return OSMO_GSUP_TO_MSGT_ERROR(type_in); } +static int decode_pdp_address(const uint8_t *data, size_t data_len, struct osmo_gsup_pdp_info *pdp_info) +{ + const struct gsm48_pdp_address *pdp_addr = (const struct gsm48_pdp_address *)data; + /* Explicitly pre-nitialize them to AF_UNSPEC to signal they are empty: */ + pdp_info->pdp_address[0].u.sa.sa_family = AF_UNSPEC; + pdp_info->pdp_address[1].u.sa.sa_family = AF_UNSPEC; + + if (data_len < 2) + return -GMM_CAUSE_PROTO_ERR_UNSPEC; + + pdp_info->pdp_type_org = pdp_addr->organization; + pdp_info->pdp_type_nr = pdp_addr->type; + + if (pdp_info->pdp_type_org != PDP_TYPE_ORG_IETF) + return -GMM_CAUSE_PROTO_ERR_UNSPEC; + + /* Skip type-org + type-nr for easy calculations below: */ + data_len -= 2; + + switch (pdp_info->pdp_type_nr) { + case PDP_TYPE_N_IETF_IPv4: + if (data_len == 0) + return 0; /* empty, marked as AF_UNSET. */ + if (data_len != sizeof(pdp_addr->ietf.v4)) + return -GMM_CAUSE_PROTO_ERR_UNSPEC; + pdp_info->pdp_address[0].u.sa.sa_family = AF_INET; + pdp_info->pdp_address[0].u.sin.sin_addr.s_addr = pdp_addr->ietf.v4; + return 0; + case PDP_TYPE_N_IETF_IPv6: + if (data_len == 0) + return 0; /* empty, marked as AF_UNSET. */ + if (data_len != sizeof(pdp_addr->ietf.v6)) + return -GMM_CAUSE_PROTO_ERR_UNSPEC; + pdp_info->pdp_address[0].u.sa.sa_family = AF_INET6; + memcpy(&pdp_info->pdp_address[0].u.sin6.sin6_addr, + pdp_addr->ietf.v6, + sizeof(pdp_addr->ietf.v6)); + return 0; + case PDP_TYPE_N_IETF_IPv4v6: + if (data_len == 0) + return 0; /* empty, marked as AF_UNSET. */ + if (data_len != sizeof(pdp_addr->ietf.v4v6)) + return -GMM_CAUSE_PROTO_ERR_UNSPEC; + pdp_info->pdp_address[0].u.sa.sa_family = AF_INET; + pdp_info->pdp_address[0].u.sin.sin_addr.s_addr = pdp_addr->ietf.v4v6.v4; + pdp_info->pdp_address[1].u.sa.sa_family = AF_INET6; + memcpy(&pdp_info->pdp_address[1].u.sin6.sin6_addr, + pdp_addr->ietf.v4v6.v6, + sizeof(pdp_addr->ietf.v4v6.v6)); + return 0; + default: + /* reserved, both pdp_info->pdp_address are preinitialied to AF_UNSET. */ + return 0; + } +} + static int decode_pdp_info(uint8_t *data, size_t data_len, struct osmo_gsup_pdp_info *pdp_info) { @@ -145,9 +205,9 @@ static int decode_pdp_info(uint8_t *data, size_t data_len, pdp_info->context_id = osmo_decode_big_endian(value, value_len); break; - case OSMO_GSUP_PDP_TYPE_IE: - pdp_info->pdp_type = - osmo_decode_big_endian(value, value_len) & 0x0fff; + case OSMO_GSUP_PDP_ADDRESS_IE: + if ((rc = decode_pdp_address(value, value_len, pdp_info)) < 0) + return rc; break; case OSMO_GSUP_ACCESS_POINT_NAME_IE: @@ -262,7 +322,7 @@ static int decode_auth_info(uint8_t *data, size_t data_len, parse_error: LOGP(DLGSUP, LOGL_ERROR, - "GSUP IE type %d, length %zu invalid in PDP info\n", iei, value_len); + "GSUP IE type %d, length %zu invalid in auth info\n", iei, value_len); return -1; } @@ -353,7 +413,7 @@ int osmo_gsup_decode(const uint8_t *const_data, size_t data_len, switch (iei) { case OSMO_GSUP_IMSI_IE: - case OSMO_GSUP_PDP_TYPE_IE: + case OSMO_GSUP_PDP_ADDRESS_IE: case OSMO_GSUP_ACCESS_POINT_NAME_IE: case OSMO_GSUP_SRES_IE: case OSMO_GSUP_KC_IE: @@ -446,6 +506,11 @@ int osmo_gsup_decode(const uint8_t *const_data, size_t data_len, gsup_msg->rand = value; break; + case OSMO_GSUP_PCO_IE: + gsup_msg->pco = value; + gsup_msg->pco_len = value_len; + break; + case OSMO_GSUP_MSISDN_IE: gsup_msg->msisdn_enc = value; gsup_msg->msisdn_enc_len = value_len; @@ -569,6 +634,11 @@ int osmo_gsup_decode(const uint8_t *const_data, size_t data_len, gsup_msg->cause_sm = value[0]; break; + case OSMO_GSUP_NUM_VECTORS_REQ_IE: + if (gsup_msg->message_type == OSMO_GSUP_MSGT_SEND_AUTH_INFO_REQUEST) + gsup_msg->num_auth_vectors = value[0]; + break; + default: LOGP(DLGSUP, LOGL_NOTICE, "GSUP IE type %d unknown\n", iei); @@ -592,11 +662,45 @@ static void encode_pdp_info(struct msgb *msg, enum osmo_gsup_iei iei, u8 = pdp_info->context_id; msgb_tlv_put(msg, OSMO_GSUP_PDP_CONTEXT_ID_IE, sizeof(u8), &u8); - if (pdp_info->pdp_type) { - msgb_tlv_put(msg, OSMO_GSUP_PDP_TYPE_IE, - OSMO_GSUP_PDP_TYPE_SIZE, - osmo_encode_big_endian(pdp_info->pdp_type | 0xf000, - OSMO_GSUP_PDP_TYPE_SIZE)); + if (pdp_info->pdp_type_org == PDP_TYPE_ORG_IETF) { + struct gsm48_pdp_address pdp_addr; + uint8_t pdp_addr_len = 2; + pdp_addr.spare = 0x0f; + pdp_addr.organization = pdp_info->pdp_type_org; + pdp_addr.type = pdp_info->pdp_type_nr; + + switch (pdp_info->pdp_type_nr) { + case PDP_TYPE_N_IETF_IPv4: + if (pdp_info->pdp_address[0].u.sa.sa_family == AF_INET) { + pdp_addr.ietf.v4 = pdp_info->pdp_address[0].u.sin.sin_addr.s_addr; + pdp_addr_len += sizeof(pdp_addr.ietf.v4); + } + break; + case PDP_TYPE_N_IETF_IPv6: + if (pdp_info->pdp_address[0].u.sa.sa_family == AF_INET6) { + memcpy(pdp_addr.ietf.v6, + &pdp_info->pdp_address[0].u.sin6.sin6_addr, + sizeof(pdp_addr.ietf.v6)); + pdp_addr_len += sizeof(pdp_addr.ietf.v6); + } + break; + case PDP_TYPE_N_IETF_IPv4v6: + if (pdp_info->pdp_address[0].u.sa.sa_family == AF_INET) { + pdp_addr.ietf.v4v6.v4 = pdp_info->pdp_address[0].u.sin.sin_addr.s_addr; + pdp_addr_len += sizeof(pdp_addr.ietf.v4v6.v4); + } + if (pdp_info->pdp_address[0].u.sa.sa_family == AF_INET6) { + memcpy(pdp_addr.ietf.v4v6.v6, + &pdp_info->pdp_address[1].u.sin6.sin6_addr, + sizeof(pdp_addr.ietf.v4v6.v6)); + pdp_addr_len += sizeof(pdp_addr.ietf.v4v6.v6); + } + break; + } + + msgb_tlv_put(msg, OSMO_GSUP_PDP_ADDRESS_IE, + pdp_addr_len, + (const uint8_t *)&pdp_addr); } if (pdp_info->apn_enc) { @@ -753,12 +857,18 @@ int osmo_gsup_encode(struct msgb *msg, const struct osmo_gsup_message *gsup_msg) } } - for (idx = 0; idx < gsup_msg->num_auth_vectors; idx++) { - const struct osmo_auth_vector *auth_vector; + if (gsup_msg->message_type == OSMO_GSUP_MSGT_SEND_AUTH_INFO_REQUEST) { + uint8_t num = gsup_msg->num_auth_vectors; + if (num != 0) + msgb_tlv_put(msg, OSMO_GSUP_NUM_VECTORS_REQ_IE, 1, &num); + } else { + for (idx = 0; idx < gsup_msg->num_auth_vectors; idx++) { + const struct osmo_auth_vector *auth_vector; - auth_vector = &gsup_msg->auth_vectors[idx]; + auth_vector = &gsup_msg->auth_vectors[idx]; - encode_auth_info(msg, OSMO_GSUP_AUTH_TUPLE_IE, auth_vector); + encode_auth_info(msg, OSMO_GSUP_AUTH_TUPLE_IE, auth_vector); + } } if (gsup_msg->auts) @@ -767,6 +877,11 @@ int osmo_gsup_encode(struct msgb *msg, const struct osmo_gsup_message *gsup_msg) if (gsup_msg->rand) msgb_tlv_put(msg, OSMO_GSUP_RAND_IE, 16, gsup_msg->rand); + if (gsup_msg->pco && gsup_msg->pco_len > 0) { + if (gsup_msg->pco_len > OSMO_GSUP_MAX_PCO_LEN) + return -EINVAL; + msgb_tlv_put(msg, OSMO_GSUP_PCO_IE, gsup_msg->pco_len, gsup_msg->pco); + } if (gsup_msg->cn_domain) { uint8_t dn = gsup_msg->cn_domain; msgb_tlv_put(msg, OSMO_GSUP_CN_DOMAIN_IE, 1, &dn); @@ -900,6 +1015,7 @@ const struct value_string osmo_gsup_message_class_names[] = { { OSMO_GSUP_MESSAGE_CLASS_SMS, "SMS" }, { OSMO_GSUP_MESSAGE_CLASS_USSD, "USSD" }, { OSMO_GSUP_MESSAGE_CLASS_INTER_MSC, "Inter-MSC" }, + { OSMO_GSUP_MESSAGE_CLASS_IPSEC_EPDG, "IPsec-ePDG" }, {} }; diff --git a/src/gsm/ipa.c b/src/gsm/ipa.c index 1563d0a3..6e41fd98 100644 --- a/src/gsm/ipa.c +++ b/src/gsm/ipa.c @@ -121,22 +121,25 @@ int ipa_ccm_idtag_parse_off(struct tlv_parsed *dec, unsigned char *buf, int len, memset(dec, 0, sizeof(*dec)); + LOGP(DLMI, LOGL_DEBUG, "Rx IPA CCM ID_GET: "); while (len >= 2) { len -= 2; t_len = *cur++; t_tag = *cur++; if (t_len < len_offset) { + LOGPC(DLMI, LOGL_DEBUG, "\n"); LOGP(DLMI, LOGL_ERROR, "minimal offset not included: %d < %d\n", t_len, len_offset); return -EINVAL; } if (t_len > len + 1) { + LOGPC(DLMI, LOGL_DEBUG, "\n"); LOGP(DLMI, LOGL_ERROR, "The tag does not fit: %d > %d\n", t_len, len + 1); return -EINVAL; } - DEBUGPC(DLMI, "%s='%s' ", ipa_ccm_idtag_name(t_tag), cur); + LOGPC(DLMI, LOGL_DEBUG, "%s='%s' ", ipa_ccm_idtag_name(t_tag), cur); dec->lv[t_tag].len = t_len - len_offset; dec->lv[t_tag].val = cur; @@ -144,6 +147,7 @@ int ipa_ccm_idtag_parse_off(struct tlv_parsed *dec, unsigned char *buf, int len, cur += t_len - len_offset; len -= t_len - len_offset; } + LOGPC(DLMI, LOGL_DEBUG, "\n"); return 0; } @@ -164,17 +168,19 @@ int ipa_ccm_id_get_parse(struct tlv_parsed *dec, const uint8_t *buf, unsigned in memset(dec, 0, sizeof(*dec)); + LOGP(DLMI, LOGL_DEBUG, "Rx IPA CCM ID_GET: "); while (len >= 2) { len -= 2; t_len = *cur++; t_tag = *cur++; if (t_len > len + 1) { + LOGPC(DLMI, LOGL_DEBUG, "\n"); LOGP(DLMI, LOGL_ERROR, "The tag does not fit: %d > %d\n", t_len, len + 1); return -EINVAL; } - DEBUGPC(DLMI, "%s='%s' ", ipa_ccm_idtag_name(t_tag), cur); + LOGPC(DLMI, LOGL_DEBUG, "%s='%s' ", ipa_ccm_idtag_name(t_tag), cur); dec->lv[t_tag].len = t_len-1; dec->lv[t_tag].val = cur; @@ -182,6 +188,7 @@ int ipa_ccm_id_get_parse(struct tlv_parsed *dec, const uint8_t *buf, unsigned in cur += t_len-1; len -= t_len-1; } + LOGPC(DLMI, LOGL_DEBUG, "\n"); return 0; } @@ -202,6 +209,7 @@ int ipa_ccm_id_resp_parse(struct tlv_parsed *dec, const uint8_t *buf, unsigned i memset(dec, 0, sizeof(*dec)); + LOGP(DLMI, LOGL_DEBUG, "Rx IPA CCM ID_RESP: "); while (len >= 3) { len -= 3; t_len = osmo_load16be(cur); @@ -209,6 +217,7 @@ int ipa_ccm_id_resp_parse(struct tlv_parsed *dec, const uint8_t *buf, unsigned i t_tag = *cur++; if (t_len > len + 1) { + LOGPC(DLMI, LOGL_DEBUG, "\n"); LOGP(DLMI, LOGL_ERROR, "The tag does not fit: %d > %d\n", t_len, len + 1); return -EINVAL; } @@ -221,6 +230,7 @@ int ipa_ccm_id_resp_parse(struct tlv_parsed *dec, const uint8_t *buf, unsigned i cur += t_len-1; len -= t_len-1; } + LOGPC(DLMI, LOGL_DEBUG, "\n"); return 0; } @@ -257,34 +267,43 @@ int ipa_parse_unitid(const char *str, struct ipaccess_unit *unit_data) return 0; } +/*! Fill ud struct from tp structure. + * \param[in,out] ud ipaccess_unit to fill + * \param[in] tp the decoded TLV structure from eg. ID_RESP message + * \returns zero on success, negative on error + * + * This function expects parameter ud's fields to be initialized to zero if not yet set. + * Existing incoming string pointer fields are expected to be allocated using + * talloc and will be deallocated as such if replaced with the content of tp. + **/ int ipa_ccm_tlv_to_unitdata(struct ipaccess_unit *ud, const struct tlv_parsed *tp) { int rc = 0; if (TLVP_PRES_LEN(tp, IPAC_IDTAG_SERNR, 1)) - ud->serno = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_SERNR)); + osmo_talloc_replace_string(ud, &ud->serno, + (char *)TLVP_VAL(tp, IPAC_IDTAG_SERNR)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_UNITNAME, 1)) - ud->unit_name = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_UNITNAME)); + osmo_talloc_replace_string(ud, &ud->unit_name, + (char *)TLVP_VAL(tp, IPAC_IDTAG_UNITNAME)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_LOCATION1, 1)) - ud->location1 = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_LOCATION1)); + osmo_talloc_replace_string(ud, &ud->location1, + (char *)TLVP_VAL(tp, IPAC_IDTAG_LOCATION1)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_LOCATION2, 1)) - ud->location2 = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_LOCATION2)); + osmo_talloc_replace_string(ud, &ud->location2, + (char *)TLVP_VAL(tp, IPAC_IDTAG_LOCATION2)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_EQUIPVERS, 1)) - ud->equipvers = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_EQUIPVERS)); + osmo_talloc_replace_string(ud, &ud->equipvers, + (char *)TLVP_VAL(tp, IPAC_IDTAG_EQUIPVERS)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_SWVERSION, 1)) - ud->swversion = talloc_strdup(ud, (char *) - TLVP_VAL(tp, IPAC_IDTAG_SWVERSION)); + osmo_talloc_replace_string(ud, &ud->swversion, + (char *)TLVP_VAL(tp, IPAC_IDTAG_SWVERSION)); if (TLVP_PRES_LEN(tp, IPAC_IDTAG_MACADDR, 17)) { rc = osmo_macaddr_parse(ud->mac_addr, (char *) @@ -378,7 +397,7 @@ struct msgb *ipa_ccm_make_id_resp(const struct ipaccess_unit *dev, tag = msgb_put(msg, 3 + strlen(str) + 1); tag[0] = 0x00; tag[1] = 1 + strlen(str) + 1; - tag[2] = ies_req[1]; + tag[2] = ies_req[i]; memcpy(tag + 3, str, strlen(str) + 1); } ipa_prepend_header(msg, IPAC_PROTO_IPACCESS); @@ -402,10 +421,14 @@ struct msgb *ipa_ccm_make_id_resp_from_req(const struct ipaccess_unit *dev, /* build a array of the IEIs */ while (len >= 2) { uint8_t t_len, t_tag; - len -= 2; + len -= 2; /* subtract the length of the two bytes read below */ t_len = *cur++; t_tag = *cur++; + /* as the 'tag' is included in the length of t_len, this cannot happen */ + if (t_len == 0) + break; + if (t_len > len + 1) { LOGP(DLINP, LOGL_ERROR, "IPA CCM tag 0x%02x does not fit\n", t_tag); break; @@ -413,13 +436,14 @@ struct msgb *ipa_ccm_make_id_resp_from_req(const struct ipaccess_unit *dev, ies[num_ies++] = t_tag; - cur += t_len; + /* we need to subtract one from t_len to account for the tag */ + cur += t_len - 1; /* prevent any unsigned integer underflow due to somebody sending us * messages with wrong length values */ if (len <= t_len) - len -= t_len; - else len = 0; + else + len -= t_len - 1; } return ipa_ccm_make_id_resp(dev, ies, num_ies); } @@ -463,7 +487,7 @@ int ipa_ccm_rcvmsg_base(struct msgb *msg, struct osmo_fd *bfd) case IPAC_MSGT_PING: ret = ipa_ccm_send_pong(bfd->fd); if (ret < 0) { - LOGP(DLINP, LOGL_ERROR, "Cannot send PING " + LOGP(DLINP, LOGL_ERROR, "Cannot send PONG " "message. Reason: %s\n", strerror(errno)); break; } diff --git a/src/gsm/iuup.c b/src/gsm/iuup.c new file mode 100644 index 00000000..16a6f5e0 --- /dev/null +++ b/src/gsm/iuup.c @@ -0,0 +1,1064 @@ +/*! \file iu_up.c + * IuUP (Iu User Plane) according to 3GPP TS 25.415 */ +/* + * (C) 2017 by Harald Welte <laforge@gnumonks.org> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <errno.h> +#include <inttypes.h> + +#include <osmocom/core/crc8gen.h> +#include <osmocom/core/crc16gen.h> +#include <osmocom/core/fsm.h> +#include <osmocom/core/prim.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/logging.h> + +#include <osmocom/gsm/prim.h> +#include <osmocom/gsm/protocol/gsm_25_415.h> +#include <osmocom/gsm/iuup.h> + +/*********************************************************************** + * CRC Calculation + ***********************************************************************/ + +/* Section 6.6.3.8 Header CRC */ +const struct osmo_crc8gen_code iuup_hdr_crc_code = { + .bits = 6, + .poly = 47, + .init = 0, + .remainder = 0, +}; + +/* Section 6.6.3.9 Payload CRC */ +const struct osmo_crc16gen_code iuup_data_crc_code = { + .bits = 10, + .poly = 563, + .init = 0, + .remainder = 0, +}; + +static int iuup_get_payload_offset(const uint8_t *iuup_pdu) +{ + uint8_t pdu_type = iuup_pdu[0] >> 4; + switch (pdu_type) { + case 0: + case 14: + return 4; + case 1: + return 3; + default: + return -1; + } +} + +int osmo_iuup_compute_payload_crc(const uint8_t *iuup_pdu, unsigned int pdu_len) +{ + ubit_t buf[1024*8]; + uint8_t pdu_type; + int offset, payload_len_bytes; + + if (pdu_len < 1) + return -1; + + pdu_type = iuup_pdu[0] >> 4; + + /* Type 1 has no CRC */ + if (pdu_type == 1) + return 0; + + offset = iuup_get_payload_offset(iuup_pdu); + if (offset < 0) + return offset; + + if (pdu_len < offset) + return -1; + + payload_len_bytes = pdu_len - offset; + osmo_pbit2ubit(buf, iuup_pdu+offset, payload_len_bytes*8); + return osmo_crc16gen_compute_bits(&iuup_data_crc_code, buf, payload_len_bytes*8); +} + +int osmo_iuup_compute_header_crc(const uint8_t *iuup_pdu, unsigned int pdu_len) +{ + ubit_t buf[2*8]; + + if (pdu_len < 2) + return -1; + + osmo_pbit2ubit(buf, iuup_pdu, 2*8); + return osmo_crc8gen_compute_bits(&iuup_hdr_crc_code, buf, 2*8); +} + +/*********************************************************************** + * Internal State / FSM (Annex B) + ***********************************************************************/ + +#define S(x) (1 << (x)) + +#define IUUP_TIMER_INIT 1 +#define IUUP_TIMER_TA 2 +#define IUUP_TIMER_RC 3 + +struct osmo_timer_nt { + uint32_t n; /* number of repetitions */ + struct osmo_iuup_tnl_prim *retrans_itp; + struct osmo_timer_list timer; +}; + +struct osmo_iuup_instance { + struct osmo_iuup_rnl_config config; + struct osmo_fsm_inst *fi; + + uint8_t mode_version; + + struct { + struct osmo_timer_nt init; + struct osmo_timer_nt ta; + struct osmo_timer_nt rc; + } timer; + /* call-back function to pass primitives up to the user */ + osmo_prim_cb user_prim_cb; + void *user_prim_priv; + osmo_prim_cb transport_prim_cb; + void *transport_prim_priv; + uint8_t type14_fn; /* 2 bits */ +}; + +enum iuup_fsm_state { + IUUP_FSM_ST_NULL, + IUUP_FSM_ST_INIT, + IUUP_FSM_ST_TrM_DATA_XFER_READY, + IUUP_FSM_ST_SMpSDU_DATA_XFER_READY, +}; + +enum iuup_fsm_event { + IUUP_FSM_EVT_IUUP_CONFIG_REQ, + IUUP_FSM_EVT_IUUP_DATA_REQ, + IUUP_FSM_EVT_IUUP_DATA_IND, + IUUP_FSM_EVT_IUUP_STATUS_REQ, + IUUP_FSM_EVT_IUUP_STATUS_IND, + IUUP_FSM_EVT_SSASAR_UNITDATA_REQ, + IUUP_FSM_EVT_SSASAR_UNITDATA_IND, + IUUP_FSM_EVT_IUUP_UNITDATA_REQ, + IUUP_FSM_EVT_IUUP_UNITDATA_IND, + IUUP_FSM_EVT_INIT, + IUUP_FSM_EVT_LAST_INIT_ACK, + IUUP_FSM_EVT_INIT_NACK, +}; + +static const struct value_string iuup_fsm_event_names[] = { + { IUUP_FSM_EVT_IUUP_CONFIG_REQ, "IuUP-CONFIG-req" }, + { IUUP_FSM_EVT_IUUP_DATA_REQ, "IuUP-DATA-req" }, + { IUUP_FSM_EVT_IUUP_DATA_IND, "IuUP-DATA-ind" }, + { IUUP_FSM_EVT_IUUP_STATUS_REQ, "IuUP-STATUS-req" }, + { IUUP_FSM_EVT_IUUP_STATUS_IND, "IuUP-STATUS-ind" }, + { IUUP_FSM_EVT_SSASAR_UNITDATA_REQ, "SSSAR-UNITDATA-req" }, + { IUUP_FSM_EVT_SSASAR_UNITDATA_IND, "SSSAR-UNITDATA-ind" }, + { IUUP_FSM_EVT_IUUP_UNITDATA_REQ, "IuUP-UNITDATA-req" }, + { IUUP_FSM_EVT_IUUP_UNITDATA_IND, "IuUP-UNITDATA-ind" }, + { IUUP_FSM_EVT_INIT, "INIT" }, + { IUUP_FSM_EVT_LAST_INIT_ACK, "LAST_INIT_ACK" }, + { IUUP_FSM_EVT_INIT_NACK, "INIT_NACK" }, + { 0, NULL } +}; + +static inline uint8_t iuup_get_pdu_type(const uint8_t *data) +{ + return data[0] >> 4; +} + +static inline uint8_t iuup_get_hdr_crc(const uint8_t *data) +{ + return data[2] >> 2; +} + +/* Helper functions to store non-packed structs in msgb so that pointers are properly aligned: */ +#define IUUP_MSGB_SIZE 4096 +#define PTR_ALIGNMENT_BYTES 8 +#define IUUP_MSGB_HEADROOM_MIN_REQUIRED (OSMO_MAX(sizeof(struct osmo_iuup_tnl_prim), sizeof(struct osmo_iuup_rnl_prim)) + (PTR_ALIGNMENT_BYTES - 1)) +static inline struct msgb *osmo_iuup_msgb_alloc_c(void *ctx, size_t size) +{ + OSMO_ASSERT(size > IUUP_MSGB_HEADROOM_MIN_REQUIRED); + return msgb_alloc_headroom_c(ctx, size, IUUP_MSGB_HEADROOM_MIN_REQUIRED, "iuup-msgb"); +} + +/* push data so that the resulting pointer to write to is aligned to 8 byte */ +static inline __attribute__((assume_aligned(PTR_ALIGNMENT_BYTES))) +unsigned char *aligned_msgb_push(struct msgb *msg, unsigned int len) +{ + uint8_t *ptr = (msgb_data(msg) - len); + size_t extra_size = ((uintptr_t)ptr & (PTR_ALIGNMENT_BYTES - 1)); + + return msgb_push(msg, len + extra_size); +} + +struct osmo_iuup_rnl_prim *osmo_iuup_rnl_prim_alloc(void *ctx, unsigned int primitive, unsigned int operation, unsigned int size) +{ + struct msgb *msg; + struct osmo_iuup_rnl_prim *irp; + + msg = osmo_iuup_msgb_alloc_c(ctx, size); + irp = (struct osmo_iuup_rnl_prim *)aligned_msgb_push(msg, sizeof(*irp)); + osmo_prim_init(&irp->oph, SAP_IUUP_RNL, primitive, operation, msg); + return irp; +} + +struct osmo_iuup_tnl_prim *osmo_iuup_tnl_prim_alloc(void *ctx, unsigned int primitive, unsigned int operation, unsigned int size) +{ + struct msgb *msg; + struct osmo_iuup_tnl_prim *itp; + + msg = osmo_iuup_msgb_alloc_c(ctx, size); + itp = (struct osmo_iuup_tnl_prim *) aligned_msgb_push(msg, sizeof(*itp)); + osmo_prim_init(&itp->oph, SAP_IUUP_TNL, primitive, operation, msg); + return itp; +} + +/* 6.6.2.3.2 */ +static struct osmo_iuup_tnl_prim *itp_ctrl_ack_alloc(struct osmo_iuup_instance *iui, enum iuup_procedure proc_ind, uint8_t fn) +{ + struct osmo_iuup_tnl_prim *itp; + struct iuup_ctrl_ack *ack; + itp = osmo_iuup_tnl_prim_alloc(iui, OSMO_IUUP_TNL_UNITDATA, PRIM_OP_REQUEST, IUUP_MSGB_SIZE); + itp->oph.msg->l2h = msgb_put(itp->oph.msg, sizeof(struct iuup_ctrl_ack)); + ack = (struct iuup_ctrl_ack *) msgb_l2(itp->oph.msg); + *ack = (struct iuup_ctrl_ack){ + .hdr = { + .frame_nr = fn, + .ack_nack = IUUP_AN_ACK, + .pdu_type = IUUP_PDU_T_CONTROL, + .proc_ind = proc_ind, + .mode_version = iui->mode_version, + .payload_crc_hi = 0, + .header_crc = 0, + .payload_crc_lo = 0, + }, + }; + ack->hdr.header_crc = osmo_iuup_compute_header_crc(msgb_l2(itp->oph.msg), msgb_l2len(itp->oph.msg)); + return itp; +} + +/* 6.6.2.3.3 */ +static struct osmo_iuup_tnl_prim *tnp_ctrl_nack_alloc(struct osmo_iuup_instance *iui, enum iuup_procedure proc_ind, enum iuup_error_cause error_cause, uint8_t fn) +{ + struct osmo_iuup_tnl_prim *itp; + struct iuup_ctrl_nack *nack; + itp = osmo_iuup_tnl_prim_alloc(iui, OSMO_IUUP_TNL_UNITDATA, PRIM_OP_REQUEST, IUUP_MSGB_SIZE); + itp->oph.msg->l2h = msgb_put(itp->oph.msg, sizeof(struct iuup_ctrl_nack)); + nack = (struct iuup_ctrl_nack *) msgb_l2(itp->oph.msg); + *nack = (struct iuup_ctrl_nack){ + .hdr = { + .frame_nr = fn, + .ack_nack = IUUP_AN_NACK, + .pdu_type = IUUP_PDU_T_CONTROL, + .proc_ind = proc_ind, + .mode_version = iui->mode_version, + .payload_crc_hi = 0, + .header_crc = 0, + .payload_crc_lo = 0, + }, + .spare = 0, + .error_cause = error_cause, + }; + nack->hdr.header_crc = osmo_iuup_compute_header_crc(msgb_l2(itp->oph.msg), msgb_l2len(itp->oph.msg)); + return itp; +} + +/* 6.6.2.3.4.1 */ +static struct osmo_iuup_tnl_prim *tnp_ctrl_init_alloc(struct osmo_iuup_instance *iui) +{ + struct osmo_iuup_tnl_prim *itp; + struct iuup_pdutype14_hdr *hdr; + struct iuup_ctrl_init_hdr *ihdr; + struct iuup_ctrl_init_rfci_hdr *ihdr_rfci; + struct iuup_ctrl_init_tail *itail; + unsigned int i, j; + uint16_t payload_crc; + uint8_t rfci_cnt; + struct msgb *msg; + + itp = osmo_iuup_tnl_prim_alloc(iui, OSMO_IUUP_TNL_UNITDATA, PRIM_OP_REQUEST, IUUP_MSGB_SIZE); + msg = itp->oph.msg; + + msg->l2h = msgb_put(msg, sizeof(*hdr)); + hdr = (struct iuup_pdutype14_hdr *)msgb_l2(msg); + hdr->frame_nr = iui->type14_fn++; + hdr->ack_nack = IUUP_AN_PROCEDURE; + hdr->pdu_type = IUUP_PDU_T_CONTROL; + hdr->proc_ind = IUUP_PROC_INIT; + hdr->mode_version = 0; /* Use here the minimum version required to negotiate */ + hdr->header_crc = osmo_iuup_compute_header_crc(msgb_l2(msg), msgb_l2len(msg)); + + ihdr = (struct iuup_ctrl_init_hdr *)msgb_put(msg, sizeof(*ihdr)); + ihdr->chain_ind = 0; /* this frame is the last frame for the procedure. TODO: support several */ + ihdr->num_subflows_per_rfci = iui->config.num_subflows; + ihdr->ti = iui->config.IPTIs_present ? 1 : 0; + ihdr->spare = 0; + + /* RFCI + subflow size part: */ + rfci_cnt = 0; + for (i = 0; i < ARRAY_SIZE(iui->config.rfci); i++) { + bool last; + uint8_t len_size; + struct osmo_iuup_rfci *rfci = &iui->config.rfci[i]; + if (!rfci->used) + continue; + rfci_cnt++; + last = (rfci_cnt == iui->config.num_rfci); + + len_size = 1; + for (j = 0; j < iui->config.num_subflows; j++) { + if (rfci->subflow_sizes[j] > UINT8_MAX) + len_size = 2; + } + + ihdr_rfci = (struct iuup_ctrl_init_rfci_hdr *)msgb_put(msg, sizeof(*ihdr_rfci) + len_size * iui->config.num_subflows); + ihdr_rfci->rfci = rfci->id; + ihdr_rfci->li = len_size - 1; + ihdr_rfci->lri = last; + if (len_size == 2) { + uint16_t *buf = (uint16_t *)&ihdr_rfci->subflow_length[0]; + for (j = 0; j < iui->config.num_subflows; j++) + osmo_store16be(rfci->subflow_sizes[j], buf++); + } else { + for (j = 0; j < iui->config.num_subflows; j++) + ihdr_rfci->subflow_length[j] = rfci->subflow_sizes[j]; + } + /* early loop termination: */ + if (last) + break; + } + /* Sanity check: */ + if (rfci_cnt != iui->config.num_rfci) { + LOGP(DLIUUP, LOGL_ERROR, "rfci_cnt %u != num_rfci %u\n", + rfci_cnt, iui->config.num_rfci); + msgb_free(msg); + return NULL; + } + + if (iui->config.IPTIs_present) { + uint8_t num_bytes = (iui->config.num_rfci + 1) / 2; + uint8_t *buf = msgb_put(msg, num_bytes); + rfci_cnt = 0; + for (i = 0; i < ARRAY_SIZE(iui->config.rfci); i++) { + struct osmo_iuup_rfci *rfci = &iui->config.rfci[i]; + if (!rfci->used) + continue; + if (!(rfci_cnt & 0x01)) /* is even: */ + buf[rfci_cnt / 2] = (((uint8_t)rfci->IPTI) << 4); + else + buf[rfci_cnt / 2] |= (rfci->IPTI & 0x0F); + rfci_cnt++; + /* early loop termination: */ + if (rfci_cnt == iui->config.num_rfci) + break; + } + } + + itail = (struct iuup_ctrl_init_tail *)msgb_put(msg, sizeof(*itail)); + osmo_store16be(iui->config.supported_versions_mask, &itail->versions_supported); + itail->spare = 0; + itail->data_pdu_type = iui->config.data_pdu_type; + + payload_crc = osmo_iuup_compute_payload_crc(msgb_l2(msg), msgb_l2len(msg)); + hdr->payload_crc_hi = (payload_crc >> 8) & 0x03; + hdr->payload_crc_lo = payload_crc & 0xff; + + + return itp; +} + +static struct osmo_iuup_rnl_prim *irp_init_ind_alloc(struct osmo_iuup_instance *iui) +{ + struct osmo_iuup_rnl_prim *irp; + + irp = osmo_iuup_rnl_prim_alloc(iui, OSMO_IUUP_RNL_STATUS, PRIM_OP_INDICATION, IUUP_MSGB_SIZE); + irp->u.status.procedure = IUUP_PROC_INIT; + irp->u.status.u.initialization.mode_version = iui->mode_version; + irp->u.status.u.initialization.data_pdu_type = iui->config.data_pdu_type; + irp->u.status.u.initialization.num_subflows = iui->config.num_subflows; + irp->u.status.u.initialization.num_rfci = iui->config.num_rfci; + irp->u.status.u.initialization.IPTIs_present = iui->config.IPTIs_present; + memcpy(irp->u.status.u.initialization.rfci, iui->config.rfci, sizeof(iui->config.rfci)); + return irp; +} + +/* transform a RNL data primitive into a TNL data primitive (down the stack) */ +static struct osmo_iuup_tnl_prim *rnl_to_tnl_data(struct osmo_iuup_instance *iui, + struct osmo_iuup_rnl_prim *irp) +{ + struct osmo_iuup_tnl_prim *itp; + struct osmo_iuup_rnl_data dt; + struct msgb *msg; + uint16_t payload_crc; + struct iuup_pdutype0_hdr *h0; + struct iuup_pdutype1_hdr *h1; + + OSMO_ASSERT(OSMO_PRIM_HDR(&irp->oph) == OSMO_PRIM(OSMO_IUUP_RNL_DATA, PRIM_OP_REQUEST)); + + msg = irp->oph.msg; + dt = irp->u.data; + + /* pull up to the IuUP payload and push a new primitive header in front */ + msgb_pull_to_l3(msg); + + /* push the PDU TYPE 0 / 1 header in front of the payload */ + switch (iui->config.data_pdu_type) { + case 0: + msg->l2h = msgb_push(msg, sizeof(*h0)); + h0 = (struct iuup_pdutype0_hdr *)msg->l2h; + h0->frame_nr = dt.frame_nr; + h0->pdu_type = IUUP_PDU_T_DATA_CRC; + h0->rfci = dt.rfci; + h0->fqc = dt.fqc; + h0->header_crc = osmo_iuup_compute_header_crc(msgb_l2(msg), msgb_l2len(msg)); + payload_crc = osmo_iuup_compute_payload_crc(msgb_l2(msg), msgb_l2len(msg)); + h0->payload_crc_hi = (payload_crc >> 8) & 0x03; + h0->payload_crc_lo = payload_crc & 0xff; + break; + case 1: + msg->l2h = msgb_push(msg, sizeof(*h1)); + h1 = (struct iuup_pdutype1_hdr *)msg->l2h; + h1->frame_nr = dt.frame_nr; + h1->pdu_type = IUUP_PDU_T_DATA_NOCRC; + h1->rfci = dt.rfci; + h1->fqc = dt.fqc; + h1->header_crc = osmo_iuup_compute_header_crc(msgb_l2(msg), msgb_l2len(msg)); + h1->spare = 0; + break; + default: + OSMO_ASSERT(0); + } + + /* Avoid allocating irp out of 8byte-aligned address, Asan is not happy with it */ + itp = (struct osmo_iuup_tnl_prim *) aligned_msgb_push(msg, sizeof(*itp)); + osmo_prim_init(&itp->oph, SAP_IUUP_TNL, OSMO_IUUP_TNL_UNITDATA, PRIM_OP_REQUEST, msg); + + return itp; +} + +/* transform a TNL primitive into a RNL primitive (up the stack) */ +static struct osmo_iuup_rnl_prim *tnl_to_rnl_data(struct osmo_iuup_tnl_prim *itp) +{ + struct msgb *msg; + struct iuup_pdutype0_hdr *h0; + struct iuup_pdutype1_hdr *h1; + struct osmo_iuup_rnl_data dt; + struct osmo_iuup_rnl_prim *irp; + + msg = itp->oph.msg; + + OSMO_ASSERT(OSMO_PRIM_HDR(&itp->oph) == OSMO_PRIM(OSMO_IUUP_TNL_UNITDATA, PRIM_OP_INDICATION)); + + switch (iuup_get_pdu_type(msgb_l2(msg))) { + case IUUP_PDU_T_DATA_CRC: + h0 = (struct iuup_pdutype0_hdr *) msgb_l2(msg); + dt.rfci = h0->rfci; + dt.frame_nr = h0->frame_nr; + dt.fqc = h0->fqc; + break; + case IUUP_PDU_T_DATA_NOCRC: + h1 = (struct iuup_pdutype1_hdr *) msgb_l2(msg); + dt.rfci = h1->rfci; + dt.frame_nr = h1->frame_nr; + dt.fqc = h1->fqc; + break; + default: + OSMO_ASSERT(0); + } + + /* pull up to the IuUP payload and push a new primitive header in front */ + msgb_pull_to_l3(msg); + + /* Avoid allocating irp out of 8byte-aligned address, Asan is not happy with it */ + irp = (struct osmo_iuup_rnl_prim *) aligned_msgb_push(msg, sizeof(*irp)); + osmo_prim_init(&irp->oph, SAP_IUUP_RNL, OSMO_IUUP_RNL_DATA, PRIM_OP_INDICATION, msg); + irp->u.data = dt; + + return irp; +} + +static struct osmo_iuup_rnl_prim *irp_error_event_alloc_c(void *ctx, enum iuup_error_cause cause, enum iuup_error_distance distance) +{ + struct osmo_iuup_rnl_prim *irp; + struct msgb *msg; + msg = msgb_alloc_c(ctx, sizeof(*irp), "iuup-tx"); + irp = (struct osmo_iuup_rnl_prim *) msgb_put(msg, sizeof(*irp)); + osmo_prim_init(&irp->oph, SAP_IUUP_RNL, OSMO_IUUP_RNL_STATUS, PRIM_OP_INDICATION, msg); + irp->u.status.procedure = IUUP_PROC_ERR_EVENT; + irp->u.status.u.error_event.cause = cause; + irp->u.status.u.error_event.distance = distance; + return irp; +} + +static struct osmo_iuup_tnl_prim *itp_copy_c(void *ctx, const struct osmo_iuup_tnl_prim *src_itp) +{ + struct msgb *msg; + struct osmo_iuup_tnl_prim *dst_itp; + + msg = msgb_copy_c(ctx, src_itp->oph.msg, "iuup-tx-retrans"); + dst_itp = (struct osmo_iuup_tnl_prim *)msgb_data(msg); + dst_itp->oph.msg = msg; + return dst_itp; +} + +static void retransmit_initialization(struct osmo_iuup_instance *iui) +{ + struct osmo_iuup_tnl_prim *itp; + iui->fi->T = IUUP_TIMER_INIT; + osmo_timer_schedule(&iui->fi->timer, iui->config.t_init.t_ms / 1000, (iui->config.t_init.t_ms % 1000) * 1000); + itp = itp_copy_c(iui, iui->timer.init.retrans_itp); + iui->transport_prim_cb(&itp->oph, iui->transport_prim_priv); +} + +/* return: whether the last Init was Acked correctly and hence can transition to next state */ +static bool iuup_rx_initialization(struct osmo_iuup_instance *iui, struct osmo_iuup_tnl_prim *itp) +{ + struct iuup_pdutype14_hdr *hdr; + struct iuup_ctrl_init_hdr *ihdr; + struct iuup_ctrl_init_rfci_hdr *ihdr_rfci; + struct iuup_ctrl_init_tail *itail; + enum iuup_error_cause err_cause; + uint8_t num_rfci = 0; + int i; + bool is_last; + uint16_t remote_mask, match_mask; + struct osmo_iuup_rnl_prim *irp; + struct osmo_iuup_tnl_prim *resp; + + /* TODO: whenever we check message boundaries, length, etc. and we fail, send NACK */ + + hdr = (struct iuup_pdutype14_hdr *)msgb_l2(itp->oph.msg); + ihdr = (struct iuup_ctrl_init_hdr *)hdr->payload; + if (ihdr->num_subflows_per_rfci == 0) { + LOGPFSML(iui->fi, LOGL_NOTICE, "Initialization: Unexpected num_subflows=0 received\n"); + err_cause = IUUP_ERR_CAUSE_UNEXPECTED_VALUE; + goto send_nack; + } + ihdr_rfci = (struct iuup_ctrl_init_rfci_hdr *)ihdr->rfci_data; + + do { + struct osmo_iuup_rfci *rfci = &iui->config.rfci[num_rfci]; + uint8_t l_size_bytes = ihdr_rfci->li + 1; + is_last = ihdr_rfci->lri; + if (num_rfci >= IUUP_MAX_RFCIS) { + LOGPFSML(iui->fi, LOGL_NOTICE, "Initialization: Too many RFCIs received (%u)\n", + num_rfci); + err_cause = IUUP_ERR_CAUSE_UNEXPECTED_RFCI; + goto send_nack; + } + rfci->used = 1; + rfci->id = ihdr_rfci->rfci; + if (l_size_bytes == 2) { + uint16_t *subflow_size = (uint16_t *)ihdr_rfci->subflow_length; + for (i = 0; i < ihdr->num_subflows_per_rfci; i++) { + rfci->subflow_sizes[i] = osmo_load16be(subflow_size); + subflow_size++; + } + } else { + uint8_t *subflow_size = ihdr_rfci->subflow_length; + for (i = 0; i < ihdr->num_subflows_per_rfci; i++) { + rfci->subflow_sizes[i] = *subflow_size; + subflow_size++; + } + } + num_rfci++; + ihdr_rfci++; + ihdr_rfci = (struct iuup_ctrl_init_rfci_hdr *)(((uint8_t *)ihdr_rfci) + ihdr->num_subflows_per_rfci * l_size_bytes); + } while (!is_last); + + if (ihdr->ti) { /* Timing information present */ + uint8_t *buf = (uint8_t *)ihdr_rfci; + uint8_t num_bytes = (num_rfci + 1) / 2; + iui->config.IPTIs_present = true; + for (i = 0; i < num_bytes - 1; i++) { + iui->config.rfci[i*2].IPTI = *buf >> 4; + iui->config.rfci[i*2 + 1].IPTI = *buf & 0x0f; + buf++; + } + iui->config.rfci[i*2].IPTI = *buf >> 4; + if (!(num_rfci & 0x01)) /* is even: */ + iui->config.rfci[i*2 + 1].IPTI = *buf & 0x0f; + buf++; + itail = (struct iuup_ctrl_init_tail *)buf; + } else { + iui->config.IPTIs_present = false; + itail = (struct iuup_ctrl_init_tail *)ihdr_rfci; + } + if (itail->data_pdu_type > 1) { + LOGPFSML(iui->fi, LOGL_NOTICE, "Initialization: Unexpected Data PDU Type %u received\n", itail->data_pdu_type); + err_cause = IUUP_ERR_CAUSE_UNEXPECTED_VALUE; + goto send_nack; + } + + remote_mask = osmo_load16be(&itail->versions_supported); + match_mask = (remote_mask & iui->config.supported_versions_mask); + if (match_mask == 0x0000) { + LOGPFSML(iui->fi, LOGL_NOTICE, + "Initialization: No match in supported versions local=0x%04x vs remote=0x%04x\n", + iui->config.supported_versions_mask, remote_mask); + err_cause = IUUP_ERR_CAUSE_UNEXPECTED_VALUE; + goto send_nack; + } + for (i = 15; i >= 0; i--) { + if (match_mask & (1<<i)) { + iui->mode_version = i; + break; + } + } + + iui->config.num_rfci = num_rfci; + iui->config.num_subflows = ihdr->num_subflows_per_rfci; + iui->config.data_pdu_type = itail->data_pdu_type; + + irp = irp_init_ind_alloc(iui); + iui->user_prim_cb(&irp->oph, iui->user_prim_priv); + + LOGPFSML(iui->fi, LOGL_DEBUG, "Tx Initialization ACK\n"); + resp = itp_ctrl_ack_alloc(iui, IUUP_PROC_INIT, hdr->frame_nr); + iui->transport_prim_cb(&resp->oph, iui->transport_prim_priv); + return ihdr->chain_ind == 0; +send_nack: + LOGPFSML(iui->fi, LOGL_NOTICE, "Tx Initialization NACK cause=%u orig_message=%s\n", + err_cause, osmo_hexdump((const unsigned char *) msgb_l2(itp->oph.msg), msgb_l2len(itp->oph.msg))); + resp = tnp_ctrl_nack_alloc(iui, IUUP_PROC_INIT, err_cause, hdr->frame_nr); + iui->transport_prim_cb(&resp->oph, iui->transport_prim_priv); + return false; +} + +/********************** + * FSM STATE FUNCTIONS + **********************/ +static void iuup_fsm_null(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_iuup_instance *iui = fi->priv; + struct osmo_iuup_rnl_prim *user_prim = NULL; + + switch (event) { + case IUUP_FSM_EVT_IUUP_CONFIG_REQ: + user_prim = data; + iui->config = user_prim->u.config; + iui->config.supported_versions_mask &= 0x0003; /* We only support versions 1 and 2 ourselves */ + //TODO: if supported_versions_mask == 0x0000,no supported versions, send error to upper layers + + if (iui->config.transparent) + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_TrM_DATA_XFER_READY, 0, 0); + else { + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_INIT, 0, 0); + } + break; + } +} + +/* transparent mode data transfer */ +static void iuup_fsm_trm_data(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + //struct osmo_iuup_instance *iui = fi->priv; + + switch (event) { + case IUUP_FSM_EVT_IUUP_CONFIG_REQ: + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_NULL, 0, 0); + break; + case IUUP_FSM_EVT_IUUP_DATA_REQ: + /* Data coming down from RNL (user) towards TNL (transport) */ + break; + case IUUP_FSM_EVT_IUUP_DATA_IND: + /* Data coming up from TNL (transport) towards RNL (user) */ + break; + case IUUP_FSM_EVT_IUUP_UNITDATA_REQ: + case IUUP_FSM_EVT_IUUP_UNITDATA_IND: + case IUUP_FSM_EVT_SSASAR_UNITDATA_REQ: + case IUUP_FSM_EVT_SSASAR_UNITDATA_IND: + /* no state change */ + break; + } +} + +static void iuup_fsm_init_on_enter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_iuup_instance *iui = fi->priv; + + iui->type14_fn = 0; + if (iui->config.active) { + iui->timer.init.n = 0; + iui->timer.init.retrans_itp = tnp_ctrl_init_alloc(iui); + retransmit_initialization(iui); + } +} + +static void iuup_fsm_init(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_iuup_instance *iui = fi->priv; + struct osmo_iuup_rnl_prim *irp; + struct osmo_iuup_tnl_prim *itp; + + switch (event) { + case IUUP_FSM_EVT_IUUP_CONFIG_REQ: + /* the only permitted 'config req' type is the request to release the instance */ + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_NULL, 0, 0); + break; + case IUUP_FSM_EVT_INIT: + itp = data; + if (iuup_rx_initialization(iui, itp)) + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_SMpSDU_DATA_XFER_READY, 0, 0); + break; + case IUUP_FSM_EVT_LAST_INIT_ACK: + /* last INIT ACK was received, transition to DATA_XFER_READY state */ + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_SMpSDU_DATA_XFER_READY, 0, 0); + break; + case IUUP_FSM_EVT_INIT_NACK: + LOGPFSML(fi, LOGL_NOTICE, "Rx Initialization NACK N=%" PRIu32 "/%" PRIu32 "\n", + iui->timer.init.n, iui->config.t_init.n_max); + osmo_timer_del(&fi->timer); + if (iui->timer.init.n == iui->config.t_init.n_max) { + irp = irp_error_event_alloc_c(iui, IUUP_ERR_CAUSE_INIT_FAILURE_REP_NACK, IUUP_ERR_DIST_SECOND_FWD); + iui->user_prim_cb(&irp->oph, iui->user_prim_priv); + return; + } + iui->timer.init.n++; + retransmit_initialization(iui); + break; + default: + OSMO_ASSERT(false); + } +} + +/* 3GPP TS 25.415 B.2.3 "Support Mode Data Transfer Ready State" */ +static void iuup_fsm_smpsdu_data(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_iuup_instance *iui = fi->priv; + struct osmo_iuup_rnl_prim *irp = NULL; + struct osmo_iuup_tnl_prim *itp = NULL; + + switch (event) { + case IUUP_FSM_EVT_IUUP_CONFIG_REQ: + irp = data; + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_NULL, 0, 0); + break; + case IUUP_FSM_EVT_INIT: + /* "In case of handover or relocation, Initialisation procedures + * may have to be performed and Iu UP instance may have to enter + * the initialisation state." */ + itp = data; + if (!iuup_rx_initialization(iui, itp)) + osmo_fsm_inst_state_chg(fi, IUUP_FSM_ST_INIT, 0, 0); + break; + case IUUP_FSM_EVT_IUUP_DATA_REQ: + /* Data coming down from RNL (user) towards TNL (transport) */ + irp = data; + itp = rnl_to_tnl_data(iui, irp); + iui->transport_prim_cb(&itp->oph, iui->transport_prim_priv); + break; + case IUUP_FSM_EVT_IUUP_DATA_IND: + /* Data coming up from TNL (transport) towards RNL (user) */ + itp = data; + irp = tnl_to_rnl_data(itp); + iui->user_prim_cb(&irp->oph, iui->user_prim_priv); + break; + } +} + +static int iuup_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct osmo_iuup_instance *iui = fi->priv; + struct osmo_iuup_rnl_prim *irp; + + switch (fi->T) { + case IUUP_TIMER_INIT: + OSMO_ASSERT(fi->state == IUUP_FSM_ST_INIT); + if (iui->timer.init.n == iui->config.t_init.n_max) { + irp = irp_error_event_alloc_c(iui, IUUP_ERR_CAUSE_INIT_FAILURE_NET_TMR, IUUP_ERR_DIST_LOCAL); + iui->user_prim_cb(&irp->oph, iui->user_prim_priv); + return 0; + } + iui->timer.init.n++; + retransmit_initialization(iui); + break; + case IUUP_TIMER_TA: + break; + case IUUP_TIMER_RC: + break; + default: + OSMO_ASSERT(0); + } + return 0; +} + + +static const struct osmo_fsm_state iuup_fsm_states[] = { + [IUUP_FSM_ST_NULL] = { + .in_event_mask = S(IUUP_FSM_EVT_IUUP_CONFIG_REQ), + .out_state_mask = S(IUUP_FSM_ST_INIT) | + S(IUUP_FSM_ST_TrM_DATA_XFER_READY), + .name = "NULL", + .action = iuup_fsm_null, + }, + [IUUP_FSM_ST_TrM_DATA_XFER_READY] = { + .in_event_mask = S(IUUP_FSM_EVT_IUUP_CONFIG_REQ) | + S(IUUP_FSM_EVT_IUUP_STATUS_REQ) | + S(IUUP_FSM_EVT_IUUP_DATA_REQ) | + S(IUUP_FSM_EVT_IUUP_DATA_IND) | + S(IUUP_FSM_EVT_IUUP_UNITDATA_REQ) | + S(IUUP_FSM_EVT_IUUP_UNITDATA_IND) | + S(IUUP_FSM_EVT_SSASAR_UNITDATA_REQ) | + S(IUUP_FSM_EVT_SSASAR_UNITDATA_IND), + .out_state_mask = S(IUUP_FSM_ST_NULL), + .name = "TrM_Data_Transfer_Ready", + .action = iuup_fsm_trm_data, + }, + [IUUP_FSM_ST_INIT] = { + .in_event_mask = S(IUUP_FSM_EVT_IUUP_CONFIG_REQ) | + S(IUUP_FSM_EVT_INIT) | + S(IUUP_FSM_EVT_LAST_INIT_ACK) | + S(IUUP_FSM_EVT_INIT_NACK), + .out_state_mask = S(IUUP_FSM_ST_NULL) | + S(IUUP_FSM_ST_SMpSDU_DATA_XFER_READY), + .name = "Initialisation", + .onenter = iuup_fsm_init_on_enter, + .action = iuup_fsm_init, + }, + [IUUP_FSM_ST_SMpSDU_DATA_XFER_READY] = { + .in_event_mask = S(IUUP_FSM_EVT_IUUP_CONFIG_REQ) | + S(IUUP_FSM_EVT_INIT) | + S(IUUP_FSM_EVT_IUUP_DATA_REQ) | + S(IUUP_FSM_EVT_IUUP_DATA_IND), + .out_state_mask = S(IUUP_FSM_ST_NULL) | + S(IUUP_FSM_ST_INIT), + .name = "SMpSDU_Data_Transfer_Ready", + .action = iuup_fsm_smpsdu_data, + }, +}; + +static struct osmo_fsm iuup_fsm = { + .name = "IuUP", + .states = iuup_fsm_states, + .num_states = ARRAY_SIZE(iuup_fsm_states), + .timer_cb = iuup_fsm_timer_cb, + .log_subsys = DLIUUP, + .event_names = iuup_fsm_event_names, +}; + +static int iuup_verify_pdu(const uint8_t *data, unsigned int len) +{ + int header_crc_computed, payload_crc_computed; + uint16_t payload_crc; + uint8_t pdu_type = iuup_get_pdu_type(data); + struct iuup_pdutype0_hdr *t0h; + struct iuup_pdutype14_hdr *t14h; + + if (len < 3) + return -EINVAL; + + header_crc_computed = osmo_iuup_compute_header_crc(data, len); + if (iuup_get_hdr_crc(data) != header_crc_computed) { + LOGP(DLIUUP, LOGL_NOTICE, "Header Checksum error: rx 0x%02x vs exp 0x%02x\n", + iuup_get_hdr_crc(data), header_crc_computed); + return -EIO; + } + switch (pdu_type) { + case IUUP_PDU_T_DATA_NOCRC: + if (len < 4) + return -EINVAL; + break; + case IUUP_PDU_T_DATA_CRC: + t0h = (struct iuup_pdutype0_hdr *) data; + payload_crc = ((uint16_t)t0h->payload_crc_hi << 8) | t0h->payload_crc_lo; + payload_crc_computed = osmo_iuup_compute_payload_crc(data, len); + if (payload_crc != payload_crc_computed) + goto payload_crc_err; + break; + case IUUP_PDU_T_CONTROL: + t14h = (struct iuup_pdutype14_hdr *) data; + if (t14h->ack_nack == IUUP_AN_PROCEDURE) { + payload_crc = ((uint16_t)t14h->payload_crc_hi << 8) | t14h->payload_crc_lo; + payload_crc_computed = osmo_iuup_compute_payload_crc(data, len); + if (payload_crc != payload_crc_computed) + goto payload_crc_err; + } + break; + default: + return -EINVAL; + } + return 0; + +payload_crc_err: + LOGP(DLIUUP, LOGL_NOTICE, "Payload Checksum error (pdu type %u): rx 0x%02x vs exp 0x%02x\n", + pdu_type, payload_crc, payload_crc_computed); + return -EIO; +} + +/* A IuUP TNL SAP primitive from transport (lower layer) */ +int osmo_iuup_tnl_prim_up(struct osmo_iuup_instance *inst, struct osmo_iuup_tnl_prim *itp) +{ + struct osmo_prim_hdr *oph = &itp->oph; + struct iuup_pdutype14_hdr *t14h; + int rc = 0; + + OSMO_ASSERT(oph->sap == SAP_IUUP_TNL); + + switch (OSMO_PRIM_HDR(oph)) { + case OSMO_PRIM(OSMO_IUUP_TNL_UNITDATA, PRIM_OP_INDICATION): + if (iuup_verify_pdu(msgb_l2(oph->msg), msgb_l2len(oph->msg)) < 0) { + LOGPFSML(inst->fi, LOGL_NOTICE, "Discarding invalid IuUP PDU: %s\n", + osmo_hexdump((const unsigned char *) msgb_l2(oph->msg), msgb_l2len(oph->msg))); + /* don't return error as the caller is not responsible for the PDU which + * was transmitted from some remote peer */ + return 0; + } + switch (iuup_get_pdu_type(msgb_l2(oph->msg))) { + case IUUP_PDU_T_DATA_CRC: + oph->msg->l3h = msgb_l2(oph->msg) + sizeof(struct iuup_pdutype0_hdr); + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_IUUP_DATA_IND, itp); + break; + case IUUP_PDU_T_DATA_NOCRC: + oph->msg->l3h = msgb_l2(oph->msg) + sizeof(struct iuup_pdutype1_hdr); + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_IUUP_DATA_IND, itp); + break; + case IUUP_PDU_T_CONTROL: + t14h = (struct iuup_pdutype14_hdr *) msgb_l2(oph->msg); + switch (t14h->ack_nack) { + case IUUP_AN_PROCEDURE: + switch (t14h->proc_ind) { + case IUUP_PROC_INIT: + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_INIT, itp); + break; + case IUUP_PROC_RATE_CTRL: + case IUUP_PROC_TIME_ALIGN: + case IUUP_PROC_ERR_EVENT: + LOGPFSML(inst->fi, LOGL_NOTICE, "Received Request for " + "unsupported IuUP procedure %u\n", t14h->proc_ind); + break; + default: + LOGPFSML(inst->fi, LOGL_NOTICE, "Received Request for " + "unknown IuUP procedure %u\n", t14h->proc_ind); + break; + } + break; + case IUUP_AN_ACK: + switch (t14h->proc_ind) { + case IUUP_PROC_INIT: + rc = osmo_fsm_inst_dispatch(inst->fi, + IUUP_FSM_EVT_LAST_INIT_ACK, itp); + break; + default: + LOGPFSML(inst->fi, LOGL_ERROR, "Received ACK for " + "unknown IuUP procedure %u\n", t14h->proc_ind); + break; + } + break; + case IUUP_AN_NACK: + switch (t14h->proc_ind) { + case IUUP_PROC_INIT: + rc = osmo_fsm_inst_dispatch(inst->fi, + IUUP_FSM_EVT_INIT_NACK, itp); + break; + default: + LOGPFSML(inst->fi, LOGL_ERROR, "Received NACK for " + "unknown IuUP procedure %u\n", t14h->proc_ind); + break; + } + break; + default: + LOGPFSML(inst->fi, LOGL_ERROR, "Received unknown IuUP ACK/NACK\n"); + break; + } + break; + default: + LOGPFSML(inst->fi, LOGL_NOTICE, "Received unknown IuUP PDU type %u\n", + iuup_get_pdu_type(msgb_l2(oph->msg))); + break; + } + break; + default: + /* exception: return an error code due to a wrong primitive */ + return -EINVAL; + } + + return rc; +} + +/* A IuUP RNL SAP primitive from user (higher layer) */ +int osmo_iuup_rnl_prim_down(struct osmo_iuup_instance *inst, struct osmo_iuup_rnl_prim *irp) +{ + struct osmo_prim_hdr *oph = &irp->oph; + int rc; + + OSMO_ASSERT(oph->sap == SAP_IUUP_RNL); + + switch (OSMO_PRIM_HDR(oph)) { + case OSMO_PRIM(OSMO_IUUP_RNL_CONFIG, PRIM_OP_REQUEST): + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_IUUP_CONFIG_REQ, irp); + msgb_free(irp->oph.msg); + break; + case OSMO_PRIM(OSMO_IUUP_RNL_DATA, PRIM_OP_REQUEST): + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_IUUP_DATA_REQ, irp); + if (rc != 0) + msgb_free(irp->oph.msg); + break; + case OSMO_PRIM(OSMO_IUUP_RNL_STATUS, PRIM_OP_REQUEST): + rc = osmo_fsm_inst_dispatch(inst->fi, IUUP_FSM_EVT_IUUP_STATUS_REQ, irp); + msgb_free(irp->oph.msg); + break; + default: + rc = -EINVAL; + msgb_free(irp->oph.msg); + } + return rc; +} + +struct osmo_iuup_instance *osmo_iuup_instance_alloc(void *ctx, const char *id) +{ + struct osmo_iuup_instance *iui; + iui = talloc_zero(ctx, struct osmo_iuup_instance); + if (!iui) + return NULL; + + iui->fi = osmo_fsm_inst_alloc(&iuup_fsm, NULL, iui, LOGL_DEBUG, id); + if (!iui->fi) + goto free_ret; + + return iui; +free_ret: + talloc_free(iui); + return NULL; +} + +void osmo_iuup_instance_free(struct osmo_iuup_instance *iui) +{ + if (!iui) + return; + + if (iui->fi) + osmo_fsm_inst_free(iui->fi); + iui->fi = NULL; + talloc_free(iui); +} + +void osmo_iuup_instance_set_user_prim_cb(struct osmo_iuup_instance *iui, osmo_prim_cb func, void *priv) +{ + iui->user_prim_cb = func; + iui->user_prim_priv = priv; +} +void osmo_iuup_instance_set_transport_prim_cb(struct osmo_iuup_instance *iui, osmo_prim_cb func, void *priv) +{ + iui->transport_prim_cb = func; + iui->transport_prim_priv = priv; +} + +static __attribute__((constructor)) void on_dso_load_iuup_fsm(void) +{ + OSMO_ASSERT(osmo_fsm_register(&iuup_fsm) == 0); +} diff --git a/src/gsm/kasumi.c b/src/gsm/kasumi.c index f93c002b..78885bdd 100644 --- a/src/gsm/kasumi.c +++ b/src/gsm/kasumi.c @@ -16,10 +16,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> diff --git a/src/gsm/kdf.c b/src/gsm/kdf.c new file mode 100644 index 00000000..4113aada --- /dev/null +++ b/src/gsm/kdf.c @@ -0,0 +1,163 @@ +/* + * (C) 2021 by sysmocom s.f.m.c. GmbH + * + * Author: Eric Wild <ewild@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <stdint.h> +#include <string.h> + +#include "config.h" +#if (USE_GNUTLS) +#include <gnutls/gnutls.h> +#include <gnutls/crypto.h> +#define HMAC_FUNC(k,lk,s,sl,out) gnutls_hmac_fast(GNUTLS_MAC_SHA256,k,lk,s,sl,out) +#else +#include <osmocom/crypt/kdf.h> +#define HMAC_FUNC(k,lk,s,sl,out) hmac_sha256(k,lk,s,sl,out) +#endif + +#include <osmocom/core/bit32gen.h> +#include <osmocom/crypt/kdf.h> + +#include "kdf/common.h" +#include "kdf/sha256.h" + + +#if (USE_GNUTLS) +/* gnutls < 3.3.0 requires global init. + * gnutls >= 3.3.0 does it automatic. + * It doesn't hurt calling it twice, + * as long it's not done at the same time (threads). + */ +__attribute__((constructor)) +static void on_dso_load_gnutls(void) +{ + if (!gnutls_check_version("3.3.0")) + gnutls_global_init(); +} + +__attribute__((destructor)) +static void on_dso_unload_gnutls(void) +{ + if (!gnutls_check_version("3.3.0")) + gnutls_global_deinit(); +} +#endif + +/* + * This file uses the generic key derivation function defined in 3GPP TS 33.220 Annex B + * + * The S parameter always consists of concatenated values FC | P0 | L0 | Pi | Li | ... + * with Pi = Parameter number i and Li = Length of Pi (two octets) + * + * FC is either a single octet or two octets 0xff | FC + * FC values ranges depend on the specification parts that use the KDF, + * they are defined in 3GPP TS 33.220 Annex B.2.2 + * + */ + +/*! \addtogroup kdf + * @{ + * key derivation functions + * + * \file kdf.c */ + +/* 3GPP TS 33.102 B.5 */ +void osmo_kdf_kc128(const uint8_t* ck, const uint8_t* ik, uint8_t* kc128) { + uint8_t k[16*2]; + uint8_t s[1]; + uint8_t out_tmp256[32]; + memcpy (&k[0], ck, 16); + memcpy (&k[16], ik, 16); + + s[0] = 0x32; // yeah, really just one FC byte.. + + HMAC_FUNC(k, 32, s, 1, out_tmp256); + memcpy(kc128, out_tmp256, 16); +} + +/* 3GPP TS 33.401 A.2 */ +void osmo_kdf_kasme(const uint8_t *ck, const uint8_t *ik, const uint8_t* plmn_id, + const uint8_t *sqn, const uint8_t *ak, uint8_t *kasme) +{ + uint8_t s[14]; + uint8_t k[16*2]; + int i; + + memcpy(&k[0], ck, 16); + memcpy(&k[16], ik, 16); + + s[0] = 0x10; + memcpy(&s[1], plmn_id, 3); + s[4] = 0x00; + s[5] = 0x03; + + for (i = 0; i < 6; i++) + s[6+i] = sqn[i] ^ ak[i]; + s[12] = 0x00; + s[13] = 0x06; + + HMAC_FUNC(k, 32, s, 14, kasme); +} + +/* 3GPP TS 33.401 A.3 */ +void osmo_kdf_enb(const uint8_t *kasme, uint32_t ul_count, uint8_t *kenb) +{ + uint8_t s[7]; + + s[0] = 0x11; + osmo_store32be(ul_count, &s[1]); + s[5] = 0x00; + s[6] = 0x04; + + HMAC_FUNC(kasme, 32, s, 7, kenb); +} + +/* 3GPP TS 33.401 A.4 */ +void osmo_kdf_nh(const uint8_t *kasme, const uint8_t *sync_input, uint8_t *nh) +{ + uint8_t s[35]; + + s[0] = 0x12; + memcpy(s+1, sync_input, 32); + s[33] = 0x00; + s[34] = 0x20; + + HMAC_FUNC(kasme, 32, s, 35, nh); +} + +/* 3GPP TS 33.401 A.7 */ +void osmo_kdf_nas(uint8_t algo_type, uint8_t algo_id, const uint8_t *kasme, uint8_t *knas) +{ + uint8_t s[7]; + uint8_t out[32]; + + s[0] = 0x15; + s[1] = algo_type; + s[2] = 0x00; + s[3] = 0x01; + s[4] = algo_id; + s[5] = 0x00; + s[6] = 0x01; + + HMAC_FUNC(kasme, 32, s, 7, out); + memcpy(knas, out+16, 16); +} + +/*! @} */ diff --git a/src/gsm/kdf/common.h b/src/gsm/kdf/common.h new file mode 100644 index 00000000..c1ef17ef --- /dev/null +++ b/src/gsm/kdf/common.h @@ -0,0 +1,101 @@ +#pragma once + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> + +#define CONFIG_CRYPTO_INTERNAL +#define TEST_FAIL() 0 + +#define MSG_DEBUG +#define wpa_hexdump(x, args...) +#define wpa_hexdump_key(x, args...) +#define wpa_printf(x, args...) + +#define os_memcpy(x, y, z) memcpy(x, y, z) +#define os_memcmp(x, y, z) memcmp(x, y, z) +#define os_memset(x, y, z) memset(x, y, z) +#define os_malloc(x) malloc(x) +#define os_free(x) free(x) +#define os_strlen(x) strlen(x) + +#define forced_memzero(ptr, len) memset(ptr, 0, len); + +typedef uint64_t u64; +typedef uint32_t u32; +typedef uint16_t u16; +typedef uint8_t u8; +typedef int64_t s64; +typedef int32_t s32; +typedef int16_t s16; +typedef int8_t s8; + + +/* Macros for handling unaligned memory accesses */ + +#define WPA_GET_BE16(a) ((u16) (((a)[0] << 8) | (a)[1])) +#define WPA_PUT_BE16(a, val) \ + do { \ + (a)[0] = ((u16) (val)) >> 8; \ + (a)[1] = ((u16) (val)) & 0xff; \ + } while (0) + +#define WPA_GET_LE16(a) ((u16) (((a)[1] << 8) | (a)[0])) +#define WPA_PUT_LE16(a, val) \ + do { \ + (a)[1] = ((u16) (val)) >> 8; \ + (a)[0] = ((u16) (val)) & 0xff; \ + } while (0) + +#define WPA_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \ + ((u32) (a)[2])) +#define WPA_PUT_BE24(a, val) \ + do { \ + (a)[0] = (u8) ((((u32) (val)) >> 16) & 0xff); \ + (a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff); \ + (a)[2] = (u8) (((u32) (val)) & 0xff); \ + } while (0) + +#define WPA_GET_BE32(a) ((((u32) (a)[0]) << 24) | (((u32) (a)[1]) << 16) | \ + (((u32) (a)[2]) << 8) | ((u32) (a)[3])) +#define WPA_PUT_BE32(a, val) \ + do { \ + (a)[0] = (u8) ((((u32) (val)) >> 24) & 0xff); \ + (a)[1] = (u8) ((((u32) (val)) >> 16) & 0xff); \ + (a)[2] = (u8) ((((u32) (val)) >> 8) & 0xff); \ + (a)[3] = (u8) (((u32) (val)) & 0xff); \ + } while (0) + +#define WPA_GET_LE32(a) ((((u32) (a)[3]) << 24) | (((u32) (a)[2]) << 16) | \ + (((u32) (a)[1]) << 8) | ((u32) (a)[0])) +#define WPA_PUT_LE32(a, val) \ + do { \ + (a)[3] = (u8) ((((u32) (val)) >> 24) & 0xff); \ + (a)[2] = (u8) ((((u32) (val)) >> 16) & 0xff); \ + (a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff); \ + (a)[0] = (u8) (((u32) (val)) & 0xff); \ + } while (0) + +#define WPA_GET_BE64(a) ((((u64) (a)[0]) << 56) | (((u64) (a)[1]) << 48) | \ + (((u64) (a)[2]) << 40) | (((u64) (a)[3]) << 32) | \ + (((u64) (a)[4]) << 24) | (((u64) (a)[5]) << 16) | \ + (((u64) (a)[6]) << 8) | ((u64) (a)[7])) +#define WPA_PUT_BE64(a, val) \ + do { \ + (a)[0] = (u8) (((u64) (val)) >> 56); \ + (a)[1] = (u8) (((u64) (val)) >> 48); \ + (a)[2] = (u8) (((u64) (val)) >> 40); \ + (a)[3] = (u8) (((u64) (val)) >> 32); \ + (a)[4] = (u8) (((u64) (val)) >> 24); \ + (a)[5] = (u8) (((u64) (val)) >> 16); \ + (a)[6] = (u8) (((u64) (val)) >> 8); \ + (a)[7] = (u8) (((u64) (val)) & 0xff); \ + } while (0) + +#define WPA_GET_LE64(a) ((((u64) (a)[7]) << 56) | (((u64) (a)[6]) << 48) | \ + (((u64) (a)[5]) << 40) | (((u64) (a)[4]) << 32) | \ + (((u64) (a)[3]) << 24) | (((u64) (a)[2]) << 16) | \ + (((u64) (a)[1]) << 8) | ((u64) (a)[0])) + + +#define __must_check diff --git a/src/gsm/kdf/crypto.h b/src/gsm/kdf/crypto.h new file mode 100644 index 00000000..6dca191c --- /dev/null +++ b/src/gsm/kdf/crypto.h @@ -0,0 +1,470 @@ +/* + * WPA Supplicant / wrapper functions for crypto libraries + * Copyright (c) 2004-2009, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + * + * This file defines the cryptographic functions that need to be implemented + * for wpa_supplicant and hostapd. When TLS is not used, internal + * implementation of MD5, SHA1, and AES is used and no external libraries are + * required. When TLS is enabled (e.g., by enabling EAP-TLS or EAP-PEAP), the + * crypto library used by the TLS implementation is expected to be used for + * non-TLS needs, too, in order to save space by not implementing these + * functions twice. + * + * Wrapper code for using each crypto library is in its own file (crypto*.c) + * and one of these files is build and linked in to provide the functions + * defined here. + */ + +#ifndef CRYPTO_H +#define CRYPTO_H + +/** + * md4_vector - MD4 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 on failure + */ +int md4_vector(size_t num_elem, const u8 *addr[], const size_t *len, u8 *mac); + +/** + * md5_vector - MD5 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 on failure + */ +int md5_vector(size_t num_elem, const u8 *addr[], const size_t *len, u8 *mac); + +#ifdef CONFIG_FIPS +/** + * md5_vector_non_fips_allow - MD5 hash for data vector (non-FIPS use allowed) + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 on failure + */ +int md5_vector_non_fips_allow(size_t num_elem, const u8 *addr[], + const size_t *len, u8 *mac); +#else /* CONFIG_FIPS */ +#define md5_vector_non_fips_allow md5_vector +#endif /* CONFIG_FIPS */ + + +/** + * sha1_vector - SHA-1 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 on failure + */ +int sha1_vector(size_t num_elem, const u8 *addr[], const size_t *len, + u8 *mac); + +/** + * fips186_2-prf - NIST FIPS Publication 186-2 change notice 1 PRF + * @seed: Seed/key for the PRF + * @seed_len: Seed length in bytes + * @x: Buffer for PRF output + * @xlen: Output length in bytes + * Returns: 0 on success, -1 on failure + * + * This function implements random number generation specified in NIST FIPS + * Publication 186-2 for EAP-SIM. This PRF uses a function that is similar to + * SHA-1, but has different message padding. + */ +int __must_check fips186_2_prf(const u8 *seed, size_t seed_len, u8 *x, + size_t xlen); + +/** + * sha256_vector - SHA256 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 on failure + */ +int sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len, + u8 *mac); + +/** + * des_encrypt - Encrypt one block with DES + * @clear: 8 octets (in) + * @key: 7 octets (in) (no parity bits included) + * @cypher: 8 octets (out) + */ +void des_encrypt(const u8 *clear, const u8 *key, u8 *cypher); + +/** + * aes_encrypt_init - Initialize AES for encryption + * @key: Encryption key + * @len: Key length in bytes (usually 16, i.e., 128 bits) + * Returns: Pointer to context data or %NULL on failure + */ +void * aes_encrypt_init(const u8 *key, size_t len); + +/** + * aes_encrypt - Encrypt one AES block + * @ctx: Context pointer from aes_encrypt_init() + * @plain: Plaintext data to be encrypted (16 bytes) + * @crypt: Buffer for the encrypted data (16 bytes) + */ +void aes_encrypt(void *ctx, const u8 *plain, u8 *crypt); + +/** + * aes_encrypt_deinit - Deinitialize AES encryption + * @ctx: Context pointer from aes_encrypt_init() + */ +void aes_encrypt_deinit(void *ctx); + +/** + * aes_decrypt_init - Initialize AES for decryption + * @key: Decryption key + * @len: Key length in bytes (usually 16, i.e., 128 bits) + * Returns: Pointer to context data or %NULL on failure + */ +void * aes_decrypt_init(const u8 *key, size_t len); + +/** + * aes_decrypt - Decrypt one AES block + * @ctx: Context pointer from aes_encrypt_init() + * @crypt: Encrypted data (16 bytes) + * @plain: Buffer for the decrypted data (16 bytes) + */ +void aes_decrypt(void *ctx, const u8 *crypt, u8 *plain); + +/** + * aes_decrypt_deinit - Deinitialize AES decryption + * @ctx: Context pointer from aes_encrypt_init() + */ +void aes_decrypt_deinit(void *ctx); + + +enum crypto_hash_alg { + CRYPTO_HASH_ALG_MD5, CRYPTO_HASH_ALG_SHA1, + CRYPTO_HASH_ALG_HMAC_MD5, CRYPTO_HASH_ALG_HMAC_SHA1, + CRYPTO_HASH_ALG_SHA256, CRYPTO_HASH_ALG_HMAC_SHA256 +}; + +struct crypto_hash; + +/** + * crypto_hash_init - Initialize hash/HMAC function + * @alg: Hash algorithm + * @key: Key for keyed hash (e.g., HMAC) or %NULL if not needed + * @key_len: Length of the key in bytes + * Returns: Pointer to hash context to use with other hash functions or %NULL + * on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +struct crypto_hash * crypto_hash_init(enum crypto_hash_alg alg, const u8 *key, + size_t key_len); + +/** + * crypto_hash_update - Add data to hash calculation + * @ctx: Context pointer from crypto_hash_init() + * @data: Data buffer to add + * @len: Length of the buffer + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +void crypto_hash_update(struct crypto_hash *ctx, const u8 *data, size_t len); + +/** + * crypto_hash_finish - Complete hash calculation + * @ctx: Context pointer from crypto_hash_init() + * @hash: Buffer for hash value or %NULL if caller is just freeing the hash + * context + * @len: Pointer to length of the buffer or %NULL if caller is just freeing the + * hash context; on return, this is set to the actual length of the hash value + * Returns: 0 on success, -1 if buffer is too small (len set to needed length), + * or -2 on other failures (including failed crypto_hash_update() operations) + * + * This function calculates the hash value and frees the context buffer that + * was used for hash calculation. + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int crypto_hash_finish(struct crypto_hash *ctx, u8 *hash, size_t *len); + + +enum crypto_cipher_alg { + CRYPTO_CIPHER_NULL = 0, CRYPTO_CIPHER_ALG_AES, CRYPTO_CIPHER_ALG_3DES, + CRYPTO_CIPHER_ALG_DES, CRYPTO_CIPHER_ALG_RC2, CRYPTO_CIPHER_ALG_RC4 +}; + +struct crypto_cipher; + +/** + * crypto_cipher_init - Initialize block/stream cipher function + * @alg: Cipher algorithm + * @iv: Initialization vector for block ciphers or %NULL for stream ciphers + * @key: Cipher key + * @key_len: Length of key in bytes + * Returns: Pointer to cipher context to use with other cipher functions or + * %NULL on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +struct crypto_cipher * crypto_cipher_init(enum crypto_cipher_alg alg, + const u8 *iv, const u8 *key, + size_t key_len); + +/** + * crypto_cipher_encrypt - Cipher encrypt + * @ctx: Context pointer from crypto_cipher_init() + * @plain: Plaintext to cipher + * @crypt: Resulting ciphertext + * @len: Length of the plaintext + * Returns: 0 on success, -1 on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_cipher_encrypt(struct crypto_cipher *ctx, + const u8 *plain, u8 *crypt, size_t len); + +/** + * crypto_cipher_decrypt - Cipher decrypt + * @ctx: Context pointer from crypto_cipher_init() + * @crypt: Ciphertext to decrypt + * @plain: Resulting plaintext + * @len: Length of the cipher text + * Returns: 0 on success, -1 on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_cipher_decrypt(struct crypto_cipher *ctx, + const u8 *crypt, u8 *plain, size_t len); + +/** + * crypto_cipher_decrypt - Free cipher context + * @ctx: Context pointer from crypto_cipher_init() + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +void crypto_cipher_deinit(struct crypto_cipher *ctx); + + +struct crypto_public_key; +struct crypto_private_key; + +/** + * crypto_public_key_import - Import an RSA public key + * @key: Key buffer (DER encoded RSA public key) + * @len: Key buffer length in bytes + * Returns: Pointer to the public key or %NULL on failure + * + * This function can just return %NULL if the crypto library supports X.509 + * parsing. In that case, crypto_public_key_from_cert() is used to import the + * public key from a certificate. + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +struct crypto_public_key * crypto_public_key_import(const u8 *key, size_t len); + +/** + * crypto_private_key_import - Import an RSA private key + * @key: Key buffer (DER encoded RSA private key) + * @len: Key buffer length in bytes + * @passwd: Key encryption password or %NULL if key is not encrypted + * Returns: Pointer to the private key or %NULL on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +struct crypto_private_key * crypto_private_key_import(const u8 *key, + size_t len, + const char *passwd); + +/** + * crypto_public_key_from_cert - Import an RSA public key from a certificate + * @buf: DER encoded X.509 certificate + * @len: Certificate buffer length in bytes + * Returns: Pointer to public key or %NULL on failure + * + * This function can just return %NULL if the crypto library does not support + * X.509 parsing. In that case, internal code will be used to parse the + * certificate and public key is imported using crypto_public_key_import(). + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +struct crypto_public_key * crypto_public_key_from_cert(const u8 *buf, + size_t len); + +/** + * crypto_public_key_encrypt_pkcs1_v15 - Public key encryption (PKCS #1 v1.5) + * @key: Public key + * @in: Plaintext buffer + * @inlen: Length of plaintext buffer in bytes + * @out: Output buffer for encrypted data + * @outlen: Length of output buffer in bytes; set to used length on success + * Returns: 0 on success, -1 on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_public_key_encrypt_pkcs1_v15( + struct crypto_public_key *key, const u8 *in, size_t inlen, + u8 *out, size_t *outlen); + +/** + * crypto_private_key_decrypt_pkcs1_v15 - Private key decryption (PKCS #1 v1.5) + * @key: Private key + * @in: Encrypted buffer + * @inlen: Length of encrypted buffer in bytes + * @out: Output buffer for encrypted data + * @outlen: Length of output buffer in bytes; set to used length on success + * Returns: 0 on success, -1 on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_private_key_decrypt_pkcs1_v15( + struct crypto_private_key *key, const u8 *in, size_t inlen, + u8 *out, size_t *outlen); + +/** + * crypto_private_key_sign_pkcs1 - Sign with private key (PKCS #1) + * @key: Private key from crypto_private_key_import() + * @in: Plaintext buffer + * @inlen: Length of plaintext buffer in bytes + * @out: Output buffer for encrypted (signed) data + * @outlen: Length of output buffer in bytes; set to used length on success + * Returns: 0 on success, -1 on failure + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_private_key_sign_pkcs1(struct crypto_private_key *key, + const u8 *in, size_t inlen, + u8 *out, size_t *outlen); + +/** + * crypto_public_key_free - Free public key + * @key: Public key + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +void crypto_public_key_free(struct crypto_public_key *key); + +/** + * crypto_private_key_free - Free private key + * @key: Private key from crypto_private_key_import() + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +void crypto_private_key_free(struct crypto_private_key *key); + +/** + * crypto_public_key_decrypt_pkcs1 - Decrypt PKCS #1 signature + * @key: Public key + * @crypt: Encrypted signature data (using the private key) + * @crypt_len: Encrypted signature data length + * @plain: Buffer for plaintext (at least crypt_len bytes) + * @plain_len: Plaintext length (max buffer size on input, real len on output); + * Returns: 0 on success, -1 on failure + */ +int __must_check crypto_public_key_decrypt_pkcs1( + struct crypto_public_key *key, const u8 *crypt, size_t crypt_len, + u8 *plain, size_t *plain_len); + +/** + * crypto_global_init - Initialize crypto wrapper + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_global_init(void); + +/** + * crypto_global_deinit - Deinitialize crypto wrapper + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +void crypto_global_deinit(void); + +/** + * crypto_mod_exp - Modular exponentiation of large integers + * @base: Base integer (big endian byte array) + * @base_len: Length of base integer in bytes + * @power: Power integer (big endian byte array) + * @power_len: Length of power integer in bytes + * @modulus: Modulus integer (big endian byte array) + * @modulus_len: Length of modulus integer in bytes + * @result: Buffer for the result + * @result_len: Result length (max buffer size on input, real len on output) + * Returns: 0 on success, -1 on failure + * + * This function calculates result = base ^ power mod modulus. modules_len is + * used as the maximum size of modulus buffer. It is set to the used size on + * success. + * + * This function is only used with internal TLSv1 implementation + * (CONFIG_TLS=internal). If that is not used, the crypto wrapper does not need + * to implement this. + */ +int __must_check crypto_mod_exp(const u8 *base, size_t base_len, + const u8 *power, size_t power_len, + const u8 *modulus, size_t modulus_len, + u8 *result, size_t *result_len); + +/** + * rc4_skip - XOR RC4 stream to given data with skip-stream-start + * @key: RC4 key + * @keylen: RC4 key length + * @skip: number of bytes to skip from the beginning of the RC4 stream + * @data: data to be XOR'ed with RC4 stream + * @data_len: buf length + * Returns: 0 on success, -1 on failure + * + * Generate RC4 pseudo random stream for the given key, skip beginning of the + * stream, and XOR the end result with the data buffer to perform RC4 + * encryption/decryption. + */ +int rc4_skip(const u8 *key, size_t keylen, size_t skip, + u8 *data, size_t data_len); + +#endif /* CRYPTO_H */ diff --git a/src/gsm/kdf/sha1-internal.c b/src/gsm/kdf/sha1-internal.c new file mode 100644 index 00000000..2216b439 --- /dev/null +++ b/src/gsm/kdf/sha1-internal.c @@ -0,0 +1,307 @@ +/* + * SHA1 hash implementation and interface functions + * Copyright (c) 2003-2005, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + + +#include "common.h" +#include "sha1.h" +#include "sha1_i.h" +//#include "md5.h" +#include "crypto.h" + +typedef struct SHA1Context SHA1_CTX; + +void SHA1Transform(u32 state[5], const unsigned char buffer[64]); + + +/** + * sha1_vector - SHA-1 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 of failure + */ +int sha1_vector(size_t num_elem, const u8 *addr[], const size_t *len, u8 *mac) +{ + SHA1_CTX ctx; + size_t i; + + SHA1Init(&ctx); + for (i = 0; i < num_elem; i++) + SHA1Update(&ctx, addr[i], len[i]); + SHA1Final(mac, &ctx); + return 0; +} + + +/* ===== start - public domain SHA1 implementation ===== */ + +/* +SHA-1 in C +By Steve Reid <sreid@sea-to-sky.net> +100% Public Domain + +----------------- +Modified 7/98 +By James H. Brown <jbrown@burgoyne.com> +Still 100% Public Domain + +Corrected a problem which generated improper hash values on 16 bit machines +Routine SHA1Update changed from + void SHA1Update(SHA1_CTX* context, unsigned char* data, unsigned int +len) +to + void SHA1Update(SHA1_CTX* context, unsigned char* data, unsigned +long len) + +The 'len' parameter was declared an int which works fine on 32 bit machines. +However, on 16 bit machines an int is too small for the shifts being done +against +it. This caused the hash function to generate incorrect values if len was +greater than 8191 (8K - 1) due to the 'len << 3' on line 3 of SHA1Update(). + +Since the file IO in main() reads 16K at a time, any file 8K or larger would +be guaranteed to generate the wrong hash (e.g. Test Vector #3, a million +"a"s). + +I also changed the declaration of variables i & j in SHA1Update to +unsigned long from unsigned int for the same reason. + +These changes should make no difference to any 32 bit implementations since +an +int and a long are the same size in those environments. + +-- +I also corrected a few compiler warnings generated by Borland C. +1. Added #include <process.h> for exit() prototype +2. Removed unused variable 'j' in SHA1Final +3. Changed exit(0) to return(0) at end of main. + +ALL changes I made can be located by searching for comments containing 'JHB' +----------------- +Modified 8/98 +By Steve Reid <sreid@sea-to-sky.net> +Still 100% public domain + +1- Removed #include <process.h> and used return() instead of exit() +2- Fixed overwriting of finalcount in SHA1Final() (discovered by Chris Hall) +3- Changed email address from steve@edmweb.com to sreid@sea-to-sky.net + +----------------- +Modified 4/01 +By Saul Kravitz <Saul.Kravitz@celera.com> +Still 100% PD +Modified to run on Compaq Alpha hardware. + +----------------- +Modified 4/01 +By Jouni Malinen <j@w1.fi> +Minor changes to match the coding style used in Dynamics. + +Modified September 24, 2004 +By Jouni Malinen <j@w1.fi> +Fixed alignment issue in SHA1Transform when SHA1HANDSOFF is defined. + +*/ + +/* +Test Vectors (from FIPS PUB 180-1) +"abc" + A9993E36 4706816A BA3E2571 7850C26C 9CD0D89D +"abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq" + 84983E44 1C3BD26E BAAE4AA1 F95129E5 E54670F1 +A million repetitions of "a" + 34AA973C D4C4DAA4 F61EEB2B DBAD2731 6534016F +*/ + +#define SHA1HANDSOFF + +#define rol(value, bits) (((value) << (bits)) | ((value) >> (32 - (bits)))) + +/* blk0() and blk() perform the initial expand. */ +/* I got the idea of expanding during the round function from SSLeay */ +#ifndef WORDS_BIGENDIAN +#define blk0(i) (block->l[i] = (rol(block->l[i], 24) & 0xFF00FF00) | \ + (rol(block->l[i], 8) & 0x00FF00FF)) +#else +#define blk0(i) block->l[i] +#endif +#define blk(i) (block->l[i & 15] = rol(block->l[(i + 13) & 15] ^ \ + block->l[(i + 8) & 15] ^ block->l[(i + 2) & 15] ^ block->l[i & 15], 1)) + +/* (R0+R1), R2, R3, R4 are the different operations used in SHA1 */ +#define R0(v,w,x,y,z,i) \ + z += ((w & (x ^ y)) ^ y) + blk0(i) + 0x5A827999 + rol(v, 5); \ + w = rol(w, 30); +#define R1(v,w,x,y,z,i) \ + z += ((w & (x ^ y)) ^ y) + blk(i) + 0x5A827999 + rol(v, 5); \ + w = rol(w, 30); +#define R2(v,w,x,y,z,i) \ + z += (w ^ x ^ y) + blk(i) + 0x6ED9EBA1 + rol(v, 5); w = rol(w, 30); +#define R3(v,w,x,y,z,i) \ + z += (((w | x) & y) | (w & x)) + blk(i) + 0x8F1BBCDC + rol(v, 5); \ + w = rol(w, 30); +#define R4(v,w,x,y,z,i) \ + z += (w ^ x ^ y) + blk(i) + 0xCA62C1D6 + rol(v, 5); \ + w=rol(w, 30); + + +#ifdef VERBOSE /* SAK */ +void SHAPrintContext(SHA1_CTX *context, char *msg) +{ + printf("%s (%d,%d) %x %x %x %x %x\n", + msg, + context->count[0], context->count[1], + context->state[0], + context->state[1], + context->state[2], + context->state[3], + context->state[4]); +} +#endif + +/* Hash a single 512-bit block. This is the core of the algorithm. */ + +void SHA1Transform(u32 state[5], const unsigned char buffer[64]) +{ + u32 a, b, c, d, e; + typedef union { + unsigned char c[64]; + u32 l[16]; + } CHAR64LONG16; + CHAR64LONG16* block; +#ifdef SHA1HANDSOFF + CHAR64LONG16 workspace; + block = &workspace; + os_memcpy(block, buffer, 64); +#else + block = (CHAR64LONG16 *) buffer; +#endif + /* Copy context->state[] to working vars */ + a = state[0]; + b = state[1]; + c = state[2]; + d = state[3]; + e = state[4]; + /* 4 rounds of 20 operations each. Loop unrolled. */ + R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3); + R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7); + R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11); + R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15); + R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19); + R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23); + R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27); + R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31); + R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35); + R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39); + R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43); + R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47); + R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51); + R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55); + R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59); + R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63); + R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67); + R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71); + R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75); + R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79); + /* Add the working vars back into context.state[] */ + state[0] += a; + state[1] += b; + state[2] += c; + state[3] += d; + state[4] += e; + /* Wipe variables */ + a = b = c = d = e = 0; +#ifdef SHA1HANDSOFF + os_memset(block, 0, 64); +#endif +} + + +/* SHA1Init - Initialize new context */ + +void SHA1Init(SHA1_CTX* context) +{ + /* SHA1 initialization constants */ + context->state[0] = 0x67452301; + context->state[1] = 0xEFCDAB89; + context->state[2] = 0x98BADCFE; + context->state[3] = 0x10325476; + context->state[4] = 0xC3D2E1F0; + context->count[0] = context->count[1] = 0; +} + + +/* Run your data through this. */ + +void SHA1Update(SHA1_CTX* context, const void *_data, u32 len) +{ + u32 i, j; + const unsigned char *data = _data; + +#ifdef VERBOSE + SHAPrintContext(context, "before"); +#endif + j = (context->count[0] >> 3) & 63; + if ((context->count[0] += len << 3) < (len << 3)) + context->count[1]++; + context->count[1] += (len >> 29); + if ((j + len) > 63) { + os_memcpy(&context->buffer[j], data, (i = 64-j)); + SHA1Transform(context->state, context->buffer); + for ( ; i + 63 < len; i += 64) { + SHA1Transform(context->state, &data[i]); + } + j = 0; + } + else i = 0; + os_memcpy(&context->buffer[j], &data[i], len - i); +#ifdef VERBOSE + SHAPrintContext(context, "after "); +#endif +} + + +/* Add padding and return the message digest. */ + +void SHA1Final(unsigned char digest[20], SHA1_CTX* context) +{ + u32 i; + unsigned char finalcount[8]; + + for (i = 0; i < 8; i++) { + finalcount[i] = (unsigned char) + ((context->count[(i >= 4 ? 0 : 1)] >> + ((3-(i & 3)) * 8) ) & 255); /* Endian independent */ + } + SHA1Update(context, (unsigned char *) "\200", 1); + while ((context->count[0] & 504) != 448) { + SHA1Update(context, (unsigned char *) "\0", 1); + } + SHA1Update(context, finalcount, 8); /* Should cause a SHA1Transform() + */ + for (i = 0; i < 20; i++) { + digest[i] = (unsigned char) + ((context->state[i >> 2] >> ((3 - (i & 3)) * 8)) & + 255); + } + /* Wipe variables */ + i = 0; + os_memset(context->buffer, 0, 64); + os_memset(context->state, 0, 20); + os_memset(context->count, 0, 8); + os_memset(finalcount, 0, 8); +} + +/* ===== end - public domain SHA1 implementation ===== */ diff --git a/src/gsm/kdf/sha1.c b/src/gsm/kdf/sha1.c new file mode 100644 index 00000000..0d39f77b --- /dev/null +++ b/src/gsm/kdf/sha1.c @@ -0,0 +1,162 @@ +/* + * SHA1 hash implementation and interface functions + * Copyright (c) 2003-2005, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + + +#include "common.h" +#include "sha1.h" +#include "crypto.h" + + +/** + * hmac_sha1_vector - HMAC-SHA1 over data vector (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash (20 bytes) + * Returns: 0 on success, -1 on failure + */ +int hmac_sha1_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac) +{ + unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */ + unsigned char tk[20]; + const u8 *_addr[6]; + size_t _len[6], i; + + if (num_elem > 5) { + /* + * Fixed limit on the number of fragments to avoid having to + * allocate memory (which could fail). + */ + return -1; + } + + /* if key is longer than 64 bytes reset it to key = SHA1(key) */ + if (key_len > 64) { + if (sha1_vector(1, &key, &key_len, tk)) + return -1; + key = tk; + key_len = 20; + } + + /* the HMAC_SHA1 transform looks like: + * + * SHA1(K XOR opad, SHA1(K XOR ipad, text)) + * + * where K is an n byte key + * ipad is the byte 0x36 repeated 64 times + * opad is the byte 0x5c repeated 64 times + * and text is the data being protected */ + + /* start out by storing key in ipad */ + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with ipad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x36; + + /* perform inner SHA1 */ + _addr[0] = k_pad; + _len[0] = 64; + for (i = 0; i < num_elem; i++) { + _addr[i + 1] = addr[i]; + _len[i + 1] = len[i]; + } + if (sha1_vector(1 + num_elem, _addr, _len, mac)) + return -1; + + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with opad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x5c; + + /* perform outer SHA1 */ + _addr[0] = k_pad; + _len[0] = 64; + _addr[1] = mac; + _len[1] = SHA1_MAC_LEN; + return sha1_vector(2, _addr, _len, mac); +} + + +/** + * hmac_sha1 - HMAC-SHA1 over data buffer (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @data: Pointers to the data area + * @data_len: Length of the data area + * @mac: Buffer for the hash (20 bytes) + * Returns: 0 on success, -1 of failure + */ +int hmac_sha1(const u8 *key, size_t key_len, const u8 *data, size_t data_len, + u8 *mac) +{ + return hmac_sha1_vector(key, key_len, 1, &data, &data_len, mac); +} + + +/** + * sha1_prf - SHA1-based Pseudo-Random Function (PRF) (IEEE 802.11i, 8.5.1.1) + * @key: Key for PRF + * @key_len: Length of the key in bytes + * @label: A unique label for each purpose of the PRF + * @data: Extra data to bind into the key + * @data_len: Length of the data + * @buf: Buffer for the generated pseudo-random key + * @buf_len: Number of bytes of key to generate + * Returns: 0 on success, -1 of failure + * + * This function is used to derive new, cryptographically separate keys from a + * given key (e.g., PMK in IEEE 802.11i). + */ +int sha1_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len) +{ + u8 counter = 0; + size_t pos, plen; + u8 hash[SHA1_MAC_LEN]; + size_t label_len = os_strlen(label) + 1; + const unsigned char *addr[3]; + size_t len[3]; + + addr[0] = (u8 *) label; + len[0] = label_len; + addr[1] = data; + len[1] = data_len; + addr[2] = &counter; + len[2] = 1; + + pos = 0; + while (pos < buf_len) { + plen = buf_len - pos; + if (plen >= SHA1_MAC_LEN) { + if (hmac_sha1_vector(key, key_len, 3, addr, len, + &buf[pos])) + return -1; + pos += SHA1_MAC_LEN; + } else { + if (hmac_sha1_vector(key, key_len, 3, addr, len, + hash)) + return -1; + os_memcpy(&buf[pos], hash, plen); + break; + } + counter++; + } + + return 0; +} diff --git a/src/gsm/kdf/sha1.h b/src/gsm/kdf/sha1.h new file mode 100644 index 00000000..f0c1a5f9 --- /dev/null +++ b/src/gsm/kdf/sha1.h @@ -0,0 +1,33 @@ +/* + * SHA1 hash implementation and interface functions + * Copyright (c) 2003-2009, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + +#ifndef SHA1_H +#define SHA1_H + +#define SHA1_MAC_LEN 20 + +int hmac_sha1_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac); +int hmac_sha1(const u8 *key, size_t key_len, const u8 *data, size_t data_len, + u8 *mac); +int sha1_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len); +int sha1_t_prf(const u8 *key, size_t key_len, const char *label, + const u8 *seed, size_t seed_len, u8 *buf, size_t buf_len); +int __must_check tls_prf_sha1_md5(const u8 *secret, size_t secret_len, + const char *label, const u8 *seed, + size_t seed_len, u8 *out, size_t outlen); +int pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len, + int iterations, u8 *buf, size_t buflen); +#endif /* SHA1_H */ diff --git a/src/gsm/kdf/sha1_i.h b/src/gsm/kdf/sha1_i.h new file mode 100644 index 00000000..ec2f82f7 --- /dev/null +++ b/src/gsm/kdf/sha1_i.h @@ -0,0 +1,29 @@ +/* + * SHA1 internal definitions + * Copyright (c) 2003-2005, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + +#ifndef SHA1_I_H +#define SHA1_I_H + +struct SHA1Context { + u32 state[5]; + u32 count[2]; + unsigned char buffer[64]; +}; + +void SHA1Init(struct SHA1Context *context); +void SHA1Update(struct SHA1Context *context, const void *data, u32 len); +void SHA1Final(unsigned char digest[20], struct SHA1Context *context); +void SHA1Transform(u32 state[5], const unsigned char buffer[64]); + +#endif /* SHA1_I_H */ diff --git a/src/gsm/kdf/sha256-internal.c b/src/gsm/kdf/sha256-internal.c new file mode 100644 index 00000000..18cc8f80 --- /dev/null +++ b/src/gsm/kdf/sha256-internal.c @@ -0,0 +1,231 @@ +/* + * SHA-256 hash implementation and interface functions + * Copyright (c) 2003-2011, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + + +#include "common.h" +#include "sha256.h" +#include "sha256_i.h" +#include "crypto.h" + + +/** + * sha256_vector - SHA256 hash for data vector + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash + * Returns: 0 on success, -1 of failure + */ +int sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len, + u8 *mac) +{ + struct sha256_state ctx; + size_t i; + + sha256_init(&ctx); + for (i = 0; i < num_elem; i++) + if (sha256_process(&ctx, addr[i], len[i])) + return -1; + if (sha256_done(&ctx, mac)) + return -1; + return 0; +} + + +/* ===== start - public domain SHA256 implementation ===== */ + +/* This is based on SHA256 implementation in LibTomCrypt that was released into + * public domain by Tom St Denis. */ + +/* the K array */ +static const unsigned long K[64] = { + 0x428a2f98UL, 0x71374491UL, 0xb5c0fbcfUL, 0xe9b5dba5UL, 0x3956c25bUL, + 0x59f111f1UL, 0x923f82a4UL, 0xab1c5ed5UL, 0xd807aa98UL, 0x12835b01UL, + 0x243185beUL, 0x550c7dc3UL, 0x72be5d74UL, 0x80deb1feUL, 0x9bdc06a7UL, + 0xc19bf174UL, 0xe49b69c1UL, 0xefbe4786UL, 0x0fc19dc6UL, 0x240ca1ccUL, + 0x2de92c6fUL, 0x4a7484aaUL, 0x5cb0a9dcUL, 0x76f988daUL, 0x983e5152UL, + 0xa831c66dUL, 0xb00327c8UL, 0xbf597fc7UL, 0xc6e00bf3UL, 0xd5a79147UL, + 0x06ca6351UL, 0x14292967UL, 0x27b70a85UL, 0x2e1b2138UL, 0x4d2c6dfcUL, + 0x53380d13UL, 0x650a7354UL, 0x766a0abbUL, 0x81c2c92eUL, 0x92722c85UL, + 0xa2bfe8a1UL, 0xa81a664bUL, 0xc24b8b70UL, 0xc76c51a3UL, 0xd192e819UL, + 0xd6990624UL, 0xf40e3585UL, 0x106aa070UL, 0x19a4c116UL, 0x1e376c08UL, + 0x2748774cUL, 0x34b0bcb5UL, 0x391c0cb3UL, 0x4ed8aa4aUL, 0x5b9cca4fUL, + 0x682e6ff3UL, 0x748f82eeUL, 0x78a5636fUL, 0x84c87814UL, 0x8cc70208UL, + 0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL +}; + + +/* Various logical functions */ +#define RORc(x, y) \ +( ((((unsigned long) (x) & 0xFFFFFFFFUL) >> (unsigned long) ((y) & 31)) | \ + ((unsigned long) (x) << (unsigned long) (32 - ((y) & 31)))) & 0xFFFFFFFFUL) +#define Ch(x,y,z) (z ^ (x & (y ^ z))) +#define Maj(x,y,z) (((x | y) & z) | (x & y)) +#define S(x, n) RORc((x), (n)) +#define R(x, n) (((x)&0xFFFFFFFFUL)>>(n)) +#define Sigma0(x) (S(x, 2) ^ S(x, 13) ^ S(x, 22)) +#define Sigma1(x) (S(x, 6) ^ S(x, 11) ^ S(x, 25)) +#define Gamma0(x) (S(x, 7) ^ S(x, 18) ^ R(x, 3)) +#define Gamma1(x) (S(x, 17) ^ S(x, 19) ^ R(x, 10)) +#ifndef MIN +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) +#endif + +/* compress 512-bits */ +static int sha256_compress(struct sha256_state *md, unsigned char *buf) +{ + u32 S[8], W[64], t0, t1; + u32 t; + int i; + + /* copy state into S */ + for (i = 0; i < 8; i++) { + S[i] = md->state[i]; + } + + /* copy the state into 512-bits into W[0..15] */ + for (i = 0; i < 16; i++) + W[i] = WPA_GET_BE32(buf + (4 * i)); + + /* fill W[16..63] */ + for (i = 16; i < 64; i++) { + W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) + + W[i - 16]; + } + + /* Compress */ +#define RND(a,b,c,d,e,f,g,h,i) \ + t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i]; \ + t1 = Sigma0(a) + Maj(a, b, c); \ + d += t0; \ + h = t0 + t1; + + for (i = 0; i < 64; ++i) { + RND(S[0], S[1], S[2], S[3], S[4], S[5], S[6], S[7], i); + t = S[7]; S[7] = S[6]; S[6] = S[5]; S[5] = S[4]; + S[4] = S[3]; S[3] = S[2]; S[2] = S[1]; S[1] = S[0]; S[0] = t; + } + + /* feedback */ + for (i = 0; i < 8; i++) { + md->state[i] = md->state[i] + S[i]; + } + return 0; +} + + +/* Initialize the hash state */ +void sha256_init(struct sha256_state *md) +{ + md->curlen = 0; + md->length = 0; + md->state[0] = 0x6A09E667UL; + md->state[1] = 0xBB67AE85UL; + md->state[2] = 0x3C6EF372UL; + md->state[3] = 0xA54FF53AUL; + md->state[4] = 0x510E527FUL; + md->state[5] = 0x9B05688CUL; + md->state[6] = 0x1F83D9ABUL; + md->state[7] = 0x5BE0CD19UL; +} + +/** + Process a block of memory though the hash + @param md The hash state + @param in The data to hash + @param inlen The length of the data (octets) + @return CRYPT_OK if successful +*/ +int sha256_process(struct sha256_state *md, const unsigned char *in, + unsigned long inlen) +{ + unsigned long n; + + if (md->curlen >= sizeof(md->buf)) + return -1; + + while (inlen > 0) { + if (md->curlen == 0 && inlen >= SHA256_BLOCK_SIZE) { + if (sha256_compress(md, (unsigned char *) in) < 0) + return -1; + md->length += SHA256_BLOCK_SIZE * 8; + in += SHA256_BLOCK_SIZE; + inlen -= SHA256_BLOCK_SIZE; + } else { + n = MIN(inlen, (SHA256_BLOCK_SIZE - md->curlen)); + os_memcpy(md->buf + md->curlen, in, n); + md->curlen += n; + in += n; + inlen -= n; + if (md->curlen == SHA256_BLOCK_SIZE) { + if (sha256_compress(md, md->buf) < 0) + return -1; + md->length += 8 * SHA256_BLOCK_SIZE; + md->curlen = 0; + } + } + } + + return 0; +} + + +/** + Terminate the hash to get the digest + @param md The hash state + @param out [out] The destination of the hash (32 bytes) + @return CRYPT_OK if successful +*/ +int sha256_done(struct sha256_state *md, unsigned char *out) +{ + int i; + + if (md->curlen >= sizeof(md->buf)) + return -1; + + /* increase the length of the message */ + md->length += md->curlen * 8; + + /* append the '1' bit */ + md->buf[md->curlen++] = (unsigned char) 0x80; + + /* if the length is currently above 56 bytes we append zeros + * then compress. Then we can fall back to padding zeros and length + * encoding like normal. + */ + if (md->curlen > 56) { + while (md->curlen < SHA256_BLOCK_SIZE) { + md->buf[md->curlen++] = (unsigned char) 0; + } + sha256_compress(md, md->buf); + md->curlen = 0; + } + + /* pad up to 56 bytes of zeroes */ + while (md->curlen < 56) { + md->buf[md->curlen++] = (unsigned char) 0; + } + + /* store length */ + WPA_PUT_BE64(md->buf + 56, md->length); + sha256_compress(md, md->buf); + + /* copy output */ + for (i = 0; i < 8; i++) + WPA_PUT_BE32(out + (4 * i), md->state[i]); + + return 0; +} + +/* ===== end - public domain SHA256 implementation ===== */ diff --git a/src/gsm/kdf/sha256.c b/src/gsm/kdf/sha256.c new file mode 100644 index 00000000..4f8b6cc5 --- /dev/null +++ b/src/gsm/kdf/sha256.c @@ -0,0 +1,156 @@ +/* + * SHA-256 hash implementation and interface functions + * Copyright (c) 2003-2007, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + + +#include "common.h" +#include "sha256.h" +#include "crypto.h" + + +/** + * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @num_elem: Number of elements in the data vector + * @addr: Pointers to the data areas + * @len: Lengths of the data blocks + * @mac: Buffer for the hash (32 bytes) + */ +void hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac) +{ + unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */ + unsigned char tk[32]; + const u8 *_addr[6]; + size_t _len[6], i; + + if (num_elem > 5) { + /* + * Fixed limit on the number of fragments to avoid having to + * allocate memory (which could fail). + */ + return; + } + + /* if key is longer than 64 bytes reset it to key = SHA256(key) */ + if (key_len > 64) { + sha256_vector(1, &key, &key_len, tk); + key = tk; + key_len = 32; + } + + /* the HMAC_SHA256 transform looks like: + * + * SHA256(K XOR opad, SHA256(K XOR ipad, text)) + * + * where K is an n byte key + * ipad is the byte 0x36 repeated 64 times + * opad is the byte 0x5c repeated 64 times + * and text is the data being protected */ + + /* start out by storing key in ipad */ + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with ipad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x36; + + /* perform inner SHA256 */ + _addr[0] = k_pad; + _len[0] = 64; + for (i = 0; i < num_elem; i++) { + _addr[i + 1] = addr[i]; + _len[i + 1] = len[i]; + } + sha256_vector(1 + num_elem, _addr, _len, mac); + + os_memset(k_pad, 0, sizeof(k_pad)); + os_memcpy(k_pad, key, key_len); + /* XOR key with opad values */ + for (i = 0; i < 64; i++) + k_pad[i] ^= 0x5c; + + /* perform outer SHA256 */ + _addr[0] = k_pad; + _len[0] = 64; + _addr[1] = mac; + _len[1] = SHA256_MAC_LEN; + sha256_vector(2, _addr, _len, mac); +} + + +/** + * hmac_sha256 - HMAC-SHA256 over data buffer (RFC 2104) + * @key: Key for HMAC operations + * @key_len: Length of the key in bytes + * @data: Pointers to the data area + * @data_len: Length of the data area + * @mac: Buffer for the hash (20 bytes) + */ +void hmac_sha256(const u8 *key, size_t key_len, const u8 *data, + size_t data_len, u8 *mac) +{ + hmac_sha256_vector(key, key_len, 1, &data, &data_len, mac); +} + + +/** + * sha256_prf - SHA256-based Pseudo-Random Function (IEEE 802.11r, 8.5.1.5.2) + * @key: Key for PRF + * @key_len: Length of the key in bytes + * @label: A unique label for each purpose of the PRF + * @data: Extra data to bind into the key + * @data_len: Length of the data + * @buf: Buffer for the generated pseudo-random key + * @buf_len: Number of bytes of key to generate + * + * This function is used to derive new, cryptographically separate keys from a + * given key. + */ +void sha256_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len) +{ + u16 counter = 1; + size_t pos, plen; + u8 hash[SHA256_MAC_LEN]; + const u8 *addr[4]; + size_t len[4]; + u8 counter_le[2], length_le[2]; + + addr[0] = counter_le; + len[0] = 2; + addr[1] = (u8 *) label; + len[1] = os_strlen(label); + addr[2] = data; + len[2] = data_len; + addr[3] = length_le; + len[3] = sizeof(length_le); + + WPA_PUT_LE16(length_le, buf_len * 8); + pos = 0; + while (pos < buf_len) { + plen = buf_len - pos; + WPA_PUT_LE16(counter_le, counter); + if (plen >= SHA256_MAC_LEN) { + hmac_sha256_vector(key, key_len, 4, addr, len, + &buf[pos]); + pos += SHA256_MAC_LEN; + } else { + hmac_sha256_vector(key, key_len, 4, addr, len, hash); + os_memcpy(&buf[pos], hash, plen); + break; + } + counter++; + } +} diff --git a/src/gsm/kdf/sha256.h b/src/gsm/kdf/sha256.h new file mode 100644 index 00000000..b1ce6afe --- /dev/null +++ b/src/gsm/kdf/sha256.h @@ -0,0 +1,30 @@ +/* + * SHA256 hash implementation and interface functions + * Copyright (c) 2003-2011, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + +#ifndef SHA256_H +#define SHA256_H + +#define SHA256_MAC_LEN 32 + +void hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem, + const u8 *addr[], const size_t *len, u8 *mac); +void hmac_sha256(const u8 *key, size_t key_len, const u8 *data, + size_t data_len, u8 *mac); +void sha256_prf(const u8 *key, size_t key_len, const char *label, + const u8 *data, size_t data_len, u8 *buf, size_t buf_len); +void tls_prf_sha256(const u8 *secret, size_t secret_len, + const char *label, const u8 *seed, size_t seed_len, + u8 *out, size_t outlen); + +#endif /* SHA256_H */ diff --git a/src/gsm/kdf/sha256_i.h b/src/gsm/kdf/sha256_i.h new file mode 100644 index 00000000..20ae4889 --- /dev/null +++ b/src/gsm/kdf/sha256_i.h @@ -0,0 +1,31 @@ +/* + * SHA-256 internal definitions + * Copyright (c) 2003-2011, Jouni Malinen <j@w1.fi> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Alternatively, this software may be distributed under the terms of BSD + * license. + * + * See README and COPYING for more details. + */ + +#ifndef SHA256_I_H +#define SHA256_I_H + +#define SHA256_BLOCK_SIZE 64 + +struct sha256_state { + u64 length; + u32 state[8], curlen; + u8 buf[SHA256_BLOCK_SIZE]; +}; + +void sha256_init(struct sha256_state *md); +int sha256_process(struct sha256_state *md, const unsigned char *in, + unsigned long inlen); +int sha256_done(struct sha256_state *md, unsigned char *out); + +#endif /* SHA256_I_H */ diff --git a/src/gsm/lapdm.c b/src/gsm/lapdm.c index 80840293..1fc986d6 100644 --- a/src/gsm/lapdm.c +++ b/src/gsm/lapdm.c @@ -19,10 +19,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup lapdm @@ -31,6 +27,7 @@ #include <stdio.h> #include <stdint.h> +#include <inttypes.h> #include <string.h> #include <errno.h> @@ -61,6 +58,7 @@ #define LAPDm_ADDR_SAPI(addr) (((addr) >> 2) & 0x7) #define LAPDm_ADDR_CR(addr) (((addr) >> 1) & 0x1) #define LAPDm_ADDR_EA(addr) ((addr) & 0x1) +#define LAPDm_ADDR_SHORT_L2(addr) ((addr) & 0x3) /* TS 04.06 Table 3 / Section 3.4.3 */ #define LAPDm_CTRL_I(nr, ns, p) ((((nr) & 0x7) << 5) | (((p) & 0x1) << 4) | (((ns) & 0x7) << 1)) @@ -126,17 +124,20 @@ const struct value_string osmo_ph_prim_names[] = { { 0, NULL } }; +extern void *tall_lapd_ctx; + static int lapdm_send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg); static int send_rslms_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx); static int update_pending_frames(struct lapd_msg_ctx *lctx); static void lapdm_dl_init(struct lapdm_datalink *dl, - struct lapdm_entity *entity, int t200_ms, uint32_t n200) + struct lapdm_entity *entity, int t200_ms, uint32_t n200, + const char *name) { memset(dl, 0, sizeof(*dl)); dl->entity = entity; - lapd_dl_init(&dl->dl, 1, 8, 251); /* Section 5.8.5 of TS 04.06 */ + lapd_dl_init2(&dl->dl, 1, 8, 251, name); /* Section 5.8.5 of TS 04.06 */ dl->dl.reestablish = 0; /* GSM uses no reestablish */ dl->dl.send_ph_data_req = lapdm_send_ph_data_req; dl->dl.send_dlsap = send_rslms_dlsap; @@ -165,7 +166,7 @@ void lapdm_entity_init(struct lapdm_entity *le, enum lapdm_mode mode, int t200) for (i = 0; i < ARRAY_SIZE(t200_ms_sapi_arr); i++) t200_ms_sapi_arr[i] = t200 * 1000; - return lapdm_entity_init2(le, mode, t200_ms_sapi_arr, N200); + return lapdm_entity_init3(le, mode, t200_ms_sapi_arr, N200, NULL); } /*! initialize a LAPDm entity and all datalinks inside @@ -177,10 +178,30 @@ void lapdm_entity_init(struct lapdm_entity *le, enum lapdm_mode mode, int t200) void lapdm_entity_init2(struct lapdm_entity *le, enum lapdm_mode mode, const int *t200_ms, int n200) { + lapdm_entity_init3(le, mode, t200_ms, n200, NULL); +} + +/*! initialize a LAPDm entity and all datalinks inside + * \param[in] le LAPDm entity + * \param[in] mode lapdm_mode (BTS/MS) + * \param[in] t200_ms per-SAPI array of T200 re-transmission timer in milli-seconds + * \param[in] n200 N200 re-transmisison count + * \param[in] name human-readable name (will be copied internally + extended with SAPI) + */ +void lapdm_entity_init3(struct lapdm_entity *le, enum lapdm_mode mode, + const int *t200_ms, int n200, const char *name_pfx) +{ unsigned int i; - for (i = 0; i < ARRAY_SIZE(le->datalink); i++) - lapdm_dl_init(&le->datalink[i], le, t200_ms[i], n200); + for (i = 0; i < ARRAY_SIZE(le->datalink); i++) { + char name[256]; + if (name_pfx) { + snprintf(name, sizeof(name), "%s[%s]", name_pfx, i == 0 ? "0" : "3"); + lapdm_dl_init(&le->datalink[i], le, (t200_ms) ? t200_ms[i] : 0, n200, name); + } else + lapdm_dl_init(&le->datalink[i], le, (t200_ms) ? t200_ms[i] : 0, n200, NULL); + INIT_LLIST_HEAD(&le->datalink[i].tx_ui_queue); + } lapdm_entity_set_mode(le, mode); } @@ -213,7 +234,7 @@ void lapdm_channel_init(struct lapdm_channel *lc, enum lapdm_mode mode) const int t200_ms_dcch[_NR_DL_SAPI] = { 1000, 1000 }; const int t200_ms_acch[_NR_DL_SAPI] = { 2000, 2000 }; - lapdm_channel_init2(lc, mode, t200_ms_dcch, t200_ms_acch, GSM_LCHAN_SDCCH); + lapdm_channel_init3(lc, mode, t200_ms_dcch, t200_ms_acch, GSM_LCHAN_SDCCH, NULL); } /*! initialize a LAPDm channel and all its channels @@ -226,14 +247,42 @@ void lapdm_channel_init(struct lapdm_channel *lc, enum lapdm_mode mode) int lapdm_channel_init2(struct lapdm_channel *lc, enum lapdm_mode mode, const int *t200_ms_dcch, const int *t200_ms_acch, enum gsm_chan_t chan_t) { + return lapdm_channel_init3(lc, mode, t200_ms_dcch, t200_ms_acch, chan_t, NULL); +} + +/*! initialize a LAPDm channel and all its channels + * \param[in] lc \ref lapdm_channel to be initialized + * \param[in] mode \ref lapdm_mode (BTS/MS) + * \param[in] t200_ms_dcch per-SAPI array of T200 in milli-seconds for DCCH + * \param[in] t200_ms_acch per-SAPI array of T200 in milli-seconds for SACCH + * \param[in] chan_t GSM channel type (to correctly set N200) + * \param[in] name_pfx human-readable name (copied by function + extended with ACCH/DCCH) + */ +int lapdm_channel_init3(struct lapdm_channel *lc, enum lapdm_mode mode, + const int *t200_ms_dcch, const int *t200_ms_acch, enum gsm_chan_t chan_t, + const char *name_pfx) +{ int n200_dcch = get_n200_dcch(chan_t); + char namebuf[256]; + char *name = NULL; + if (n200_dcch < 0) return -EINVAL; - lapdm_entity_init2(&lc->lapdm_acch, mode, t200_ms_acch, N200_TR_SACCH); + osmo_talloc_replace_string(tall_lapd_ctx, &lc->name, name_pfx); + + if (name_pfx) { + snprintf(namebuf, sizeof(namebuf), "%s[ACCH]", name_pfx); + name = namebuf; + } + lapdm_entity_init3(&lc->lapdm_acch, mode, t200_ms_acch, N200_TR_SACCH, name); lc->lapdm_acch.lapdm_ch = lc; - lapdm_entity_init2(&lc->lapdm_dcch, mode, t200_ms_dcch, n200_dcch); + if (name_pfx) { + snprintf(namebuf, sizeof(namebuf), "%s[DCCH]", name_pfx); + name = namebuf; + } + lapdm_entity_init3(&lc->lapdm_dcch, mode, t200_ms_dcch, n200_dcch, name); lc->lapdm_dcch.lapdm_ch = lc; return 0; @@ -248,6 +297,7 @@ void lapdm_entity_exit(struct lapdm_entity *le) for (i = 0; i < ARRAY_SIZE(le->datalink); i++) { dl = &le->datalink[i]; lapd_dl_exit(&dl->dl); + msgb_queue_free(&dl->tx_ui_queue); } } @@ -286,8 +336,8 @@ static void lapdm_pad_msgb(struct msgb *msg, uint8_t n201) return; } - data = msgb_put(msg, pad_len); - memset(data, 0x2B, pad_len); + data = msgb_put(msg, pad_len); /* TODO: random padding */ + memset(data, GSM_MACBLOCK_PADDING, pad_len); } /* input function that L2 calls when sending messages up to L3 */ @@ -311,11 +361,23 @@ static int tx_ph_data_enqueue(struct lapdm_datalink *dl, struct msgb *msg, /* if there is a pending message, queue it */ if (le->tx_pending || le->flags & LAPDM_ENT_F_POLLING_ONLY) { + struct msgb *old_msg; + + /* In 'RTS' mode there can be only one message. */ + if (le->flags & LAPDM_ENT_F_RTS) { + /* Overwrite existing message by removing it first. */ + if ((old_msg = msgb_dequeue(&dl->dl.tx_queue))) { + msgb_free(old_msg); + /* Reset V(S) to V(A), because there is no outstanding message now. */ + dl->dl.v_send = dl->dl.v_ack; + } + } + *msgb_push(msg, 1) = pad; *msgb_push(msg, 1) = link_id; *msgb_push(msg, 1) = chan_nr; msgb_enqueue(&dl->dl.tx_queue, msg); - return -EBUSY; + return 0; } osmo_prim_init(&pp.oph, SAP_GSM_PH, PRIM_PH_DATA, @@ -330,7 +392,87 @@ static int tx_ph_data_enqueue(struct lapdm_datalink *dl, struct msgb *msg, return le->l1_prim_cb(&pp.oph, le->l1_ctx); } -static struct msgb *tx_dequeue_msgb(struct lapdm_entity *le) +static int tx_ph_data_enqueue_ui(struct lapdm_datalink *dl, struct msgb *msg, + uint8_t chan_nr, uint8_t link_id, uint8_t pad) +{ + struct lapdm_entity *le = dl->entity; + struct osmo_phsap_prim pp; + + /* if there is a pending message, queue it */ + if (le->tx_pending || le->flags & LAPDM_ENT_F_POLLING_ONLY) { + *msgb_push(msg, 1) = pad; + *msgb_push(msg, 1) = link_id; + *msgb_push(msg, 1) = chan_nr; + msgb_enqueue(&dl->tx_ui_queue, msg); + return 0; + } + + osmo_prim_init(&pp.oph, SAP_GSM_PH, PRIM_PH_DATA, + PRIM_OP_REQUEST, msg); + pp.u.data.chan_nr = chan_nr; + pp.u.data.link_id = link_id; + + /* send the frame now */ + le->tx_pending = 0; /* disabled flow control */ + lapdm_pad_msgb(msg, pad); + + return le->l1_prim_cb(&pp.oph, le->l1_ctx); +} + +/* Get transmit frame from queue, if any. In polling mode, indicate RTS to LAPD and start T200, if pending. */ +static struct msgb *tx_dequeue_msgb(struct lapdm_datalink *dl, uint32_t fn) +{ + struct msgb *msg; + + /* Call RTS function of LAPD, to queue next frame. */ + if (dl->entity->flags & LAPDM_ENT_F_RTS) { + struct lapd_msg_ctx lctx; + int rc; + + /* Poll next frame. */ + lctx.dl = &dl->dl; + rc = lapd_ph_rts_ind(&lctx); + + /* If T200 has been started, calculate timeout FN. */ + if (rc == 1) { + /* Set T200 in advance. */ + dl->t200_timeout = fn; + ADD_MODULO(dl->t200_timeout, dl->t200_fn, GSM_MAX_FN); + + LOGDL(&dl->dl, LOGL_INFO, + "T200 running from FN %"PRIu32" to FN %"PRIu32" (%"PRIu32" frames).\n", + fn, dl->t200_timeout, dl->t200_fn); + } + } + + /* If there is no frame from LAPD, send UI frame, if any. */ + msg = msgb_dequeue(&dl->dl.tx_queue); + if (msg) + LOGDL(&dl->dl, LOGL_INFO, "Sending frame from TX queue. (FN %"PRIu32")\n", fn); + else { + msg = msgb_dequeue(&dl->tx_ui_queue); + if (msg) + LOGDL(&dl->dl, LOGL_INFO, "Sending UI frame from TX queue. (FN %"PRIu32")\n", fn); + } + return msg; +} + +/* Dequeue a Downlink message for DCCH (dedicated channel) */ +static struct msgb *tx_dequeue_dcch_msgb(struct lapdm_entity *le, uint32_t fn) +{ + struct msgb *msg; + + /* SAPI=0 always has higher priority than SAPI=3 */ + msg = tx_dequeue_msgb(&le->datalink[DL_SAPI0], fn); + if (msg == NULL) { /* no SAPI=0 messages, dequeue SAPI=3 (if any) */ + msg = tx_dequeue_msgb(&le->datalink[DL_SAPI3], fn); + } + + return msg; +} + +/* Dequeue a Downlink message for ACCH (associated channel) */ +static struct msgb *tx_dequeue_acch_msgb(struct lapdm_entity *le, uint32_t fn) { struct lapdm_datalink *dl; int last = le->last_tx_dequeue; @@ -342,7 +484,7 @@ static struct msgb *tx_dequeue_msgb(struct lapdm_entity *le) /* next */ i = (i + 1) % n; dl = &le->datalink[i]; - if ((msg = msgb_dequeue(&dl->dl.tx_queue))) + if ((msg = tx_dequeue_msgb(dl, fn))) break; } while (i != last); @@ -356,12 +498,17 @@ static struct msgb *tx_dequeue_msgb(struct lapdm_entity *le) /*! dequeue a msg that's pending transmission via L1 and wrap it into * a osmo_phsap_prim */ -int lapdm_phsap_dequeue_prim(struct lapdm_entity *le, struct osmo_phsap_prim *pp) +int lapdm_phsap_dequeue_prim_fn(struct lapdm_entity *le, struct osmo_phsap_prim *pp, uint32_t fn) { struct msgb *msg; uint8_t pad; - msg = tx_dequeue_msgb(le); + /* Dequeue depending on channel type: DCCH or ACCH. + * See 3GPP TS 44.005, section 4.2.2 "Priority". */ + if (le == &le->lapdm_ch->lapdm_dcch) + msg = tx_dequeue_dcch_msgb(le, fn); + else + msg = tx_dequeue_acch_msgb(le, fn); if (!msg) return -ENODEV; @@ -383,6 +530,57 @@ int lapdm_phsap_dequeue_prim(struct lapdm_entity *le, struct osmo_phsap_prim *pp return 0; } +static void lapdm_t200_fn_dl(struct lapdm_datalink *dl, uint32_t fn) +{ + uint32_t diff; + + OSMO_ASSERT((dl->dl.lapd_flags & LAPD_F_RTS)); + + /* If T200 is running, check if it has fired. */ + if (dl->dl.t200_rts != LAPD_T200_RTS_RUNNING) + return; + + /* Calculate how many frames fn is behind t200_timeout. + * If it is negative (>= GSM_MAX_FN / 2), we have not reached t200_timeout yet. + * If it is 0 or positive, we reached it or we are a bit too late, which is not a problem. + */ + diff = fn; + ADD_MODULO(diff, GSM_MAX_FN - dl->t200_timeout, GSM_MAX_FN); + if (diff >= GSM_MAX_FN / 2) + return; + + LOGDL(&dl->dl, LOGL_INFO, "T200 timeout at FN %"PRIu32", detected at FN %"PRIu32".\n", dl->t200_timeout, fn); + + lapd_t200_timeout(&dl->dl); +} + +/*! Get receive frame number from L1. It is used to check the T200 timeout. + * This function is used if LAPD is in RTS mode only. (Applies if the LAPDM_ENT_F_POLLING_ONLY flag is set.) + * This function must be called for every valid or invalid data frame received. + * The frame number fn must be the frame number of the first burst of a data frame. + * This function must be called after the frame is delivered to layer 2. + * In case of TCH, this this function must be called for every speech frame received, meaning that there was no valid + * data frame. */ +void lapdm_t200_fn(struct lapdm_entity *le, uint32_t fn) +{ + unsigned int i; + + if (!(le->flags & LAPDM_ENT_F_POLLING_ONLY)) { + LOGP(DLLAPD, LOGL_ERROR, "Function call not allowed on timer based T200.\n"); + return; + } + + for (i = 0; i < ARRAY_SIZE(le->datalink); i++) + lapdm_t200_fn_dl(&le->datalink[i], fn); +} + +/*! dequeue a msg that's pending transmission via L1 and wrap it into + * a osmo_phsap_prim */ +int lapdm_phsap_dequeue_prim(struct lapdm_entity *le, struct osmo_phsap_prim *pp) +{ + return lapdm_phsap_dequeue_prim_fn(le, pp, 0); +} + /* get next frame from the tx queue. because the ms has multiple datalinks, * each datalink's queue is read round-robin. */ @@ -478,7 +676,7 @@ static int rsl_rll_error(uint8_t cause, struct lapdm_msg_ctx *mctx) { struct msgb *msg; - LOGP(DLLAPD, LOGL_NOTICE, "sending MDL-ERROR-IND %d\n", cause); + LOGDL(&mctx->dl->dl, LOGL_NOTICE, "sending MDL-ERROR-IND %d\n", cause); msg = rsl_rll_simple(RSL_MT_ERROR_IND, mctx->chan_nr, mctx->link_id, 0); msgb_tlv_put(msg, RSL_IE_RLM_CAUSE, 1, &cause); return rslms_sendmsg(msg, mctx->dl->entity); @@ -523,7 +721,7 @@ static int send_rslms_dlsap(struct osmo_dlsap_prim *dp, } if (!rll_msg) { - LOGP(DLLAPD, LOGL_ERROR, "Unsupported op %d, prim %d. Please " + LOGDL(dl, LOGL_ERROR, "Unsupported op %d, prim %d. Please " "fix!\n", dp->oph.primitive, dp->oph.operation); return -EINVAL; } @@ -592,7 +790,8 @@ static int update_pending_frames(struct lapd_msg_ctx *lctx) LAPDm_CTRL_PF_BIT(msg->l2h[1])); rc = 0; } else if (LAPDm_CTRL_is_S(msg->l2h[1])) { - LOGP(DLLAPD, LOGL_ERROR, "Supervisory frame in queue, this shouldn't happen\n"); + msg->l2h[1] = LAPDm_CTRL_S(dl->v_recv, LAPDm_CTRL_S_BITS(msg->l2h[1]), + LAPDm_CTRL_PF_BIT(msg->l2h[1])); } } @@ -634,7 +833,7 @@ static int lapdm_rx_not_permitted(const struct lapdm_entity *le, /* input into layer2 (from layer 1) */ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, - uint8_t chan_nr, uint8_t link_id) + uint8_t chan_nr, uint8_t link_id, uint32_t fn) { uint8_t cbits = chan_nr >> 3; uint8_t sapi; /* we cannot take SAPI from link_id, as L1 has no clue */ @@ -649,6 +848,7 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, memset(&mctx, 0, sizeof(mctx)); mctx.chan_nr = chan_nr; mctx.link_id = link_id; + mctx.fn = fn; /* check for L1 chan_nr/link_id and determine LAPDm hdr format */ if (cbits == 0x10 || cbits == 0x12) { @@ -660,17 +860,24 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, if (mctx.link_id & 0x40) { /* It was received from network on SACCH */ - /* If UI on SACCH sent by BTS, lapdm_fmt must be B4 */ - if (le->mode == LAPDM_MODE_MS - && LAPDm_CTRL_is_U(msg->l2h[3]) - && LAPDm_CTRL_U_BITS(msg->l2h[3]) == 0) { + /* A Short L3 header has both bits == 0. */ + if (LAPDm_ADDR_SHORT_L2(msg->l2h[2]) == 0) { + mctx.lapdm_fmt = LAPDm_FMT_Bter; + n201 = N201_Bter_SACCH; + sapi = 0; + } else if (le->mode == LAPDM_MODE_MS + && LAPDm_CTRL_is_U(msg->l2h[3]) + && LAPDm_CTRL_U_BITS(msg->l2h[3]) == 0) { + /* If UI on SACCH sent by BTS, lapdm_fmt must be B4 */ mctx.lapdm_fmt = LAPDm_FMT_B4; n201 = N201_B4; - LOGP(DLLAPD, LOGL_INFO, "fmt=B4\n"); + /* sapi is found after two-btyte L1 header */ + sapi = (msg->l2h[2] >> 2) & 7; } else { mctx.lapdm_fmt = LAPDm_FMT_B; n201 = N201_AB_SACCH; - LOGP(DLLAPD, LOGL_INFO, "fmt=B\n"); + /* sapi is found after two-btyte L1 header */ + sapi = (msg->l2h[2] >> 2) & 7; } /* SACCH frames have a two-byte L1 header that * OsmocomBB L1 doesn't strip */ @@ -678,20 +885,24 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, mctx.ta_ind = msg->l2h[1]; msgb_pull(msg, 2); msg->l2h += 2; - sapi = (msg->l2h[0] >> 2) & 7; } else { - mctx.lapdm_fmt = LAPDm_FMT_B; - LOGP(DLLAPD, LOGL_INFO, "fmt=B\n"); - n201 = N201_AB_SDCCH; - sapi = (msg->l2h[0] >> 2) & 7; + /* A Short L3 header has both bits == 0. */ + if (LAPDm_ADDR_SHORT_L2(msg->l2h[0]) == 0) { + mctx.lapdm_fmt = LAPDm_FMT_Bter; + n201 = N201_Bter_SDCCH; + sapi = 0; + } else { + mctx.lapdm_fmt = LAPDm_FMT_B; + n201 = N201_AB_SDCCH; + sapi = (msg->l2h[0] >> 2) & 7; + } } } mctx.dl = lapdm_datalink_for_sapi(le, sapi); /* G.2.1 No action on frames containing an unallocated SAPI. */ if (!mctx.dl) { - LOGP(DLLAPD, LOGL_NOTICE, "Received frame for unsupported " - "SAPI %d!\n", sapi); + LOGP(DLLAPD, LOGL_NOTICE, "Received frame for unsupported SAPI %d!\n", sapi); msgb_free(msg); return -EIO; } @@ -705,8 +916,7 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, mctx.link_id |= LAPDm_ADDR_SAPI(msg->l2h[0]); /* G.2.3 EA bit set to "0" is not allowed in GSM */ if (!LAPDm_ADDR_EA(msg->l2h[0])) { - LOGP(DLLAPD, LOGL_NOTICE, "EA bit 0 is not allowed in " - "GSM\n"); + LOGDL(lctx.dl, LOGL_NOTICE, "EA bit 0 is not allowed in GSM\n"); msgb_free(msg); rsl_rll_error(RLL_CAUSE_FRM_UNIMPL, &mctx); return -EINVAL; @@ -737,8 +947,7 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, /* 5.3.3 UI frames with invalid SAPI values shall be * discarded */ - LOGP(DLLAPD, LOGL_INFO, "sapi=%u (discarding)\n", - lctx.sapi); + LOGDL(lctx.dl, LOGL_INFO, "sapi=%u (discarding)\n", lctx.sapi); msgb_free(msg); return 0; } @@ -755,8 +964,7 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, * MDL-ERROR-INDICATION primitive with cause * "frame not implemented" is sent to the * mobile management entity. */ - LOGP(DLLAPD, LOGL_NOTICE, "we don't support " - "multi-octet length\n"); + LOGDL(lctx.dl, LOGL_NOTICE, "we don't support multi-octet length\n"); msgb_free(msg); rsl_rll_error(RLL_CAUSE_FRM_UNIMPL, &mctx); return -EINVAL; @@ -771,21 +979,21 @@ static int l2_ph_data_ind(struct msgb *msg, struct lapdm_entity *le, memcpy(&mctx.dl->mctx, &mctx, sizeof(mctx.dl->mctx)); rc =lapdm_rx_not_permitted(le, &lctx); if (rc > 0) { - LOGP(DLLAPD, LOGL_NOTICE, "received message not permitted\n"); + LOGDL(lctx.dl, LOGL_NOTICE, "received message not permitted\n"); msgb_free(msg); rsl_rll_error(rc, &mctx); return -EINVAL; } /* send to LAPD */ + LOGDL(lctx.dl, LOGL_DEBUG, "Frame received at FN %"PRIu32".\n", fn); rc = lapd_ph_data_ind(msg, &lctx); break; case LAPDm_FMT_Bter: - /* FIXME */ - msgb_free(msg); - break; + /* fall-through */ case LAPDm_FMT_Bbis: + /* Update context so that users can read fields like fn: */ + memcpy(&mctx.dl->mctx, &mctx, sizeof(mctx.dl->mctx)); /* directly pass up to layer3 */ - LOGP(DLLAPD, LOGL_INFO, "fmt=Bbis UI\n"); msg->l3h = msg->l2h; msgb_pull_to_l3(msg); rc = send_rslms_rll_l3(RSL_MT_UNIT_DATA_IND, &mctx, msg); @@ -838,56 +1046,33 @@ int lapdm_phsap_up(struct osmo_prim_hdr *oph, struct lapdm_entity *le) if (oph->sap != SAP_GSM_PH) { LOGP(DLLAPD, LOGL_ERROR, "primitive for unknown SAP %u\n", oph->sap); - rc = -ENODEV; - goto free; + msgb_free(oph->msg); + return -ENODEV; } - switch (oph->primitive) { - case PRIM_PH_DATA: - if (oph->operation != PRIM_OP_INDICATION) { - LOGP(DLLAPD, LOGL_ERROR, "PH_DATA is not INDICATION %u\n", - oph->operation); - rc = -ENODEV; - goto free; - } + switch (OSMO_PRIM_HDR(oph)) { + case OSMO_PRIM(PRIM_PH_DATA, PRIM_OP_INDICATION): rc = l2_ph_data_ind(oph->msg, le, pp->u.data.chan_nr, - pp->u.data.link_id); + pp->u.data.link_id, pp->u.data.fn); break; - case PRIM_PH_RTS: - if (oph->operation != PRIM_OP_INDICATION) { - LOGP(DLLAPD, LOGL_ERROR, "PH_RTS is not INDICATION %u\n", - oph->operation); - rc = -ENODEV; - goto free; - } + case OSMO_PRIM(PRIM_PH_RTS, PRIM_OP_INDICATION): rc = l2_ph_data_conf(oph->msg, le); break; - case PRIM_PH_RACH: - switch (oph->operation) { - case PRIM_OP_INDICATION: - rc = l2_ph_rach_ind(le, pp->u.rach_ind.ra, pp->u.rach_ind.fn, - pp->u.rach_ind.acc_delay); - break; - case PRIM_OP_CONFIRM: - rc = l2_ph_chan_conf(oph->msg, le, pp->u.rach_ind.fn); - break; - default: - rc = -EIO; - goto free; - } + case OSMO_PRIM(PRIM_PH_RACH, PRIM_OP_INDICATION): + rc = l2_ph_rach_ind(le, pp->u.rach_ind.ra, pp->u.rach_ind.fn, + pp->u.rach_ind.acc_delay); + break; + case OSMO_PRIM(PRIM_PH_RACH, PRIM_OP_CONFIRM): + rc = l2_ph_chan_conf(oph->msg, le, pp->u.rach_ind.fn); break; default: LOGP(DLLAPD, LOGL_ERROR, "Unknown primitive %u\n", oph->primitive); - rc = -EINVAL; - goto free; + msgb_free(oph->msg); + return -EINVAL; } return rc; - -free: - msgb_free(oph->msg); - return rc; } @@ -930,7 +1115,7 @@ static int rslms_rx_rll_est_req(struct msgb *msg, struct lapdm_datalink *dl) if (sapi != 0) { /* According to clause 6, the contention resolution * procedure is only permitted with SAPI value 0 */ - LOGP(DLLAPD, LOGL_ERROR, "SAPI != 0 but contention" + LOGDL(&dl->dl, LOGL_ERROR, "SAPI != 0 but contention" "resolution (discarding)\n"); msgb_free(msg); return send_rll_simple(RSL_MT_REL_IND, &dl->mctx); @@ -946,7 +1131,7 @@ static int rslms_rx_rll_est_req(struct msgb *msg, struct lapdm_datalink *dl) /* check if the layer3 message length exceeds N201 */ if (length > n201) { - LOGP(DLLAPD, LOGL_ERROR, "frame too large: %d > N201(%d) " + LOGDL(&dl->dl, LOGL_ERROR, "frame too large: %d > N201(%d) " "(discarding)\n", length, n201); msgb_free(msg); return send_rll_simple(RSL_MT_REL_IND, &dl->mctx); @@ -973,9 +1158,10 @@ static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl) uint8_t sapi = link_id & 7; struct tlv_parsed tv; int length, ui_bts; + bool use_b_ter; if (!le) { - LOGP(DLLAPD, LOGL_ERROR, "lapdm_datalink without entity error\n"); + LOGDL(&dl->dl, LOGL_ERROR, "lapdm_datalink without entity error\n"); msgb_free(msg); return -EMLINK; } @@ -992,35 +1178,38 @@ static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl) le->tx_power = *TLVP_VAL(&tv, RSL_IE_MS_POWER); } if (!TLVP_PRESENT(&tv, RSL_IE_L3_INFO)) { - LOGP(DLLAPD, LOGL_ERROR, "unit data request without message " - "error\n"); + LOGDL(&dl->dl, LOGL_ERROR, "unit data request without message error\n"); msgb_free(msg); return -EINVAL; } msg->l3h = (uint8_t *) TLVP_VAL(&tv, RSL_IE_L3_INFO); length = TLVP_LEN(&tv, RSL_IE_L3_INFO); + /* check for Bter frame */ + use_b_ter = (length == ((link_id & 0x40) ? 21 : 23) && sapi == 0); /* check if the layer3 message length exceeds N201 */ - if (length + ((link_id & 0x40) ? 4 : 2) + !ui_bts > 23) { - LOGP(DLLAPD, LOGL_ERROR, "frame too large: %d > N201(%d) " + if (length + ((link_id & 0x40) ? 4 : 2) + !ui_bts > 23 && !use_b_ter) { + LOGDL(&dl->dl, LOGL_ERROR, "frame too large: %d > N201(%d) " "(discarding)\n", length, ((link_id & 0x40) ? 18 : 20) + ui_bts); msgb_free(msg); return -EIO; } - LOGP(DLLAPD, LOGL_INFO, "sending unit data (tx_power=%d, ta=%d)\n", - le->tx_power, le->ta); + LOGDL(&dl->dl, LOGL_INFO, "sending unit data (tx_power=%d, ta=%d)\n", le->tx_power, le->ta); /* Remove RLL header from msgb and set length to L3-info */ msgb_pull_to_l3(msg); msgb_trim(msg, length); /* Push L1 + LAPDm header on msgb */ - msg->l2h = msgb_push(msg, 2 + !ui_bts); - msg->l2h[0] = LAPDm_ADDR(LAPDm_LPD_NORMAL, sapi, dl->dl.cr.loc2rem.cmd); - msg->l2h[1] = LAPDm_CTRL_U(LAPDm_U_UI, 0); - if (!ui_bts) - msg->l2h[2] = LAPDm_LEN(length); + if (!use_b_ter) { + msg->l2h = msgb_push(msg, 2 + !ui_bts); + msg->l2h[0] = LAPDm_ADDR(LAPDm_LPD_NORMAL, sapi, dl->dl.cr.loc2rem.cmd); + msg->l2h[1] = LAPDm_CTRL_U(LAPDm_U_UI, 0); + if (!ui_bts) + msg->l2h[2] = LAPDm_LEN(length); + } else + msg->l2h = msg->data; if (link_id & 0x40) { msg->l2h = msgb_push(msg, 2); msg->l2h[0] = le->tx_power; @@ -1028,7 +1217,7 @@ static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl) } /* Tramsmit */ - return tx_ph_data_enqueue(dl, msg, chan_nr, link_id, 23); + return tx_ph_data_enqueue_ui(dl, msg, chan_nr, link_id, 23); } /* L3 requests transfer of acknowledged information */ @@ -1041,8 +1230,7 @@ static int rslms_rx_rll_data_req(struct msgb *msg, struct lapdm_datalink *dl) rsl_tlv_parse(&tv, rllh->data, msgb_l2len(msg)-sizeof(*rllh)); if (!TLVP_PRESENT(&tv, RSL_IE_L3_INFO)) { - LOGP(DLLAPD, LOGL_ERROR, "data request without message " - "error\n"); + LOGDL(&dl->dl, LOGL_ERROR, "data request without message error\n"); msgb_free(msg); return -EINVAL; } @@ -1068,7 +1256,7 @@ static int rslms_rx_rll_susp_req(struct msgb *msg, struct lapdm_datalink *dl) struct osmo_dlsap_prim dp; if (sapi != 0) { - LOGP(DLLAPD, LOGL_ERROR, "SAPI != 0 while suspending\n"); + LOGDL(&dl->dl, LOGL_ERROR, "SAPI != 0 while suspending\n"); msgb_free(msg); return -EINVAL; } @@ -1098,7 +1286,7 @@ static int rslms_rx_rll_res_req(struct msgb *msg, struct lapdm_datalink *dl) rsl_tlv_parse(&tv, rllh->data, msgb_l2len(msg)-sizeof(*rllh)); if (!TLVP_PRESENT(&tv, RSL_IE_L3_INFO)) { - LOGP(DLLAPD, LOGL_ERROR, "resume without message error\n"); + LOGDL(&dl->dl, LOGL_ERROR, "resume without message error\n"); msgb_free(msg); return send_rll_simple(RSL_MT_REL_IND, &dl->mctx); } @@ -1235,7 +1423,7 @@ static int rslms_rx_rll(struct msgb *msg, struct lapdm_channel *lc) case RSL_MT_SUSP_REQ: case RSL_MT_RES_REQ: case RSL_MT_RECON_REQ: - LOGP(DLLAPD, LOGL_NOTICE, "(%p) RLL Message '%s' unsupported in BTS side LAPDm\n", + LOGP(DLLAPD, LOGL_NOTICE, "(%s) RLL Message '%s' unsupported in BTS side LAPDm\n", lc->name, rsl_msg_name(msg_type)); msgb_free(msg); return -EINVAL; @@ -1262,14 +1450,14 @@ static int rslms_rx_rll(struct msgb *msg, struct lapdm_channel *lc) /* This is triggered in abnormal error conditions where * set_lapdm_context() was not called for the channel earlier. */ if (!dl->dl.lctx.dl) { - LOGP(DLLAPD, LOGL_NOTICE, "(%p) RLL Message '%s' received without LAPDm context. (sapi %d)\n", + LOGP(DLLAPD, LOGL_NOTICE, "(%s) RLL Message '%s' received without LAPDm context. (sapi %d)\n", lc->name, rsl_msg_name(msg_type), sapi); msgb_free(msg); return -EINVAL; } break; default: - LOGP(DLLAPD, LOGL_INFO, "(%p) RLL Message '%s' received. (sapi %d)\n", + LOGP(DLLAPD, LOGL_INFO, "(%s) RLL Message '%s' received. (sapi %d)\n", lc->name, rsl_msg_name(msg_type), sapi); } @@ -1426,6 +1614,7 @@ void lapdm_entity_reset(struct lapdm_entity *le) for (i = 0; i < ARRAY_SIZE(le->datalink); i++) { dl = &le->datalink[i]; lapd_dl_reset(&dl->dl); + msgb_queue_free(&dl->tx_ui_queue); } } @@ -1439,7 +1628,22 @@ void lapdm_channel_reset(struct lapdm_channel *lc) /*! Set the flags of a LAPDm entity */ void lapdm_entity_set_flags(struct lapdm_entity *le, unsigned int flags) { + unsigned int dl_flags = 0; + struct lapdm_datalink *dl; + int i; + le->flags = flags; + + /* Set flags at LAPD. */ + if (le->flags & LAPDM_ENT_F_RTS) + dl_flags |= LAPD_F_RTS; + if (le->flags & LAPDM_ENT_F_DROP_2ND_REJ) + dl_flags |= LAPD_F_DROP_2ND_REJ; + + for (i = 0; i < ARRAY_SIZE(le->datalink); i++) { + dl = &le->datalink[i]; + lapd_dl_set_flags(&dl->dl, dl_flags); + } } /*! Set the flags of all LAPDm entities in a LAPDm channel */ @@ -1449,4 +1653,25 @@ void lapdm_channel_set_flags(struct lapdm_channel *lc, unsigned int flags) lapdm_entity_set_flags(&lc->lapdm_acch, flags); } +/*! Set the T200 FN timer of a LAPDm entity + * \param[in] \ref lapdm_entity + * \param[in] t200_fn Array of T200 timeout in frame numbers for all SAPIs (0, 3) */ +void lapdm_entity_set_t200_fn(struct lapdm_entity *le, const uint32_t *t200_fn) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(le->datalink); i++) + le->datalink[i].t200_fn = t200_fn[i]; +} + +/*! Set the T200 FN timer of all LAPDm entities in a LAPDm channel + * \param[in] \ref lapdm_channel + * \param[in] t200_fn_dcch Array of T200 timeout in frame numbers for all SAPIs (0, 3) on SDCCH/FACCH + * \param[in] t200_fn_acch Array of T200 timeout in frame numbers for all SAPIs (0, 3) on SACCH */ +void lapdm_channel_set_t200_fn(struct lapdm_channel *lc, const uint32_t *t200_fn_dcch, const uint32_t *t200_fn_acch) +{ + lapdm_entity_set_t200_fn(&lc->lapdm_dcch, t200_fn_dcch); + lapdm_entity_set_t200_fn(&lc->lapdm_acch, t200_fn_acch); +} + /*! @} */ diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map index a0e3b324..2c4c621c 100644 --- a/src/gsm/libosmogsm.map +++ b/src/gsm/libosmogsm.map @@ -40,6 +40,14 @@ abis_nm_put_sw_file; abis_nm_get_sw_conf; abis_nm_get_sw_desc_len; +abis_nm_ipacc_freq_band_desc; +abis_nm_ipacc_ciph_algo_desc; +abis_nm_ipacc_chant_desc; +abis_nm_ipacc_chanm_desc; +abis_nm_ipacc_gprs_coding_desc; +abis_nm_ipacc_rtp_feat_desc; +abis_nm_ipacc_rsl_feat_desc; + osmo_sitype_strs; osmo_c4; osmo_get_rand_id; @@ -86,6 +94,8 @@ gprs_service_t_strs; gsm0341_build_msg; +gsm0406_dlci_sapi_names; + gsm0480_create_notifySS; gsm0480_create_unstructuredSS_Notify; gsm0480_create_ussd_resp; @@ -108,6 +118,8 @@ gsm0480_gen_reject; gsm0502_calc_paging_group; gsm0502_fn_remap; +gsm0502_hop_seq_gen; +gsm0502_fn2ccch_block; gsm0503_xcch; gsm0503_rach; @@ -118,6 +130,11 @@ gsm0503_cs3; gsm0503_cs2_np; gsm0503_cs3_np; gsm0503_tch_fr; +gsm0503_tch_f24; +gsm0503_tch_h24; +gsm0503_tch_f48; +gsm0503_tch_f96; +gsm0503_tch_f144; gsm0503_tch_hr; gsm0503_tch_afs_12_2; gsm0503_tch_afs_10_2; @@ -133,6 +150,7 @@ gsm0503_tch_ahs_6_7; gsm0503_tch_ahs_5_9; gsm0503_tch_ahs_5_15; gsm0503_tch_ahs_4_75; +gsm0503_tch_axs_sid_update; gsm0503_mcs1_dl_hdr; gsm0503_mcs1_ul_hdr; gsm0503_mcs1; @@ -150,10 +168,14 @@ gsm0503_mcs8; gsm0503_mcs9; gsm0808_att_tlvdef; +gsm0808_old_bss_to_new_bss_info_att_tlvdef; gsm0808_bssap_name; gsm0808_bssmap_name; gsm0808_cause_name; gsm0808_cause_class_name; +gsm0808_get_cause; +gsm0808_diagnostics_octet_location_str; +gsm0808_diagnostics_bit_location_str; gsm0808_create_ass; gsm0808_create_ass2; gsm0808_create_assignment_completed; @@ -162,6 +184,7 @@ gsm0808_create_ass_compl2; gsm0808_create_assignment_failure; gsm0808_create_ass_fail; gsm0808_create_cipher; +gsm0808_create_cipher2; gsm0808_create_cipher_complete; gsm0808_create_cipher_reject; gsm0808_create_cipher_reject_ext; @@ -183,6 +206,7 @@ gsm0808_create_lcls_conn_ctrl_ack; gsm0808_create_lcls_notification; gsm0808_create_reset; gsm0808_create_reset_ack; +gsm0808_create_sapi_reject_cause; gsm0808_create_sapi_reject; gsm0808_create_handover_required; gsm0808_create_handover_required_reject; @@ -195,23 +219,64 @@ gsm0808_create_handover_succeeded; gsm0808_create_handover_complete; gsm0808_create_handover_failure; gsm0808_create_handover_performed; +gsm0808_create_vgcs_vbs_setup; +gsm0808_create_vgcs_vbs_setup_ack; +gsm0808_create_vgcs_vbs_setup_refuse; +gsm0808_create_vgcs_vbs_assign_req; +gsm0808_create_vgcs_vbs_assign_res; +gsm0808_create_vgcs_vbs_assign_fail; +gsm0808_create_uplink_request; +gsm0808_create_uplink_request_ack; +gsm0808_create_uplink_request_cnf; +gsm0808_create_uplink_app_data; +gsm0808_create_uplink_release_ind; +gsm0808_create_uplink_reject_cmd; +gsm0808_create_uplink_release_cmd; +gsm0808_create_uplink_seized_cmd; +gsm0808_create_vgcs_additional_info; +gsm0808_create_vgcs_vbs_area_cell_info; +gsm0808_create_vgcs_vbs_assign_stat; +gsm0808_create_vgcs_sms; +gsm0808_create_notification_data; +gsm0808_create_common_id; gsm0808_prepend_dtap_header; gsm0808_enc_cause; gsm0808_enc_aoip_trasp_addr; gsm0808_dec_aoip_trasp_addr; gsm0808_dec_osmux_cid; gsm0808_enc_speech_codec; +gsm0808_enc_speech_codec2; gsm0808_dec_speech_codec; gsm0808_enc_speech_codec_list; +gsm0808_enc_speech_codec_list2; gsm0808_dec_speech_codec_list; gsm0808_enc_channel_type; gsm0808_dec_channel_type; gsm0808_enc_encrypt_info; gsm0808_dec_encrypt_info; +gsm0808_enc_kc128; +gsm0808_dec_kc128; gsm0808_enc_cell_id_list; gsm0808_enc_cell_id_list2; gsm0808_dec_cell_id_list; gsm0808_dec_cell_id_list2; +gsm0808_enc_group_callref; +gsm0808_dec_group_callref; +gsm0808_enc_priority; +gsm0808_dec_priority; +gsm0808_enc_vgcs_feature_flags; +gsm0808_dec_vgcs_feature_flags; +gsm0808_enc_data_identity; +gsm0808_dec_data_identity; +gsm0808_enc_msisdn; +gsm0808_dec_msisdn; +gsm0808_enc_talker_identity; +gsm0808_dec_talker_identity; +gsm0808_enc_assign_req; +gsm0808_dec_assign_req; +gsm0808_enc_cell_id_list_segment; +gsm0808_dec_cell_id_list_segment; +gsm0808_dec_call_id; gsm0808_cell_id_list_add; gsm0808_cell_id_to_list; gsm0808_cell_id_to_cgi; @@ -233,6 +298,8 @@ gsm0808_chan_type_to_speech_codec; gsm0808_speech_codec_from_chan_type; gsm0808_sc_cfg_from_gsm48_mr_cfg; gsm48_mr_cfg_from_gsm0808_sc_cfg; +gsm0808_amr_modes_from_cfg; +gsm0808_amr_mode_names; gsm0808_speech_codec_type_names; gsm0808_permitted_speech_names; gsm0808_chosen_enc_alg_names; @@ -246,6 +313,9 @@ gsm0808_enc_lcls; gsm0808_dec_lcls; gsm0808_msgb_put_cell_id_u; gsm0808_decode_cell_id_u; +gsm0808_create_perform_location_request; +gsm0808_create_perform_location_response; +gsm0808_create_perform_location_abort; gsm29118_msgb_alloc; gsm29118_create_alert_req; @@ -308,11 +378,17 @@ osmo_gsm48_rest_octets_si2quater_encode; osmo_gsm48_rest_octets_si6_encode; osmo_gsm48_rest_octets_si3_encode; osmo_gsm48_rest_octets_si4_encode; +osmo_gsm48_rest_octets_si13_decode; osmo_gsm48_rest_octets_si13_encode; osmo_gsm48_rest_octets_si3_decode; +osmo_gsm48_rest_octets_si4_decode; +osmo_gsm48_si1ro_nch_pos_encode; +osmo_gsm48_si1ro_nch_pos_decode; +gsm48_rr_short_pd_msg_name; gsm48_rr_msg_name; gsm48_cc_state_name; gsm48_construct_ra; +gsm48_ra_equal; gsm48_encode_ra; gsm48_hdr_gmm_cipherable; gsm48_decode_bcd_number; @@ -326,6 +402,7 @@ gsm48_decode_cccap; gsm48_decode_connected; gsm48_decode_facility; gsm48_decode_freq_list; +gsm48_decode_classmark3; gsm48_decode_keypad; gsm48_decode_lai; gsm48_decode_notify; @@ -356,6 +433,20 @@ gsm48_generate_mid; gsm48_generate_mid_from_imsi; gsm48_generate_mid_from_tmsi; gsm48_mi_to_string; +gsm48_chan_mode_to_vamos; +gsm48_chan_mode_to_non_vamos; +osmo_mobile_identity_to_str_buf; +osmo_mobile_identity_to_str_c; +osmo_mobile_identity_cmp; +osmo_mobile_identity_decode; +osmo_mobile_identity_decode_from_l3_buf; +osmo_mobile_identity_decode_from_l3; +osmo_mobile_identity_encoded_len; +osmo_mobile_identity_encode_buf; +osmo_mobile_identity_encode_msgb; +osmo_routing_area_id_decode; +osmo_routing_area_id_encode_buf; +osmo_routing_area_id_encode_msgb; gsm48_mm_att_tlvdef; gsm48_number_of_paging_subchannels; gsm48_parse_ra; @@ -372,6 +463,7 @@ gsm48_generate_lai2; gsm48_decode_lai2; osmo_bts_features_descs; osmo_bts_feature_name; +osmo_bts_features_names; osmo_plmn_to_bcd; osmo_plmn_from_bcd; osmo_mcc_name; @@ -384,16 +476,27 @@ osmo_plmn_name; osmo_plmn_name_buf; osmo_plmn_name_c; osmo_plmn_name2; +osmo_lai_cmp; osmo_lai_name; osmo_lai_name_buf; osmo_lai_name_c; +osmo_rai_cmp; osmo_rai_name; osmo_rai_name_buf; osmo_rai_name_c; +osmo_rai_name2; +osmo_rai_name2_buf; +osmo_rai_name2_c; +osmo_cgi_cmp; osmo_cgi_name; osmo_cgi_name_buf; osmo_cgi_name_c; osmo_cgi_name2; +osmo_cgi_ps_cmp; +osmo_cgi_ps_name; +osmo_cgi_ps_name_buf; +osmo_cgi_ps_name_c; +osmo_cgi_ps_name2; osmo_gummei_name; osmo_gummei_name_buf; osmo_gummei_name_c; @@ -417,6 +520,13 @@ gsm48_pdisc_msgtype_name_buf; gsm48_pdisc_msgtype_name_c; gsm48_reject_value_names; +osmo_gsm44068_msg_type_names; +osmo_gsm44068_priority_level_names; +osmo_gsm44068_cause_names; +osmo_gsm44068_call_state_names; +osmo_gsm44068_talker_priority_names; +osmo_gsm44068_att_tlvdef; + gsm_7bit_decode; gsm_7bit_decode_ussd; gsm_7bit_encode; @@ -442,13 +552,17 @@ gsm_gsmtime2fn; osmo_dump_gsmtime; osmo_dump_gsmtime_buf; osmo_dump_gsmtime_c; +gsm_rfn2fn; gsm_milenage; gsm_septet_encode; +gsm_septet_pack; gsm_septets2octets; lapd_dl_exit; lapd_dl_init; +lapd_dl_init2; +lapd_dl_set_name; lapd_dl_reset; lapd_msgb_alloc; lapd_ph_data_ind; @@ -459,6 +573,7 @@ lapd_state_names; lapdm_channel_exit; lapdm_channel_init; lapdm_channel_init2; +lapdm_channel_init3; lapdm_channel_reset; lapdm_channel_set_flags; lapdm_channel_set_l1; @@ -468,10 +583,15 @@ lapdm_datalink_for_sapi; lapdm_entity_exit; lapdm_entity_init; lapdm_entity_init2; +lapdm_entity_init3; lapdm_entity_reset; lapdm_entity_set_flags; lapdm_entity_set_mode; +lapdm_entity_set_t200_fn; +lapdm_channel_set_t200_fn; lapdm_phsap_dequeue_prim; +lapdm_phsap_dequeue_prim_fn; +lapdm_t200_fn; lapdm_phsap_up; lapdm_rslms_recvmsg; @@ -495,14 +615,23 @@ osmo_a5_2; osmo_auth_alg_name; osmo_auth_alg_parse; osmo_auth_gen_vec; +osmo_auth_gen_vec2; osmo_auth_gen_vec_auts; +osmo_auth_gen_vec_auts2; osmo_auth_3g_from_2g; osmo_auth_load; osmo_auth_register; osmo_auth_supported; +osmo_auth_c2; osmo_auth_c3; osmo_sub_auth_type_names; +osmo_kdf_kc128; +osmo_kdf_kasme; +osmo_kdf_enb; +osmo_kdf_nh; +osmo_kdf_nas; + osmo_rsl2sitype; osmo_sitype2rsl; @@ -554,6 +683,11 @@ osmo_shift_tlv; osmo_match_shift_tlv; osmo_shift_lv; +osmo_tlv_prot_msg_name; +osmo_tlv_prot_ie_name; +osmo_tlv_prot_validate_tp; +osmo_tlv_prot_parse; + gan_msgt_vals; gan_pdisc_vals; @@ -659,6 +793,84 @@ osmo_cbsp_encode; osmo_cbsp_decode; osmo_cbsp_recv_buffered; osmo_cbsp_errstr; +osmo_cbsp_segmentation_cb; + +osmo_i460_demux_in; +osmo_i460_mux_enqueue; +osmo_i460_mux_out; +osmo_i460_subchan_add; +osmo_i460_subchan_del; +osmo_i460_ts_init; + +osmo_nri_v_validate; +osmo_nri_v_matches_ranges; +osmo_nri_v_limit_by_ranges; +osmo_tmsi_nri_v_get; +osmo_tmsi_nri_v_set; +osmo_tmsi_nri_v_limit_by_ranges; +osmo_nri_range_validate; +osmo_nri_range_overlaps_ranges; +osmo_nri_ranges_alloc; +osmo_nri_ranges_free; +osmo_nri_ranges_add; +osmo_nri_ranges_del; +osmo_nri_ranges_vty_add; +osmo_nri_ranges_vty_del; +osmo_nri_ranges_to_str_buf; +osmo_nri_ranges_to_str_c; + +osmo_bssmap_le_msgt_names; +osmo_bssap_le_enc; +osmo_bssap_le_dec; +osmo_lcs_cause_enc; +osmo_lcs_cause_dec; +osmo_bssmap_le_ie_enc_location_type; +osmo_bssmap_le_ie_dec_location_type; +osmo_bssmap_le_msgt; +osmo_bssap_le_pdu_to_str_buf; +osmo_bssap_le_pdu_to_str_c; + +osmo_bsslap_enc; +osmo_bsslap_dec; +osmo_bsslap_msgt_names; + +osmo_gad_enc; +osmo_gad_dec; +osmo_gad_to_str_buf; +osmo_gad_to_str_c; +osmo_gad_enc_lat; +osmo_gad_dec_lat; +osmo_gad_enc_lon; +osmo_gad_dec_lon; +osmo_gad_enc_unc; +osmo_gad_dec_unc; +osmo_gad_raw_read; +osmo_gad_raw_write; +osmo_gad_type_names; + +osmo_iuup_compute_header_crc; +osmo_iuup_compute_payload_crc; +osmo_iuup_instance_alloc; +osmo_iuup_instance_free; +osmo_iuup_instance_set_user_prim_cb; +osmo_iuup_instance_set_transport_prim_cb; +osmo_iuup_tnl_prim_up; +osmo_iuup_rnl_prim_down; +osmo_iuup_rnl_prim_alloc; +osmo_iuup_tnl_prim_alloc; + +osmo_csd_12k_6k_decode_frame; +osmo_csd_12k_6k_encode_frame; +osmo_csd_3k6_decode_frame; +osmo_csd_3k6_encode_frame; +osmo_csd_ubit_dump; + +osmo_rlp_decode; +osmo_rlp_encode; +osmo_rlp_fcs_compute; +osmo_rlp_ftype_s_vals; +osmo_rlp_ftype_u_vals; +osmo_rlp_ftype_vals; local: *; }; diff --git a/src/gsm/milenage/milenage.c b/src/gsm/milenage/milenage.c index 3c14ab96..ba9ab33b 100644 --- a/src/gsm/milenage/milenage.c +++ b/src/gsm/milenage/milenage.c @@ -244,19 +244,13 @@ int milenage_auts(const u8 *opc, const u8 *k, const u8 *_rand, const u8 *auts, int gsm_milenage(const u8 *opc, const u8 *k, const u8 *_rand, u8 *sres, u8 *kc) { u8 res[8], ck[16], ik[16]; - int i; if (milenage_f2345(opc, k, _rand, res, ck, ik, NULL, NULL)) return -1; osmo_auth_c3(kc, ck, ik); + osmo_auth_c2(sres, res, sizeof(res), 1); -#ifdef GSM_MILENAGE_ALT_SRES - os_memcpy(sres, res, 4); -#else /* GSM_MILENAGE_ALT_SRES */ - for (i = 0; i < 4; i++) - sres[i] = res[i] ^ res[i + 4]; -#endif /* GSM_MILENAGE_ALT_SRES */ return 0; } diff --git a/src/gsm/mncc.c b/src/gsm/mncc.c index 938cf9a6..8a7dc4d6 100644 --- a/src/gsm/mncc.c +++ b/src/gsm/mncc.c @@ -20,7 +20,7 @@ * */ -#include "../config.h" +#include "config.h" #ifdef HAVE_SYS_SOCKET_H diff --git a/src/gsm/rlp.c b/src/gsm/rlp.c new file mode 100644 index 00000000..1e90a689 --- /dev/null +++ b/src/gsm/rlp.c @@ -0,0 +1,243 @@ +/* + * Copyright (C) 2022-2023 Harald Welte <laforge@osmocom.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +/*! \addtogroup rlp + * @{ + * RLP (Radio Link Protocol) as per 3GPP TS 24.022 + * + */ + +#include <stdint.h> +#include <stddef.h> +#include <string.h> +#include <errno.h> + +#include <osmocom/gsm/rlp.h> + +const struct value_string osmo_rlp_ftype_vals[] = { + { OSMO_RLP_FT_U, "U" }, + { OSMO_RLP_FT_S, "S" }, + { OSMO_RLP_FT_IS, "IS" }, + { 0, NULL } +}; + +const struct value_string osmo_rlp_ftype_u_vals[] = { + { OSMO_RLP_U_FT_SABM, "SABM" }, + { OSMO_RLP_U_FT_UA, "UA" }, + { OSMO_RLP_U_FT_DISC, "DISC" }, + { OSMO_RLP_U_FT_DM, "DM" }, + { OSMO_RLP_U_FT_NULL, "NULL" }, + { OSMO_RLP_U_FT_UI, "UI" }, + { OSMO_RLP_U_FT_XID, "XID" }, + { OSMO_RLP_U_FT_TEST, "TEST" }, + { OSMO_RLP_U_FT_REMAP, "REMAP" }, + { 0, NULL } +}; + +const struct value_string osmo_rlp_ftype_s_vals[] = { + { OSMO_RLP_S_FT_RR, "RR" }, + { OSMO_RLP_S_FT_REJ, "REJ" }, + { OSMO_RLP_S_FT_RNR, "RNR" }, + { OSMO_RLP_S_FT_SREJ, "SREJ" }, + { 0, NULL } +}; + +/* number of bytes used up by FCS */ +#define FCS_SIZE_BYTES 3 + +/*! decode a RLP frame into its abstract representation. Doesn't check FCS correctness. + * \param[out] out caller-allocated memory for output of decoded frame + * \param[in] version RLP version number to use when decoding + * \param[in] data raw RLP frame input data + * \param[in] data_len length of data (in octets); must be 30 (240bit) or 72 (576bit) + * \returns 0 in case of success; negative on error */ +int osmo_rlp_decode(struct osmo_rlp_frame_decoded *out, uint8_t version, const uint8_t *data, size_t data_len) +{ + const uint8_t hdr_len = 2; /* will become a variable when we introduce v2 support */ + uint8_t n_s, n_r; + + if (data_len != 240/8 && data_len != 576/8) + return -EINVAL; + + /* we only support version 0+1 so far */ + if (version >= 2) + return -ENOTSUP; + + memset(out, 0, sizeof(*out)); + out->version = version; + + out->c_r = data[0] & 1; + n_s = (data[0] >> 3) | (data[1] & 1) << 5; + n_r = (data[1] >> 2); + out->fcs = (data[data_len-1] << 16) | (data[data_len-2]) << 8 | (data[data_len-3] << 0); + out->p_f = (data[1] >> 1) & 1; + + switch (n_s) { + case 0x3f: + out->ftype = OSMO_RLP_FT_U; + out->u_ftype = n_r & 0x1f; + if (out->u_ftype == OSMO_RLP_U_FT_XID) { + memcpy(out->info, data + hdr_len, data_len - (hdr_len + FCS_SIZE_BYTES)); + out->info_len = data_len - (hdr_len + FCS_SIZE_BYTES); + } + break; + case 0x3e: + out->ftype = OSMO_RLP_FT_S; + out->s_ftype = (data[0] >> 1) & 3; + out->n_r = n_r; + break; + default: + out->ftype = OSMO_RLP_FT_IS; + out->s_ftype = (data[0] >> 1) & 3; + out->n_s = n_s; + out->n_r = n_r; + memcpy(out->info, data + hdr_len, data_len - (hdr_len + FCS_SIZE_BYTES)); + out->info_len = data_len - (2 + 3); + break; + } + + return 0; +} + +/*! encode a RLP frame from its abstract representation. Generates FCS. + * \param[out] out caller-allocated output buffer + * \param[in] out_size size of output buffer (in octets); must be 30 (240bit) or 72 (576bit) + * \param[in] in decoded RLP frame which is to be encoded + * \returns number of output bytes used; negative on error */ +int osmo_rlp_encode(uint8_t *out, size_t out_size, const struct osmo_rlp_frame_decoded *in) +{ + const uint8_t hdr_len = 2; /* will become a variable when we introduce v2 support */ + uint8_t n_s, n_r, s_bits; + uint32_t fcs; + + /* we only support version 0+1 so far */ + if (in->version >= 2) + return -ENOTSUP; + + if (out_size != 240/8 && out_size != 576/8) + return -EINVAL; + + memset(out, 0, out_size); + + if (in->c_r) + out[0] |= 0x01; + if (in->p_f) + out[1] |= 0x02; + + switch (in->ftype) { + case OSMO_RLP_FT_U: + n_s = 0x3f; + n_r = in->u_ftype; + s_bits = 0; + if (in->u_ftype == OSMO_RLP_U_FT_XID) { + if (in->info_len > out_size - (hdr_len + FCS_SIZE_BYTES)) + return -EINVAL; + memcpy(out+hdr_len, in->info, in->info_len); + } + break; + case OSMO_RLP_FT_S: + n_s = 0x3e; + n_r = in->n_r; + s_bits = in->s_ftype; + break; + case OSMO_RLP_FT_IS: + /* we only support 240 bit so far */ + if (in->info_len > out_size - (hdr_len + FCS_SIZE_BYTES)) + return -EINVAL; + n_s = in->n_s; + n_r = in->n_r; + s_bits = in->s_ftype; + memcpy(out+hdr_len, in->info, in->info_len); + break; + default: + return -EINVAL; + } + + /* patch N(S) into output data */ + out[0] |= (n_s & 0x1F) << 3; + out[1] |= (n_s & 0x20) >> 5; + + /* patch N(R) / M-bits into output data */ + out[1] |= (n_r & 0x3f) << 2; + + /* patch S-bits into output data */ + out[0] |= (s_bits & 3) << 1; + + /* compute FCS + add it to end of frame */ + fcs = osmo_rlp_fcs_compute(out, out_size - FCS_SIZE_BYTES); + out[out_size - 3] = (fcs >> 0) & 0xff; + out[out_size - 2] = (fcs >> 8) & 0xff; + out[out_size - 1] = (fcs >> 16) & 0xff; + + return out_size; +} + + +static const uint32_t rlp_fcs_table[256] = { + 0x00B29D2D, 0x00643A5B, 0x0044D87A, 0x00927F0C, 0x00051C38, 0x00D3BB4E, 0x00F3596F, 0x0025FE19, + 0x008694BC, 0x005033CA, 0x0070D1EB, 0x00A6769D, 0x003115A9, 0x00E7B2DF, 0x00C750FE, 0x0011F788, + 0x00DA8E0F, 0x000C2979, 0x002CCB58, 0x00FA6C2E, 0x006D0F1A, 0x00BBA86C, 0x009B4A4D, 0x004DED3B, + 0x00EE879E, 0x003820E8, 0x0018C2C9, 0x00CE65BF, 0x0059068B, 0x008FA1FD, 0x00AF43DC, 0x0079E4AA, + 0x0062BB69, 0x00B41C1F, 0x0094FE3E, 0x00425948, 0x00D53A7C, 0x00039D0A, 0x00237F2B, 0x00F5D85D, + 0x0056B2F8, 0x0080158E, 0x00A0F7AF, 0x007650D9, 0x00E133ED, 0x0037949B, 0x001776BA, 0x00C1D1CC, + 0x000AA84B, 0x00DC0F3D, 0x00FCED1C, 0x002A4A6A, 0x00BD295E, 0x006B8E28, 0x004B6C09, 0x009DCB7F, + 0x003EA1DA, 0x00E806AC, 0x00C8E48D, 0x001E43FB, 0x008920CF, 0x005F87B9, 0x007F6598, 0x00A9C2EE, + 0x0049DA1E, 0x009F7D68, 0x00BF9F49, 0x0069383F, 0x00FE5B0B, 0x0028FC7D, 0x00081E5C, 0x00DEB92A, + 0x007DD38F, 0x00AB74F9, 0x008B96D8, 0x005D31AE, 0x00CA529A, 0x001CF5EC, 0x003C17CD, 0x00EAB0BB, + 0x0021C93C, 0x00F76E4A, 0x00D78C6B, 0x00012B1D, 0x00964829, 0x0040EF5F, 0x00600D7E, 0x00B6AA08, + 0x0015C0AD, 0x00C367DB, 0x00E385FA, 0x0035228C, 0x00A241B8, 0x0074E6CE, 0x005404EF, 0x0082A399, + 0x0099FC5A, 0x004F5B2C, 0x006FB90D, 0x00B91E7B, 0x002E7D4F, 0x00F8DA39, 0x00D83818, 0x000E9F6E, + 0x00ADF5CB, 0x007B52BD, 0x005BB09C, 0x008D17EA, 0x001A74DE, 0x00CCD3A8, 0x00EC3189, 0x003A96FF, + 0x00F1EF78, 0x0027480E, 0x0007AA2F, 0x00D10D59, 0x00466E6D, 0x0090C91B, 0x00B02B3A, 0x00668C4C, + 0x00C5E6E9, 0x0013419F, 0x0033A3BE, 0x00E504C8, 0x007267FC, 0x00A4C08A, 0x008422AB, 0x005285DD, + 0x001F18F0, 0x00C9BF86, 0x00E95DA7, 0x003FFAD1, 0x00A899E5, 0x007E3E93, 0x005EDCB2, 0x00887BC4, + 0x002B1161, 0x00FDB617, 0x00DD5436, 0x000BF340, 0x009C9074, 0x004A3702, 0x006AD523, 0x00BC7255, + 0x00770BD2, 0x00A1ACA4, 0x00814E85, 0x0057E9F3, 0x00C08AC7, 0x00162DB1, 0x0036CF90, 0x00E068E6, + 0x00430243, 0x0095A535, 0x00B54714, 0x0063E062, 0x00F48356, 0x00222420, 0x0002C601, 0x00D46177, + 0x00CF3EB4, 0x001999C2, 0x00397BE3, 0x00EFDC95, 0x0078BFA1, 0x00AE18D7, 0x008EFAF6, 0x00585D80, + 0x00FB3725, 0x002D9053, 0x000D7272, 0x00DBD504, 0x004CB630, 0x009A1146, 0x00BAF367, 0x006C5411, + 0x00A72D96, 0x00718AE0, 0x005168C1, 0x0087CFB7, 0x0010AC83, 0x00C60BF5, 0x00E6E9D4, 0x00304EA2, + 0x00932407, 0x00458371, 0x00656150, 0x00B3C626, 0x0024A512, 0x00F20264, 0x00D2E045, 0x00044733, + 0x00E45FC3, 0x0032F8B5, 0x00121A94, 0x00C4BDE2, 0x0053DED6, 0x008579A0, 0x00A59B81, 0x00733CF7, + 0x00D05652, 0x0006F124, 0x00261305, 0x00F0B473, 0x0067D747, 0x00B17031, 0x00919210, 0x00473566, + 0x008C4CE1, 0x005AEB97, 0x007A09B6, 0x00ACAEC0, 0x003BCDF4, 0x00ED6A82, 0x00CD88A3, 0x001B2FD5, + 0x00B84570, 0x006EE206, 0x004E0027, 0x0098A751, 0x000FC465, 0x00D96313, 0x00F98132, 0x002F2644, + 0x00347987, 0x00E2DEF1, 0x00C23CD0, 0x00149BA6, 0x0083F892, 0x00555FE4, 0x0075BDC5, 0x00A31AB3, + 0x00007016, 0x00D6D760, 0x00F63541, 0x00209237, 0x00B7F103, 0x00615675, 0x0041B454, 0x00971322, + 0x005C6AA5, 0x008ACDD3, 0x00AA2FF2, 0x007C8884, 0x00EBEBB0, 0x003D4CC6, 0x001DAEE7, 0x00CB0991, + 0x00686334, 0x00BEC442, 0x009E2663, 0x00488115, 0x00DFE221, 0x00094557, 0x0029A776, 0x00FF0000 +}; + +/*! compute RLP FCS according to 3GPP TS 24.022 Section 4.4. + * \param[in] in input data over which to compute FCS + * \param[in] in_len length of input data (in octets) + * \returns computed frame check sequence (FCS). */ +uint32_t osmo_rlp_fcs_compute(const uint8_t *in, size_t in_len) +{ + uint32_t divider = 0; + size_t i; + + for (i = 0; i < in_len; i++) { + uint8_t input = in[i] ^ (divider & 0xff); + divider = (divider >> 8) ^ rlp_fcs_table[input]; + } + + return divider; +} + +/*! @} */ diff --git a/src/gsm/rsl.c b/src/gsm/rsl.c index 5534aa2a..fbba982a 100644 --- a/src/gsm/rsl.c +++ b/src/gsm/rsl.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> @@ -126,6 +122,11 @@ const struct tlv_definition rsl_att_tlvdef = { [RSL_IE_TFO_STATUS] = { TLV_TYPE_TV }, [RSL_IE_LLP_APDU] = { TLV_TYPE_TLV }, [RSL_IE_SIEMENS_MRPCI] = { TLV_TYPE_TV }, + [RSL_IE_OSMO_REP_ACCH_CAP] = { TLV_TYPE_TLV }, + [RSL_IE_OSMO_TRAINING_SEQUENCE] = { TLV_TYPE_TLV }, + [RSL_IE_OSMO_TEMP_OVP_ACCH_CAP] = { TLV_TYPE_TLV }, + [RSL_IE_OSMO_OSMUX_CID] = { TLV_TYPE_TLV }, + [RSL_IE_IPAC_SRTP_CONFIG] = { TLV_TYPE_TLV }, [RSL_IE_IPAC_PROXY_UDP] = { TLV_TYPE_FIXED, 2 }, [RSL_IE_IPAC_BSCMPL_TOUT] = { TLV_TYPE_TV }, [RSL_IE_IPAC_REMOTE_IP] = { TLV_TYPE_FIXED, 4 }, @@ -134,6 +135,8 @@ const struct tlv_definition rsl_att_tlvdef = { [RSL_IE_IPAC_LOCAL_PORT] = { TLV_TYPE_FIXED, 2 }, [RSL_IE_IPAC_SPEECH_MODE] = { TLV_TYPE_TV }, [RSL_IE_IPAC_LOCAL_IP] = { TLV_TYPE_FIXED, 4 }, + [RSL_IE_IPAC_CONN_STAT] = { TLV_TYPE_TLV, 28 }, + [RSL_IE_IPAC_HO_C_PARMS] = { TLV_TYPE_TLV }, [RSL_IE_IPAC_CONN_ID] = { TLV_TYPE_FIXED, 2 }, [RSL_IE_IPAC_RTP_CSD_FMT] = { TLV_TYPE_TV }, [RSL_IE_IPAC_RTP_JIT_BUF] = { TLV_TYPE_FIXED, 2 }, @@ -157,6 +160,7 @@ uint8_t rsl_enc_chan_nr(uint8_t type, uint8_t subch, uint8_t timeslot) switch (type) { case RSL_CHAN_Lm_ACCHs: + case RSL_CHAN_OSMO_VAMOS_Lm_ACCHs: subch &= 0x01; break; case RSL_CHAN_SDCCH4_ACCH: @@ -185,38 +189,34 @@ int rsl_dec_chan_nr(uint8_t chan_nr, uint8_t *type, uint8_t *subch, uint8_t *tim { *timeslot = chan_nr & 0x7; - if ((chan_nr & 0xf8) == RSL_CHAN_Bm_ACCHs) { - *type = RSL_CHAN_Bm_ACCHs; - *subch = 0; - } else if ((chan_nr & 0xf0) == RSL_CHAN_Lm_ACCHs) { - *type = RSL_CHAN_Lm_ACCHs; - *subch = (chan_nr >> 3) & 0x1; - } else if ((chan_nr & 0xe0) == RSL_CHAN_SDCCH4_ACCH) { - *type = RSL_CHAN_SDCCH4_ACCH; - *subch = (chan_nr >> 3) & 0x3; - } else if ((chan_nr & 0xc0) == RSL_CHAN_SDCCH8_ACCH) { - *type = RSL_CHAN_SDCCH8_ACCH; - *subch = (chan_nr >> 3) & 0x7; - } else if ((chan_nr & 0xf8) == RSL_CHAN_BCCH) { - *type = RSL_CHAN_BCCH; - *subch = 0; - } else if ((chan_nr & 0xf8) == RSL_CHAN_RACH) { - *type = RSL_CHAN_RACH; - *subch = 0; - } else if ((chan_nr & 0xf8) == RSL_CHAN_PCH_AGCH) { - *type = RSL_CHAN_PCH_AGCH; - *subch = 0; - } else if ((chan_nr & 0xf8) == RSL_CHAN_OSMO_PDCH) { - *type = RSL_CHAN_OSMO_PDCH; + switch (chan_nr & RSL_CHAN_NR_MASK) { + case RSL_CHAN_Bm_ACCHs: + case RSL_CHAN_BCCH: + case RSL_CHAN_RACH: + case RSL_CHAN_PCH_AGCH: + case RSL_CHAN_OSMO_PDCH: + case RSL_CHAN_OSMO_CBCH4: + case RSL_CHAN_OSMO_CBCH8: + case RSL_CHAN_OSMO_VAMOS_Bm_ACCHs: + *type = chan_nr & RSL_CHAN_NR_MASK; *subch = 0; - } else if ((chan_nr & 0xf8) == RSL_CHAN_OSMO_CBCH4) { - *type = RSL_CHAN_OSMO_CBCH4; - *subch = 0; - } else if ((chan_nr & 0xf8) == RSL_CHAN_OSMO_CBCH8) { - *type = RSL_CHAN_OSMO_CBCH8; - *subch = 0; - } else - return -EINVAL; + break; + default: + if ((chan_nr & 0xf0) == RSL_CHAN_Lm_ACCHs) { + *type = RSL_CHAN_Lm_ACCHs; + *subch = (chan_nr >> 3) & 0x1; + } else if ((chan_nr & 0xe0) == RSL_CHAN_SDCCH4_ACCH) { + *type = RSL_CHAN_SDCCH4_ACCH; + *subch = (chan_nr >> 3) & 0x3; + } else if ((chan_nr & 0xc0) == RSL_CHAN_SDCCH8_ACCH) { + *type = RSL_CHAN_SDCCH8_ACCH; + *subch = (chan_nr >> 3) & 0x7; + } else if ((chan_nr & 0xf0) == RSL_CHAN_OSMO_VAMOS_Lm_ACCHs) { + *type = RSL_CHAN_OSMO_VAMOS_Lm_ACCHs; + *subch = (chan_nr >> 3) & 0x1; + } else + return -EINVAL; + } return 0; } @@ -232,26 +232,30 @@ char *rsl_chan_nr_str_buf(char *buf, size_t buf_len, uint8_t chan_nr) int ts = chan_nr & 7; uint8_t cbits = chan_nr >> 3; - if (cbits == 0x01) + if (cbits == ABIS_RSL_CHAN_NR_CBITS_Bm_ACCHs) snprintf(buf, buf_len, "TCH/F on TS%d", ts); - else if ((cbits & 0x1e) == 0x02) + else if ((cbits & 0x1e) == ABIS_RSL_CHAN_NR_CBITS_Lm_ACCHs(0)) snprintf(buf, buf_len, "TCH/H(%u) on TS%d", cbits & 0x01, ts); - else if ((cbits & 0x1c) == 0x04) + else if ((cbits & 0x1c) == ABIS_RSL_CHAN_NR_CBITS_SDCCH4_ACCH(0)) snprintf(buf, buf_len, "SDCCH/4(%u) on TS%d", cbits & 0x03, ts); - else if ((cbits & 0x18) == 0x08) + else if ((cbits & 0x18) == ABIS_RSL_CHAN_NR_CBITS_SDCCH8_ACCH(0)) snprintf(buf, buf_len, "SDCCH/8(%u) on TS%d", cbits & 0x07, ts); - else if (cbits == 0x10) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_BCCH) snprintf(buf, buf_len, "BCCH on TS%d", ts); - else if (cbits == 0x11) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_RACH) snprintf(buf, buf_len, "RACH on TS%d", ts); - else if (cbits == 0x12) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_PCH_AGCH) snprintf(buf, buf_len, "PCH/AGCH on TS%d", ts); - else if (cbits == 0x18) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_OSMO_PDCH) snprintf(buf, buf_len, "PDCH on TS%d", ts); - else if (cbits == 0x19) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_OSMO_CBCH4) snprintf(buf, buf_len, "CBCH(SDCCH/4) on TS%d", ts); - else if (cbits == 0x1a) + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_OSMO_CBCH8) snprintf(buf, buf_len, "CBCH(SDCCH/8) on TS%d", ts); + else if (cbits == ABIS_RSL_CHAN_NR_CBITS_OSMO_VAMOS_Bm_ACCHs) + snprintf(buf, buf_len, "VAMOS TCH/F on TS%d", ts); + else if ((cbits & 0x1e) == ABIS_RSL_CHAN_NR_CBITS_OSMO_VAMOS_Lm_ACCHs(0)) + snprintf(buf, buf_len, "VAMOS TCH/H(%u) on TS%d", cbits & 0x01, ts); else snprintf(buf, buf_len, "UNKNOWN on TS%d", ts); @@ -264,7 +268,7 @@ char *rsl_chan_nr_str_buf(char *buf, size_t buf_len, uint8_t chan_nr) */ const char *rsl_chan_nr_str(uint8_t chan_nr) { - static __thread char str[20]; + static __thread char str[32]; return rsl_chan_nr_str_buf(str, sizeof(str), chan_nr); } @@ -275,10 +279,10 @@ const char *rsl_chan_nr_str(uint8_t chan_nr) */ char *rsl_chan_nr_str_c(const void *ctx, uint8_t chan_nr) { - char *str = talloc_size(ctx, 20); + char *str = talloc_size(ctx, 32); if (!str) return NULL; - return rsl_chan_nr_str_buf(str, 20, chan_nr); + return rsl_chan_nr_str_buf(str, 32, chan_nr); } static const struct value_string rsl_err_vals[] = { @@ -295,6 +299,9 @@ static const struct value_string rsl_err_vals[] = { { RSL_ERR_CCCH_OVERLOAD, "CCCH Overload" }, { RSL_ERR_ACCH_OVERLOAD, "ACCH Overload" }, { RSL_ERR_PROCESSOR_OVERLOAD, "Processor Overload" }, + { RSL_ERR_BTS_NOT_EQUIPPED, "BTS not equipped" }, + { RSL_ERR_REMOTE_TRANSC_FAIL, "Remote Transcoder Failure" }, + { RSL_ERR_NOTIFICATION_OVERFL, "Notification Overflow" }, { RSL_ERR_RES_UNAVAIL, "Resource not available, unspecified" }, { RSL_ERR_TRANSC_UNAVAIL, "Transcoding not available" }, { RSL_ERR_SERV_OPT_UNAVAIL, "Service or Option not available" }, @@ -546,7 +553,7 @@ void rsl_rll_push_l3(struct msgb *msg, uint8_t msg_type, uint8_t chan_nr, /* construct a RSLms RLL message (DATA INDICATION, UNIT DATA * INDICATION) and send it off via RSLms */ - /* Push the L3 IE tag and lengh */ + /* Push the L3 IE tag and length */ msgb_tv16_push(msg, RSL_IE_L3_INFO, l3_len); /* Then push the RSL header */ @@ -614,6 +621,10 @@ const struct tlv_definition rsl_ipac_eie_tlvdef = { [RSL_IPAC_EIE_3G_NCELL_LIST] = { TLV_TYPE_TLV }, [RSL_IPAC_EIE_SDCCH_CTL_PARAM] = { TLV_TYPE_TV }, [RSL_IPAC_EIE_AMR_CONV_THRESH] = { TLV_TYPE_FIXED, 9 }, + /* Osmocom extensions: */ + [RSL_IPAC_EIE_OSMO_MEAS_AVG_CFG]= { TLV_TYPE_TLV }, + [RSL_IPAC_EIE_OSMO_MS_PWR_CTL] = { TLV_TYPE_TLV }, + [RSL_IPAC_EIE_OSMO_PC_THRESH_COMP]= { TLV_TYPE_TLV }, }, }; diff --git a/src/gsm/rxlev_stat.c b/src/gsm/rxlev_stat.c index 9c650cc1..f1b77f4c 100644 --- a/src/gsm/rxlev_stat.c +++ b/src/gsm/rxlev_stat.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <unistd.h> diff --git a/src/gsm/sysinfo.c b/src/gsm/sysinfo.c index b615871f..40e26524 100644 --- a/src/gsm/sysinfo.c +++ b/src/gsm/sysinfo.c @@ -46,7 +46,7 @@ osmo_static_assert(sizeof(struct gsm48_system_information_type_4) == 13, _si4_si /* bs11 forgot the l2 len, 0-6 rest octets */ osmo_static_assert(sizeof(struct gsm48_system_information_type_5) == 18, _si5_size); osmo_static_assert(sizeof(struct gsm48_system_information_type_6) == 11, _si6_size); - +osmo_static_assert(sizeof(struct gsm48_system_information_type_10) == 1, _si10_size); osmo_static_assert(sizeof(struct gsm48_system_information_type_13) == 3, _si13_size); static const uint8_t sitype2rsl[_MAX_SYSINFO_TYPE] = { diff --git a/src/gsm/tlv_parser.c b/src/gsm/tlv_parser.c index 159b42bd..8dd460db 100644 --- a/src/gsm/tlv_parser.c +++ b/src/gsm/tlv_parser.c @@ -1,4 +1,4 @@ -/* (C) 2008-2017 by Harald Welte <laforge@gnumonks.org> +/* (C) 2008-2020 by Harald Welte <laforge@gnumonks.org> * (C) 2016-2017 by sysmocom - s.f.m.c. GmbH * * All Rights Reserved @@ -24,6 +24,7 @@ #include <stdint.h> #include <errno.h> #include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> #include <osmocom/gsm/tlv.h> /*! \addtogroup tlv @@ -125,7 +126,7 @@ int osmo_tlvp_merge(struct tlv_parsed *dst, const struct tlv_parsed *src) * \param[inout] msg Caller-allocated message buffer with sufficient tailroom * \param[in] type TLV type/format to use during encode * \param[in] tag Tag of TLV to be encoded - * \parma[in] len Length of TLV to be encoded + * \param[in] len Length of TLV to be encoded * \param[in] val Value part of TLV to be encoded * \returns 0 on success; negative in case of error */ int tlv_encode_one(struct msgb *msg, enum tlv_type type, uint8_t tag, @@ -224,20 +225,33 @@ int tlv_encode_ordered(struct msgb *msg, const struct tlv_definition *def, const * \param[in] def structure defining the valid TLV tags / configurations * \param[in] buf the input data buffer to be parsed * \param[in] buf_len length of the input data buffer - * \returns number of bytes consumed by the TLV entry / IE parsed; negative in case of error + * \returns number of bytes consumed by the TLV entry / IE parsed; negative in case of error. + * + * In IEs of type TLV_TYPE_SINGLE_TV, the data pointer \ref o_val will point to the + * byte shared by both the Tag and te Value, hence the tag is to be trimmed + * by the caller. */ int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val, const struct tlv_definition *def, const uint8_t *buf, int buf_len) { uint8_t tag; - int len; + int len; /* number of bytes consumed by TLV entry */ + + if (buf_len < 1) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; tag = *buf; *o_tag = tag; /* single octet TV IE */ - if (def->def[tag & 0xf0].type == TLV_TYPE_SINGLE_TV) { + if (def->def[tag >> 4].type == TLV_TYPE_SINGLE_TV) { + *o_tag = tag >> 4; + *o_val = buf; + *o_len = 1; + return 1; + } else if ((tag > 0x0f) && (def->def[tag & 0xf0].type == TLV_TYPE_SINGLE_TV)) { + /* backward compat for old IEs with half-octet tag defined as 0xN0: */ *o_tag = tag & 0xf0; *o_val = buf; *o_len = 1; @@ -264,56 +278,54 @@ int tlv_parse_one(uint8_t *o_tag, uint16_t *o_len, const uint8_t **o_val, break; case TLV_TYPE_TLV: tlv: /* GSM TS 04.07 11.2.4: Type 4 TLV */ - if (buf + 1 > buf + buf_len) - return -1; + if (buf_len < 2) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; *o_val = buf+2; *o_len = *(buf+1); len = *o_len + 2; - if (len > buf_len) - return -2; break; case TLV_TYPE_vTvLV_GAN: /* 44.318 / 11.1.4 */ /* FIXME: variable-length TAG! */ + if (buf_len < 2) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; if (*(buf+1) & 0x80) { + if (buf_len < 3) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; /* like TL16Vbut without highest bit of len */ - if (2 > buf_len) - return -1; *o_val = buf+3; *o_len = (*(buf+1) & 0x7F) << 8 | *(buf+2); len = *o_len + 3; - if (len > buf_len) - return -2; } else { /* like TLV */ goto tlv; } break; case TLV_TYPE_TvLV: + if (buf_len < 2) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; if (*(buf+1) & 0x80) { /* like TLV, but without highest bit of len */ - if (buf + 1 > buf + buf_len) - return -1; *o_val = buf+2; *o_len = *(buf+1) & 0x7f; len = *o_len + 2; - if (len > buf_len) - return -2; break; } /* like TL16V, fallthrough */ case TLV_TYPE_TL16V: - if (2 > buf_len) - return -1; + if (buf_len < 3) + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; *o_val = buf+3; *o_len = *(buf+1) << 8 | *(buf+2); len = *o_len + 3; - if (len > buf_len) - return -2; break; default: - return -3; + return OSMO_TLVP_ERR_UNKNOWN_TLV_TYPE; } + if (buf_len < len) { + *o_val = NULL; + return OSMO_TLVP_ERR_OFS_LEN_BEYOND_BUFFER; + } return len; } @@ -369,12 +381,12 @@ int tlv_parse2(struct tlv_parsed *dec, int dec_multiples, const uint8_t *val; uint16_t parsed_len; if (ofs > buf_len) - return -1; + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; val = &buf[ofs+1]; len = buf[ofs]; parsed_len = len + 1; if (ofs + parsed_len > buf_len) - return -2; + return OSMO_TLVP_ERR_OFS_LEN_BEYOND_BUFFER; num_parsed++; ofs += parsed_len; /* store the resulting val and len */ @@ -390,12 +402,12 @@ int tlv_parse2(struct tlv_parsed *dec, int dec_multiples, const uint8_t *val; uint16_t parsed_len; if (ofs > buf_len) - return -1; + return OSMO_TLVP_ERR_OFS_BEYOND_BUFFER; val = &buf[ofs+1]; len = buf[ofs]; parsed_len = len + 1; if (ofs + parsed_len > buf_len) - return -2; + return OSMO_TLVP_ERR_OFS_LEN_BEYOND_BUFFER; num_parsed++; ofs += parsed_len; /* store the resulting val and len */ @@ -431,7 +443,7 @@ int tlv_parse2(struct tlv_parsed *dec, int dec_multiples, return num_parsed; } -/*! take a master (src) tlvdev and fill up all empty slots in 'dst' +/*! take a master (src) tlv_definition and fill up all empty slots in 'dst' * \param dst TLV parser definition that is to be patched * \param[in] src TLV parser definition whose content is patched into \a dst */ void tlv_def_patch(struct tlv_definition *dst, const struct tlv_definition *src) @@ -627,4 +639,108 @@ fail: return -1; } +static __thread char ienamebuf[32]; +static __thread char msgnamebuf[32]; + +/*! get the message name for given msg_type in protocol pdef */ +const char *osmo_tlv_prot_msg_name(const struct osmo_tlv_prot_def *pdef, uint8_t msg_type) +{ + if (pdef->msg_def[msg_type].name) { + return pdef->msg_def[msg_type].name; + } else if (pdef->msgt_names) { + return get_value_string(pdef->msgt_names, msg_type); + } else { + snprintf(msgnamebuf, sizeof(msgnamebuf), "Unknown msg_type 0x%02x", msg_type); + return msgnamebuf; + } +} + +/*! get the IE name for given IEI in protocol pdef */ +const char *osmo_tlv_prot_ie_name(const struct osmo_tlv_prot_def *pdef, uint8_t iei) +{ + if (pdef->ie_def[iei].name) { + return pdef->ie_def[iei].name; + } else { + snprintf(ienamebuf, sizeof(ienamebuf), "Unknown IEI 0x%02x", iei); + return ienamebuf; + } +} + +/*! Validate an already TLV-decoded message against the protocol definition. + * \param[in] pdef protocol definition of given protocol + * \param[in] msg_type message type of the parsed message + * \param[in] tp TLV parser result + * \param[in] log_subsys logging sub-system for log messages + * \param[in] log_pfx prefix for log messages + * \returns 0 in case of success; negative osmo_tlv_parser_error in case of error + */ +int osmo_tlv_prot_validate_tp(const struct osmo_tlv_prot_def *pdef, uint8_t msg_type, + const struct tlv_parsed *tp, int log_subsys, const char *log_pfx) +{ + const struct osmo_tlv_prot_msg_def *msg_def= &pdef->msg_def[msg_type]; + unsigned int err = 0; + unsigned int i; + + if (msg_def->mand_ies) { + for (i = 0; i < msg_def->mand_count; i++) { + uint8_t iei = msg_def->mand_ies[i]; + if (!TLVP_PRESENT(tp, iei)) { + LOGP(log_subsys, LOGL_ERROR, "%s %s %s: Missing Mandatory IE: %s\n", + log_pfx, pdef->name, osmo_tlv_prot_msg_name(pdef, msg_type), + osmo_tlv_prot_ie_name(pdef, iei)); + if (!err) + err = OSMO_TLVP_ERR_MAND_IE_MISSING; + } + } + } + + for (i = 0; i < ARRAY_SIZE(tp->lv); i++) { + uint16_t min_len; + + if (!TLVP_PRESENT(tp, i)) + continue; + + min_len = pdef->ie_def[i].min_len; + if (TLVP_LEN(tp, i) < min_len) { + LOGP(log_subsys, LOGL_ERROR, "%s %s %s: Short IE %s: %u < %u\n", log_pfx, + pdef->name, osmo_tlv_prot_msg_name(pdef, msg_type), + osmo_tlv_prot_ie_name(pdef, i), TLVP_LEN(tp, i), min_len); + if (!err) + err = OSMO_TLVP_ERR_IE_TOO_SHORT; + } + } + + return err; +} + +/*! Parse + Validate a TLV-encoded message against the protocol definition. + * \param[in] pdef protocol definition of given protocol + * \param[out] dec caller-allocated pointer to \ref tlv_parsed + * \param[in] dec_multiples length of the tlv_parsed[] in \a dec. + * \param[in] msg_type message type of the parsed message + * \param[in] buf the input data buffer to be parsed + * \param[in] buf_len length of the input data buffer + * \param[in] lv_tag an initial LV tag at the start of the buffer + * \param[in] lv_tag2 a second initial LV tag following the \a lv_tag + * \param[in] log_subsys logging sub-system for log messages + * \param[in] log_pfx prefix for log messages + * \returns 0 in case of success; negative osmo_tlv_parser_error in case of error + */ +int osmo_tlv_prot_parse(const struct osmo_tlv_prot_def *pdef, + struct tlv_parsed *dec, unsigned int dec_multiples, uint8_t msg_type, + const uint8_t *buf, unsigned int buf_len, uint8_t lv_tag, uint8_t lv_tag2, + int log_subsys, const char *log_pfx) +{ + int rc; + + rc = tlv_parse2(dec, dec_multiples, pdef->tlv_def, buf, buf_len, lv_tag, lv_tag2); + if (rc < 0) { + LOGP(log_subsys, LOGL_ERROR, "%s %s %s: TLV parser error %d\n", log_pfx, + pdef->name, osmo_tlv_prot_msg_name(pdef, msg_type), rc); + return rc; + } + + return osmo_tlv_prot_validate_tp(pdef, msg_type, dec, log_subsys, log_pfx); +} + /*! @} */ diff --git a/src/gsm/tuak/KeccakP-1600-3gpp.c b/src/gsm/tuak/KeccakP-1600-3gpp.c new file mode 100644 index 00000000..3f5e2ad4 --- /dev/null +++ b/src/gsm/tuak/KeccakP-1600-3gpp.c @@ -0,0 +1,176 @@ +/* ----------------------------------------------------------------------- + * code extracted from 3GPP TS 35.231, annex E for Keccak core functions + * https://portal.3gpp.org/desktopmodules/Specifications/SpecificationDetails.aspx?specificationId=2402 + *-----------------------------------------------------------------------*/ + +/* This code may be freely used or adapted. +*/ + +#include "KeccakP-1600-3gpp.h" + + +const uint8_t Rho[25] = {0,1,62,28,27,36,44,6,55,20,3,10,43,25,39,41,45, + 15,21,8,18,2,61,56,14}; + +const uint8_t Pi[25] = {0,6,12,18,24,3,9,10,16,22,1,7,13,19,20,4,5,11,17, + 23,2,8,14,15,21}; + +const uint8_t Iota[24] = {1,146,218,112,155,33,241,89,138,136,57,42,187,203, + 217,83,82,192,26,106,241,208,33,120}; + +#define ROTATE64(value, n) \ +((((uint64_t)(value))<<(n)) | (((uint64_t)(value))>>(64-(n)))) + +/* --------------------------------------------------------------------- + 64-bit version of Keccak_f(1600) + --------------------------------------------------------------------- +*/ +void Keccak_f_64(uint64_t s[25]) +{ uint64_t t[5]; + uint8_t i, j, round; + + for(round=0; round<24; ++round) + { /* Theta function */ + for(i=0; i<5; ++i) + t[i] = s[i] ^ s[5+i] ^ s[10+i] ^ s[15+i] ^ s[20+i]; + for(i=0; i<5; ++i, s+=5) + { s[0] ^= t[4] ^ ROTATE64(t[1], 1); + s[1] ^= t[0] ^ ROTATE64(t[2], 1); + s[2] ^= t[1] ^ ROTATE64(t[3], 1); + s[3] ^= t[2] ^ ROTATE64(t[4], 1); + s[4] ^= t[3] ^ ROTATE64(t[0], 1); + } + s -= 25; + + /* Rho function */ + for(i=1; i<25; ++i) + s[i] = ROTATE64(s[i], Rho[i]); + + /* Pi function */ + for(t[1] = s[i=1]; (j=Pi[i]) > 1; s[i]=s[j], i=j); + s[i] = t[1]; + + /* Chi function */ + for(i=0; i<5; ++i, s += 5) + { t[0] = (~s[1]) & s[2]; + t[1] = (~s[2]) & s[3]; + t[2] = (~s[3]) & s[4]; + t[3] = (~s[4]) & s[0]; + t[4] = (~s[0]) & s[1]; + for(j=0; j<5; ++j) s[j] ^= t[j]; + } + s -= 25; + + /* Iota function */ + t[0] = Iota[round]; + *s ^= (t[0] | (t[0]<<11) | (t[0]<<26) | (t[0]<<57)) + & 0x800000008000808BULL; /* set & mask bits 0,1,3,7,15,31,63 */ + } +} + + +/* --------------------------------------------------------------------- + 8-bit version of Keccak_f(1600) + --------------------------------------------------------------------- +*/ +void Keccak_f_8(uint8_t s[200]) +{ uint8_t t[40], i, j, k, round; + + for(round=0; round<24; ++round) + { /* Theta function */ + for(i=0; i<40; ++i) + t[i]=s[i]^s[40+i]^s[80+i]^s[120+i]^s[160+i]; + for(i=0; i<200; i+=8) + for(j = (i+32)%40, k=0; k<8; ++k) + s[i+k] ^= t[j+k]; + for(i=0; i<40; t[i] = (t[i]<<1)|j, i+=8) + for(j = t[i+7]>>7, k=7; k; --k) + t[i+k] = (t[i+k]<<1)|(t[i+k-1]>>7); + for(i=0; i<200; i+=8) + for(j = (i+8)%40, k=0; k<8; ++k) + s[i+k] ^= t[j+k]; + + /* Rho function */ + for(i=8; i<200; i+=8) + { for(j = Rho[i>>3]>>3, k=0; k<8; ++k) /* j:=bytes to shift, s->t */ + t[(k+j)&7] = s[i+k]; + for(j = Rho[i>>3]&7, k=7; k; --k) /* j:=bits to shift, t->s */ + s[i+k] = (t[k]<<j) | (t[k-1]>>(8-j)); + s[i] = (t[0]<<j) | (t[7]>>(8-j)); + } + + /* Pi function */ + for(k=8; k<16; ++k) t[k] = s[k]; /* =memcpy(t+8, s+8, 8) */ + for(i=1; (j=Pi[i])>1; i=j) + for(k=0; k<8; ++k) /* =memcpy(s+(i<<3), s+(j<<3), 8) */ + s[(i<<3)|k] = s[(j<<3)|k]; + for(k=0; k<8; ++k) /* =memcpy(s+(i<<3), t+8, 8) */ + s[(i<<3)|k] = t[k+8]; + + /* Chi function */ + for(i=0; i<200; i+=40) + { for(j=0; j<40; ++j) + t[j]=(~s[i+(j+8)%40]) & s[i+(j+16)%40]; + for(j=0; j<40; ++j) s[i+j]^=t[j]; + } + + /* Iota function */ + k = Iota[round]; + s[0] ^= k & 0x8B; /* bits 0, 1, 3, 7 */ + s[1] ^= (k<<3)&0x80; /* bit 15 */ + s[3] ^= (k<<2)&0x80; /* bit 31 */ + s[7] ^= (k<<1)&0x80; /* bit 63 */ + + } +} + +/* --------------------------------------------------------------------- + 32-bit version of Keccak_f(1600) + --------------------------------------------------------------------- +*/ +void Keccak_f_32(uint32_t s[50]) +{ uint32_t t[10]; + uint8_t i, j, round, k; + + for(round=0; round<24; ++round) + { /* Theta function */ + for(i=0; i<10; ++i) + t[i] = s[i] ^ s[10+i] ^ s[20+i] ^ s[30+i] ^ s[40+i]; + for(i=0; i<5; ++i) + for(j=8, k=2; ; j%=10, k=(k+2)%10) + { *s++ ^= t[j++] ^ ((t[k]<<1)|(t[k+1]>>31)); + *s++ ^= t[j++] ^ ((t[k+1]<<1)|(t[k]>>31)); + if(j==8) break; + } + s -= 50; + + /* Rho function */ + for(i=2; i<50; i+=2) + { k = Rho[i>>1] & 0x1f; + t[0] = (s[i+1] << k) | (s[i] >> (32-k)); + t[1] = (s[i] << k) | (s[i+1] >> (32-k)); + k = Rho[i>>1] >> 5; + s[i] = t[1-k], s[i+1] = t[k]; + } + + /* Pi function */ + for(i=2, t[0]=s[2], t[1]=s[3]; (j=(Pi[i>>1]<<1))>2; i=j) + s[i]=s[j], s[i+1]=s[j+1]; + s[i]=t[0], s[i+1]=t[1]; + + /* Chi function */ + for(i=0; i<5; ++i, s+=10) + { for(j=0; j<10; ++j) + t[j] = (~s[(j+2)%10]) & s[(j+4)%10]; + for(j=0; j<10; ++j) + s[j] ^= t[j]; + } + s -= 50; + + /* Iota function */ + t[0] = Iota[round]; + s[0] ^= (t[0] | (t[0]<<11) | (t[0]<<26)) & 0x8000808B; + s[1] ^= (t[0]<<25) & 0x80000000; + } +} + diff --git a/src/gsm/tuak/KeccakP-1600-3gpp.h b/src/gsm/tuak/KeccakP-1600-3gpp.h new file mode 100644 index 00000000..a23cc460 --- /dev/null +++ b/src/gsm/tuak/KeccakP-1600-3gpp.h @@ -0,0 +1,25 @@ +/* ----------------------------------------------------------------------- + * code extracted from 3GPP TS 35.231, annex E for Keccak core functions + * https://portal.3gpp.org/desktopmodules/Specifications/SpecificationDetails.aspx?specificationId=2402 + *-----------------------------------------------------------------------*/ + +/* this is the trick to make the code cross-platform + * at least, Win32 / Linux */ + +#if defined(_WIN32) || defined(__WIN32__) +# include <windows.h> +# define EXPORTIT __declspec(dllexport) +#else +# define EXPORTIT +#endif + +#include <stdint.h> + +/*------------------------------------------------------------------------ + * KeccakP-1600-3gpp.h + *------------------------------------------------------------------------*/ + +EXPORTIT void Keccak_f_8 (uint8_t s[200]); +EXPORTIT void Keccak_f_32(uint32_t s[50]); +EXPORTIT void Keccak_f_64(uint64_t s[25]); + diff --git a/src/gsm/tuak/tuak.c b/src/gsm/tuak/tuak.c new file mode 100644 index 00000000..c044a377 --- /dev/null +++ b/src/gsm/tuak/tuak.c @@ -0,0 +1,372 @@ +/* (C) 2023 by Harald Welte <laforge@osmocom.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <stdint.h> +#include <string.h> +#include <string.h> +#include <errno.h> + +#include <osmocom/core/utils.h> + +#include "KeccakP-1600-3gpp.h" + +/* TUAK authentication algorithm + * as proposed by 3GPP as an alternative to Milenage + * algorithm based on SHA-3 (more exactly its KeccakP-1600 permutation) + * see 3GPP TS 35.231, 232 and 233 */ + +static unsigned int g_keccak_iterations = 1; +static const char algoname[] = "TUAK1.0"; +const uint8_t zero16[16] = { 0, }; + +void tuak_set_keccak_iterations(unsigned int i) +{ + g_keccak_iterations = i; +} + +/* append data from 'input' to 'buf' at 'idx', reversing byte order */ +#define PUSH_DATA(buf, idx, input, nbytes) \ + for (int i = nbytes-1; i >= 0; i--) { \ + buf[idx++] = input[i]; \ + } + +/* like memcpy(), but reversing they order of bytes */ +void memcpy_reverse(uint8_t *dst, const uint8_t *src, size_t len) +{ + for (size_t i = 0; i < len; i++) + dst[i] = src[len-i-1]; +} + +static void tuak_core(uint8_t buf[200], const uint8_t *opc, uint8_t instance, const uint8_t *_rand, + const uint8_t *amf, const uint8_t *sqn, const uint8_t *k, uint8_t k_len_bytes, + unsigned int keccac_iterations) +{ + unsigned int idx = 0; + + PUSH_DATA(buf, idx, opc, 32); + buf[idx++] = instance; + PUSH_DATA(buf, idx, algoname, strlen(algoname)); /* without trailing NUL */ + PUSH_DATA(buf, idx, _rand, 16); + PUSH_DATA(buf, idx, amf, 2); + PUSH_DATA(buf, idx, sqn, 6); + PUSH_DATA(buf, idx, k, k_len_bytes); + memset(buf+idx, 0, 32-k_len_bytes); idx += 32-k_len_bytes; + buf[idx++] = 0x1f; + memset(buf+idx, 0, 38); idx += 38; + buf[idx++] = 0x80; + memset(buf+idx, 0, 64); idx += 64; + OSMO_ASSERT(idx == 200); + + for (unsigned int i = 0; i < keccac_iterations; i++) + Keccak_f_64((uint64_t *) buf); +} + +/** + * tuak_f1 - TUAK f1 algorithm + * @opc: OPc = 256-bit value derived from OP and K + * @k: K = 128-bit or 256-bit subscriber key + * @_rand: RAND = 128-bit random challenge + * @sqn: SQN = 48-bit sequence number + * @amf: AMF = 16-bit authentication management field + * @mac_a: Buffer for MAC-A = 64/128/256-bit network authentication code + * Returns: 0 on success, -1 on failure + */ +int tuak_f1(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *_rand, + const uint8_t *sqn, const uint8_t *amf, uint8_t *mac_a, uint8_t mac_a_len_bytes, + unsigned int keccac_iterations) +{ + uint8_t buf[200]; + uint8_t instance = 0x00; + + switch (mac_a_len_bytes) { + case 8: + instance |= 0x08; + break; + case 16: + instance |= 0x10; + break; + case 32: + instance |= 0x20; + break; + default: + return -EINVAL; + } + + switch (k_len_bytes) { + case 16: + break; + case 32: + instance |= 0x01; + break; + default: + return -EINVAL; + } + + tuak_core(buf, opc, instance, _rand, amf, sqn, k, k_len_bytes, keccac_iterations); + + memcpy_reverse(mac_a, buf, mac_a_len_bytes); + + return 0; +} + +/** + * tuak_f1star - TUAK f1* algorithm + * @opc: OPc = 256-bit value derived from OP and K + * @k: K = 128-bit or 256-bit subscriber key + * @_rand: RAND = 128-bit random challenge + * @sqn: SQN = 48-bit sequence number + * @amf: AMF = 16-bit authentication management field + * @mac_s: Buffer for MAC-S = 64/128/256-bit resync authentication code + * Returns: 0 on success, -1 on failure + */ +int tuak_f1star(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *_rand, + const uint8_t *sqn, const uint8_t *amf, uint8_t *mac_s, uint8_t mac_s_len_bytes, + unsigned int keccac_iterations) +{ + uint8_t buf[200]; + uint8_t instance = 0x80; + + switch (mac_s_len_bytes) { + case 8: + instance |= 0x08; + break; + case 16: + instance |= 0x10; + break; + case 32: + instance |= 0x20; + break; + default: + return -EINVAL; + } + + switch (k_len_bytes) { + case 16: + break; + case 32: + instance |= 0x01; + break; + default: + return -EINVAL; + } + + tuak_core(buf, opc, instance, _rand, amf, sqn, k, k_len_bytes, keccac_iterations); + + memcpy_reverse(mac_s, buf, mac_s_len_bytes); + + return 0; +} + +/** + * tuak_f2345 - TUAK f2, f3, f4, f5, algorithms + * @opc: OPc = 256-bit value derived from OP and K + * @k: K = 128/256-bit subscriber key + * @_rand: RAND = 128-bit random challenge + * @res: Buffer for RES = 32/64/128/256-bit signed response (f2), or %NULL + * @ck: Buffer for CK = 128/256-bit confidentiality key (f3), or %NULL + * @ik: Buffer for IK = 128/256-bit integrity key (f4), or %NULL + * @ak: Buffer for AK = 48-bit anonymity key (f5), or %NULL + * Returns: 0 on success, -1 on failure + */ +int tuak_f2345(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, uint8_t *res, uint8_t res_len_bytes, + uint8_t *ck, uint8_t ck_len_bytes, + uint8_t *ik, uint8_t ik_len_bytes, uint8_t *ak, unsigned int keccac_iterations) +{ + uint8_t buf[200]; + uint8_t instance = 0x40; + + switch (res_len_bytes) { + case 4: + break; + case 8: + instance |= 0x08; + break; + case 16: + instance |= 0x10; + break; + case 32: + instance |= 0x20; + break; + default: + return -EINVAL; + } + + switch (ck_len_bytes) { + case 16: + break; + case 32: + instance |= 0x04; + break; + default: + return -EINVAL; + } + + switch (ik_len_bytes) { + case 16: + break; + case 32: + instance |= 0x02; + break; + default: + return -EINVAL; + } + + switch (k_len_bytes) { + case 16: + break; + case 32: + instance |= 0x01; + break; + default: + return -EINVAL; + } + + tuak_core(buf, opc, instance, _rand, zero16, zero16, k, k_len_bytes, keccac_iterations); + + if (res) + memcpy_reverse(res, buf, res_len_bytes); + + if (ck) + memcpy_reverse(ck, buf + 32, ck_len_bytes); + + if (ik) + memcpy_reverse(ik, buf + 64, ik_len_bytes); + + if (ak) + memcpy_reverse(ak, buf + 96, 6); + + return 0; +} + +/** + * tuak_f5star - TUAK f5* algorithm + * @opc: OPc = 256-bit value derived from OP and K + * @k: K = 128/256-bit subscriber key + * @_rand: RAND = 128-bit random challenge + * @ak: Buffer for AK = 48-bit anonymity key (f5) + * Returns: 0 on success, -1 on failure + */ +int tuak_f5star(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, uint8_t *ak, unsigned int keccac_iterations) +{ + uint8_t buf[200]; + uint8_t instance = 0xc0; + + switch (k_len_bytes) { + case 16: + break; + case 32: + instance += 1; + break; + default: + return -EINVAL; + } + + tuak_core(buf, opc, instance, _rand, zero16, zero16, k, k_len_bytes, keccac_iterations); + + memcpy_reverse(ak, buf + 96, 6); + + return 0; +} + +/** + * tuak_generate - Generate AKA AUTN,IK,CK,RES + * @opc: OPc = 256-bit operator variant algorithm configuration field (encr.) + * @amf: AMF = 16-bit authentication management field + * @k: K = 128/256-bit subscriber key + * @sqn: SQN = 48-bit sequence number + * @_rand: RAND = 128-bit random challenge + * @autn: Buffer for AUTN = 128-bit authentication token + * @ik: Buffer for IK = 128/256-bit integrity key (f4), or %NULL + * @ck: Buffer for CK = 128/256-bit confidentiality key (f3), or %NULL + * @res: Buffer for RES = 32/64/128-bit signed response (f2), or %NULL + * @res_len: Max length for res; set to used length or 0 on failure + */ +void tuak_generate(const uint8_t *opc, const uint8_t *amf, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *sqn, const uint8_t *_rand, uint8_t *autn, uint8_t *ik, + uint8_t *ck, uint8_t *res, size_t *res_len) +{ + int i; + uint8_t mac_a[8], ak[6]; + + if (*res_len < 4) { + *res_len = 0; + return; + } + if (tuak_f1(opc, k, k_len_bytes, _rand, sqn, amf, mac_a, sizeof(mac_a), g_keccak_iterations) || + tuak_f2345(opc, k, k_len_bytes, _rand, res, *res_len, ck, 16, ik, 16, ak, g_keccak_iterations)) { + *res_len = 0; + return; + } + + /* AUTN = (SQN ^ AK) || AMF || MAC */ + for (i = 0; i < 6; i++) + autn[i] = sqn[i] ^ ak[i]; + memcpy(autn + 6, amf, 2); + memcpy(autn + 8, mac_a, 8); +} + + +/** + * tuak_auts - Milenage AUTS validation + * @opc: OPc = 256-bit operator variant algorithm configuration field (encr.) + * @k: K = 128/256-bit subscriber key + * @_rand: RAND = 128-bit random challenge + * @auts: AUTS = 112-bit authentication token from client + * @sqn: Buffer for SQN = 48-bit sequence number + * Returns: 0 = success (sqn filled), -1 on failure + */ +int tuak_auts(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, const uint8_t *auts, uint8_t *sqn) +{ + uint8_t amf[2] = { 0x00, 0x00 }; /* TS 33.102 v7.0.0, 6.3.3 */ + uint8_t ak[6], mac_s[8]; + int i; + + if (tuak_f5star(opc, k, k_len_bytes, _rand, ak, g_keccak_iterations)) + return -1; + for (i = 0; i < 6; i++) + sqn[i] = auts[i] ^ ak[i]; + if (tuak_f1star(opc, k, k_len_bytes, _rand, sqn, amf, mac_s, 8, g_keccak_iterations) || + memcmp(mac_s, auts + 6, 8) != 0) + return -1; + return 0; +} + +int tuak_opc_gen(uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *op) +{ + uint8_t buf[200]; + uint8_t instance; + + switch (k_len_bytes) { + case 16: + instance = 0x00; + break; + case 32: + instance = 0x01; + break; + default: + return -EINVAL; + } + + tuak_core(buf, op, instance, zero16, zero16, zero16, k, k_len_bytes, g_keccak_iterations); + + memcpy_reverse(opc, buf, 32); + + return 0; +} diff --git a/src/gsm/tuak/tuak.h b/src/gsm/tuak/tuak.h new file mode 100644 index 00000000..1a808224 --- /dev/null +++ b/src/gsm/tuak/tuak.h @@ -0,0 +1,33 @@ +#pragma once +#include <stdint.h> + +/* low-level functions */ + +int tuak_f1(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *_rand, + const uint8_t *sqn, const uint8_t *amf, uint8_t *mac_a, uint8_t mac_a_len_bytes, + unsigned int keccac_iterations); + +int tuak_f1star(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *_rand, + const uint8_t *sqn, const uint8_t *amf, uint8_t *mac_s, uint8_t mac_s_len_bytes, + unsigned int keccac_iterations); + +int tuak_f2345(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, uint8_t *res, uint8_t res_len_bytes, + uint8_t *ck, uint8_t ck_len_bytes, + uint8_t *ik, uint8_t ik_len_bytes, uint8_t *ak, unsigned int keccac_iterations); + +int tuak_f5star(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, uint8_t *ak, unsigned int keccac_iterations); + +/* high-level API */ + +void tuak_set_keccak_iterations(unsigned int i); + +void tuak_generate(const uint8_t *opc, const uint8_t *amf, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *sqn, const uint8_t *_rand, uint8_t *autn, uint8_t *ik, + uint8_t *ck, uint8_t *res, size_t *res_len); + +int tuak_auts(const uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, + const uint8_t *_rand, const uint8_t *auts, uint8_t *sqn); + +int tuak_opc_gen(uint8_t *opc, const uint8_t *k, uint8_t k_len_bytes, const uint8_t *op); diff --git a/src/isdn/Makefile.am b/src/isdn/Makefile.am new file mode 100644 index 00000000..3e7f86eb --- /dev/null +++ b/src/isdn/Makefile.am @@ -0,0 +1,26 @@ +# This is _NOT_ the library release version, it's an API version. +# Please read chapter "Library interface versions" of the libtool documentation +# before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html +LIBVERSION=1:0:1 + +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) + +if ENABLE_PSEUDOTALLOC +AM_CPPFLAGS += -I$(top_srcdir)/src/pseudotalloc +endif + +noinst_LTLIBRARIES = libisdnint.la +lib_LTLIBRARIES = libosmoisdn.la + +libisdnint_la_SOURCES = i460_mux.c lapd_core.c v110.c v110_ta.c + +libisdnint_la_LDFLAGS = -no-undefined +libisdnint_la_LIBADD = $(top_builddir)/src/core/libosmocore.la + +libosmoisdn_la_SOURCES = +libosmoisdn_la_LDFLAGS = $(LTLDFLAGS_OSMOISDN) -version-info $(LIBVERSION) -no-undefined +libosmoisdn_la_LIBADD = libisdnint.la $(TALLOC_LIBS) + +EXTRA_DIST = libosmoisdn.map +EXTRA_libosmoisdn_la_DEPENDENCIES = libosmoisdn.map diff --git a/src/isdn/i460_mux.c b/src/isdn/i460_mux.c new file mode 100644 index 00000000..b070bbdc --- /dev/null +++ b/src/isdn/i460_mux.c @@ -0,0 +1,387 @@ +/*! \file i460_mux.c + * ITU-T I.460 sub-channel multiplexer + demultiplexer */ +/* + * (C) 2020 by Harald Welte <laforge@gnumonks.org> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <errno.h> + +#include <osmocom/core/bits.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/msgb.h> +#include <osmocom/isdn/i460_mux.h> + +/*! count the number of sub-channels in this I.460 slot. + * \param[in] ts timeslot that holds the I.460 subchannels. + * \return number of subchannels. */ +int osmo_i460_subchan_count(struct osmo_i460_timeslot *ts) +{ + int i, num_used = 0; + + for (i = 0; i < ARRAY_SIZE(ts->schan); i++) { + if (ts->schan[i].rate != OSMO_I460_RATE_NONE) + num_used++; + } + + return num_used; +} + +/* does this channel have no sub-streams (single 64k subchannel)? */ +static bool osmo_i460_has_single_64k_schan(struct osmo_i460_timeslot *ts) +{ + if (osmo_i460_subchan_count(ts) != 1) + return false; + + if (ts->schan[0].rate != OSMO_I460_RATE_64k) + return false; + + return true; +} + +/*********************************************************************** + * Demultiplexer + ***********************************************************************/ + +/* append a single bit to a sub-channel */ +static void demux_subchan_append_bit(struct osmo_i460_subchan *schan, uint8_t bit) +{ + struct osmo_i460_subchan_demux *demux = &schan->demux; + + OSMO_ASSERT(demux->out_bitbuf); + OSMO_ASSERT(demux->out_idx < demux->out_bitbuf_size); + + demux->out_bitbuf[demux->out_idx++] = bit ? 1 : 0; + + if (demux->out_idx >= demux->out_bitbuf_size) { + if (demux->out_cb_bits) + demux->out_cb_bits(schan, demux->user_data, demux->out_bitbuf, demux->out_idx); + else { + /* pack bits into bytes */ + OSMO_ASSERT((demux->out_idx % 8) == 0); + unsigned int num_bytes = demux->out_idx / 8; + uint8_t bytes[num_bytes]; + osmo_ubit2pbit(bytes, demux->out_bitbuf, demux->out_idx); + demux->out_cb_bytes(schan, demux->user_data, bytes, num_bytes); + } + demux->out_idx = 0; + } +} + +/* extract those bits relevant to this schan of each byte in 'data' */ +static void demux_subchan_extract_bits(struct osmo_i460_subchan *schan, const uint8_t *data, size_t data_len) +{ + int i; + + for (i = 0; i < data_len; i++) { + uint8_t inbyte = data[i]; + /* I.460 defines sub-channel 0 is using bit positions 1+2 (the two + * most significant bits, hence we extract msb-first */ + uint8_t inbits = inbyte << schan->bit_offset; + + /* extract the bits relevant to the given schan */ + switch (schan->rate) { + case OSMO_I460_RATE_8k: + demux_subchan_append_bit(schan, inbits & 0x80); + break; + case OSMO_I460_RATE_16k: + demux_subchan_append_bit(schan, inbits & 0x80); + demux_subchan_append_bit(schan, inbits & 0x40); + break; + case OSMO_I460_RATE_32k: + demux_subchan_append_bit(schan, inbits & 0x80); + demux_subchan_append_bit(schan, inbits & 0x40); + demux_subchan_append_bit(schan, inbits & 0x20); + demux_subchan_append_bit(schan, inbits & 0x10); + break; + case OSMO_I460_RATE_64k: + demux_subchan_append_bit(schan, inbits & 0x80); + demux_subchan_append_bit(schan, inbits & 0x40); + demux_subchan_append_bit(schan, inbits & 0x20); + demux_subchan_append_bit(schan, inbits & 0x10); + demux_subchan_append_bit(schan, inbits & 0x08); + demux_subchan_append_bit(schan, inbits & 0x04); + demux_subchan_append_bit(schan, inbits & 0x02); + demux_subchan_append_bit(schan, inbits & 0x01); + break; + default: + OSMO_ASSERT(0); + } + } +} + +/*! Feed multiplexed data (from an E1 timeslot) into de-multiplexer. + * \param[in] ts timeslot state. + * \param[in] data input data bytes as received from E1/T1. + * \param[in] data_len length of data in bytes. */ +void osmo_i460_demux_in(struct osmo_i460_timeslot *ts, const uint8_t *data, size_t data_len) +{ + struct osmo_i460_subchan *schan; + struct osmo_i460_subchan_demux *demux; + int i; + + /* fast path if entire 64k slot is used */ + if (osmo_i460_has_single_64k_schan(ts)) { + schan = &ts->schan[0]; + demux = &schan->demux; + if (demux->out_cb_bytes) + demux->out_cb_bytes(schan, demux->user_data, data, data_len); + else { + ubit_t bits[data_len*8]; + osmo_pbit2ubit(bits, data, data_len*8); + demux->out_cb_bits(schan, demux->user_data, bits, data_len*8); + } + return; + } + + /* Slow path iterating over all lchans */ + for (i = 0; i < ARRAY_SIZE(ts->schan); i++) { + schan = &ts->schan[i]; + if (schan->rate == OSMO_I460_RATE_NONE) + continue; + demux_subchan_extract_bits(schan, data, data_len); + } +} + + +/*********************************************************************** + * Multiplexer + ***********************************************************************/ + +/*! enqueue a to-be-transmitted message buffer containing unpacked bits */ +void osmo_i460_mux_enqueue(struct osmo_i460_subchan *schan, struct msgb *msg) +{ + OSMO_ASSERT(msgb_length(msg) > 0); + msgb_enqueue(&schan->mux.tx_queue, msg); +} + +/* mux: pull the next bit out of the given sub-channel */ +static ubit_t mux_schan_provide_bit(struct osmo_i460_subchan *schan) +{ + struct osmo_i460_subchan_mux *mux = &schan->mux; + struct msgb *msg; + ubit_t bit; + + /* if we don't have anything to transmit, return '1' bits */ + if (llist_empty(&mux->tx_queue)) { + /* User code now has a last chance to put something into the queue. */ + if (mux->in_cb_queue_empty) + mux->in_cb_queue_empty(schan, mux->user_data); + + /* If the queue is still empty, return idle bits */ + if (llist_empty(&mux->tx_queue)) + return 0x01; + } + msg = llist_entry(mux->tx_queue.next, struct msgb, list); + bit = msgb_pull_u8(msg); + + /* free msgb if we have pulled the last bit */ + if (msgb_length(msg) <= 0) { + llist_del(&msg->list); + talloc_free(msg); + } + + return bit; +} + +/*! provide one byte with the subchan-specific bits of given sub-channel. + * \param[in] schan sub-channel that is to provide bits + * \param[out] mask bitmask of those bits filled in + * \returns bits of given sub-channel */ +static uint8_t mux_subchan_provide_bits(struct osmo_i460_subchan *schan, uint8_t *mask) +{ + uint8_t outbits = 0; + uint8_t outmask; + + /* I.460 defines sub-channel 0 is using bit positions 1+2 (the two + * most significant bits, hence we provide msb-first */ + + switch (schan->rate) { + case OSMO_I460_RATE_8k: + outbits = mux_schan_provide_bit(schan) << 7; + outmask = 0x80; + break; + case OSMO_I460_RATE_16k: + outbits |= mux_schan_provide_bit(schan) << 7; + outbits |= mux_schan_provide_bit(schan) << 6; + outmask = 0xC0; + break; + case OSMO_I460_RATE_32k: + outbits |= mux_schan_provide_bit(schan) << 7; + outbits |= mux_schan_provide_bit(schan) << 6; + outbits |= mux_schan_provide_bit(schan) << 5; + outbits |= mux_schan_provide_bit(schan) << 4; + outmask = 0xF0; + break; + case OSMO_I460_RATE_64k: + outbits |= mux_schan_provide_bit(schan) << 7; + outbits |= mux_schan_provide_bit(schan) << 6; + outbits |= mux_schan_provide_bit(schan) << 5; + outbits |= mux_schan_provide_bit(schan) << 4; + outbits |= mux_schan_provide_bit(schan) << 3; + outbits |= mux_schan_provide_bit(schan) << 2; + outbits |= mux_schan_provide_bit(schan) << 1; + outbits |= mux_schan_provide_bit(schan) << 0; + outmask = 0xFF; + break; + default: + OSMO_ASSERT(0); + } + *mask = outmask >> schan->bit_offset; + return outbits >> schan->bit_offset; +} + +/* provide one byte of multiplexed I.460 bits */ +static uint8_t mux_timeslot_provide_bits(struct osmo_i460_timeslot *ts) +{ + uint8_t ret = 0xff; /* unused bits must be '1' as per I.460 */ + + for (int i = 0; i < ARRAY_SIZE(ts->schan); i++) { + struct osmo_i460_subchan *schan = &ts->schan[i]; + uint8_t bits, mask; + + if (schan->rate == OSMO_I460_RATE_NONE) + continue; + bits = mux_subchan_provide_bits(schan, &mask); + ret &= ~mask; + ret |= bits; + } + + return ret; +} + + +/*! Get multiplexed data from de-multiplexer (for feeding it into an E1 timeslot). + * \param[in] ts timeslot state. + * \param[out] out caller-provided buffer where to store generated output bytes. + * \param[in] out_len number of bytes to be stored at out. */ +int osmo_i460_mux_out(struct osmo_i460_timeslot *ts, uint8_t *out, size_t out_len) +{ + int i; + + /* fast path if entire 64k slot is used */ + //if (osmo_i460_has_single_64k_schan(ts)) { } + + for (i = 0; i < out_len; i++) + out[i] = mux_timeslot_provide_bits(ts); + + return out_len; +} + + +/*********************************************************************** + * Initialization / Control + ***********************************************************************/ + + +static int alloc_bitbuf(void *ctx, struct osmo_i460_subchan *schan, size_t num_bits) +{ + struct osmo_i460_subchan_demux *demux = &schan->demux; + + talloc_free(demux->out_bitbuf); + demux->out_bitbuf = talloc_zero_size(ctx, num_bits); + if (!demux->out_bitbuf) + return -ENOMEM; + demux->out_bitbuf_size = num_bits; + + return 0; +} + + +static int find_unused_subchan_idx(const struct osmo_i460_timeslot *ts) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(ts->schan); i++) { + const struct osmo_i460_subchan *schan = &ts->schan[i]; + if (schan->rate == OSMO_I460_RATE_NONE) + return i; + } + return -1; +} + +/* reset subchannel struct into a defined state */ +static void subchan_reset(struct osmo_i460_subchan *schan, bool first_time) +{ + /* Before we zero out the subchannel struct, we must be sure that the + * tx_queue is cleared and all dynamically allocated memory is freed. + * However, on an uninitalized subchannel struct we can not be sure + * that the pointers are valid. If the subchannel is reset the first + * time the caller must set first_time to true. */ + if (!first_time) { + if (schan->demux.out_bitbuf) + talloc_free(schan->demux.out_bitbuf); + msgb_queue_free(&schan->mux.tx_queue); + } + + /* Reset subchannel to a defined state */ + memset(schan, 0, sizeof(*schan)); + schan->rate = OSMO_I460_RATE_NONE; + INIT_LLIST_HEAD(&schan->mux.tx_queue); +} + +/*! initialize an I.460 timeslot */ +void osmo_i460_ts_init(struct osmo_i460_timeslot *ts) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(ts->schan); i++) { + struct osmo_i460_subchan *schan = &ts->schan[i]; + schan->ts = ts; + subchan_reset(schan, true); + } +} + +/*! add a new sub-channel to the given timeslot + * \param[in] ctx talloc context from where to allocate the internal buffer + * \param[in] ts timeslot to which to add a sub-channel + * \param[in] chd description of the sub-channel to be added + * \return pointer to sub-channel on success, NULL on error */ +struct osmo_i460_subchan * +osmo_i460_subchan_add(void *ctx, struct osmo_i460_timeslot *ts, const struct osmo_i460_schan_desc *chd) +{ + struct osmo_i460_subchan *schan; + int idx, rc; + + idx = find_unused_subchan_idx(ts); + if (idx < 0) + return NULL; + + schan = &ts->schan[idx]; + + schan->rate = chd->rate; + schan->bit_offset = chd->bit_offset; + + schan->demux.out_cb_bits = chd->demux.out_cb_bits; + schan->demux.out_cb_bytes = chd->demux.out_cb_bytes; + schan->demux.user_data = chd->demux.user_data; + schan->mux.in_cb_queue_empty = chd->mux.in_cb_queue_empty; + schan->mux.user_data = chd->mux.user_data; + rc = alloc_bitbuf(ctx, schan, chd->demux.num_bits); + if (rc < 0) { + subchan_reset(schan, false); + return NULL; + } + + /* return number of schan in use */ + return schan; +} + +/* remove a su-channel from the multiplex */ +void osmo_i460_subchan_del(struct osmo_i460_subchan *schan) +{ + subchan_reset(schan, false); +} + +/*! @} */ diff --git a/src/gsm/lapd_core.c b/src/isdn/lapd_core.c index a0f3c2b4..b32ed263 100644 --- a/src/gsm/lapd_core.c +++ b/src/isdn/lapd_core.c @@ -1,7 +1,7 @@ /*! \file lapd_core.c * LAPD core implementation */ /* - * (C) 2010-2011 by Harald Welte <laforge@gnumonks.org> + * (C) 2010-2020 by Harald Welte <laforge@gnumonks.org> * (C) 2010-2011 by Andreas Eversberg <jolly@eversberg.eu> * * All Rights Reserved @@ -18,10 +18,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ /*! \addtogroup lapd @@ -73,6 +69,7 @@ //#define TEST_CONTENT_RESOLUTION_NETWORK #include <stdio.h> +#include <stdbool.h> #include <stdint.h> #include <string.h> #include <errno.h> @@ -82,7 +79,7 @@ #include <osmocom/core/msgb.h> #include <osmocom/core/utils.h> #include <osmocom/core/talloc.h> -#include <osmocom/gsm/lapd_core.h> +#include <osmocom/isdn/lapd_core.h> #include <osmocom/gsm/rsl.h> /* TS 04.06 Table 4 / Section 3.8.1 */ @@ -104,13 +101,14 @@ #define CR_NET2USER_RESP 0 #define LAPD_HEADROOM 56 +#define LAPD_TAILROOM 16 #define SBIT(a) (1 << a) #define ALL_STATES 0xffffffff static void lapd_t200_cb(void *data); static void lapd_t203_cb(void *data); -static int lapd_send_i(struct lapd_msg_ctx *lctx, int line); +static int lapd_send_i(struct lapd_datalink *dl, int line, bool rts); static int lapd_est_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx); /* UTILITY FUNCTIONS */ @@ -120,7 +118,7 @@ struct msgb *lapd_msgb_alloc(int length, const char *name) /* adding space for padding, FIXME: add as an option */ if (length < 21) length = 21; - return msgb_alloc_headroom(length + LAPD_HEADROOM, LAPD_HEADROOM, name); + return msgb_alloc_headroom(length + LAPD_HEADROOM + LAPD_TAILROOM, LAPD_HEADROOM, name); } static inline uint8_t do_mod(uint8_t x, uint8_t m) @@ -171,12 +169,17 @@ static void lapd_dl_flush_hist(struct lapd_datalink *dl) } } -static void lapd_dl_flush_tx(struct lapd_datalink *dl) +static void lapd_dl_flush_tx_queue(struct lapd_datalink *dl) { struct msgb *msg; while ((msg = msgb_dequeue(&dl->tx_queue))) msgb_free(msg); +} + +static void lapd_dl_flush_tx(struct lapd_datalink *dl) +{ + lapd_dl_flush_tx_queue(dl); lapd_dl_flush_hist(dl); } @@ -201,41 +204,79 @@ static inline const char *lapd_state_name(enum lapd_state state) static void lapd_start_t200(struct lapd_datalink *dl) { - if (osmo_timer_pending(&dl->t200)) - return; - LOGP(DLLAPD, LOGL_INFO, "start T200 (dl=%p, timeout=%d.%06ds)\n", - dl, dl->t200_sec, dl->t200_usec); - osmo_timer_schedule(&dl->t200, dl->t200_sec, dl->t200_usec); + if ((dl->lapd_flags & LAPD_F_RTS)) { + if (dl->t200_rts != LAPD_T200_RTS_OFF) + return; + LOGDL(dl, LOGL_INFO, "Start T200. (pending until triggered by RTS)\n"); + dl->t200_rts = LAPD_T200_RTS_PENDING; + } else { + if (osmo_timer_pending(&dl->t200)) + return; + LOGDL(dl, LOGL_INFO, "Start T200 (timeout=%d.%06ds).\n", dl->t200_sec, dl->t200_usec); + osmo_timer_schedule(&dl->t200, dl->t200_sec, dl->t200_usec); + } +} + +/*! Handle timeout condition of T200 in RTS mode. + * The caller (LAPDm code) implements the T200 timer and must detect timeout condition. + * The function gets called by LAPDm code when it detects a timeout of T200. + * \param[in] dl caller-allocated datalink structure */ +int lapd_t200_timeout(struct lapd_datalink *dl) +{ + OSMO_ASSERT((dl->lapd_flags & LAPD_F_RTS)); + + if (dl->t200_rts != LAPD_T200_RTS_RUNNING) + return -EINVAL; + + dl->t200_rts = LAPD_T200_RTS_OFF; + + lapd_t200_cb(dl); + + return 0; } static void lapd_start_t203(struct lapd_datalink *dl) { if (osmo_timer_pending(&dl->t203)) return; - LOGP(DLLAPD, LOGL_INFO, "start T203 (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "start T203\n"); osmo_timer_schedule(&dl->t203, dl->t203_sec, dl->t203_usec); } static void lapd_stop_t200(struct lapd_datalink *dl) { - if (!osmo_timer_pending(&dl->t200)) - return; - LOGP(DLLAPD, LOGL_INFO, "stop T200 (dl=%p)\n", dl); - osmo_timer_del(&dl->t200); + if ((dl->lapd_flags & LAPD_F_RTS)) { + if (dl->t200_rts == LAPD_T200_RTS_OFF) + return; + dl->t200_rts = LAPD_T200_RTS_OFF; + } else { + if (!osmo_timer_pending(&dl->t200)) + return; + osmo_timer_del(&dl->t200); + } + LOGDL(dl, LOGL_INFO, "stop T200\n"); +} + +static bool lapd_is_t200_started(struct lapd_datalink *dl) +{ + if ((dl->lapd_flags & LAPD_F_RTS)) + return (dl->t200_rts != LAPD_T200_RTS_OFF); + else + return osmo_timer_pending(&dl->t200); } static void lapd_stop_t203(struct lapd_datalink *dl) { if (!osmo_timer_pending(&dl->t203)) return; - LOGP(DLLAPD, LOGL_INFO, "stop T203 (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "stop T203\n"); osmo_timer_del(&dl->t203); } static void lapd_dl_newstate(struct lapd_datalink *dl, uint32_t state) { - LOGP(DLLAPD, LOGL_INFO, "new state %s -> %s (dl=%p)\n", - lapd_state_name(dl->state), lapd_state_name(state), dl); + LOGDL(dl, LOGL_INFO, "new state %s -> %s\n", + lapd_state_name(dl->state), lapd_state_name(state)); if (state != LAPD_STATE_MF_EST && dl->state == LAPD_STATE_MF_EST) { /* stop T203 on leaving MF EST state, if running */ @@ -253,11 +294,16 @@ static void lapd_dl_newstate(struct lapd_datalink *dl, uint32_t state) dl->state = state; } -static void *tall_lapd_ctx = NULL; +void *tall_lapd_ctx = NULL; -/* init datalink instance and allocate history */ -void lapd_dl_init(struct lapd_datalink *dl, uint8_t k, uint8_t v_range, - int maxf) +/*! Initialize LAPD datalink instance and allocate history + * \param[in] dl caller-allocated datalink structure + * \param[in] k maximum number of unacknowledged frames + * \param[in] v_range range of sequence numbers + * \param[in] maxf maximum frame size (after defragmentation) + * \param[in] name human-readable name for this LAPD datalink */ +void lapd_dl_init2(struct lapd_datalink *dl, uint8_t k, uint8_t v_range, int maxf, + const char *name) { int m; @@ -293,22 +339,47 @@ void lapd_dl_init(struct lapd_datalink *dl, uint8_t k, uint8_t v_range, } } - LOGP(DLLAPD, LOGL_INFO, "Init DL layer: sequence range = %d, k = %d, " - "history range = %d (dl=%p)\n", dl->v_range, dl->k, - dl->range_hist, dl); + if (!tall_lapd_ctx) { + tall_lapd_ctx = talloc_named_const(NULL, 1, "lapd context"); + OSMO_ASSERT(tall_lapd_ctx); + } + + talloc_free(dl->name); + if (name) + dl->name = talloc_strdup(tall_lapd_ctx, name); + else + dl->name = talloc_asprintf(tall_lapd_ctx, "dl=%p", dl); + + LOGDL(dl, LOGL_INFO, "Init DL layer: sequence range = %d, k = %d, " + "history range = %d\n", dl->v_range, dl->k, dl->range_hist); lapd_dl_newstate(dl, LAPD_STATE_IDLE); - if (!tall_lapd_ctx) - tall_lapd_ctx = talloc_named_const(NULL, 1, "lapd context"); dl->tx_hist = talloc_zero_array(tall_lapd_ctx, struct lapd_history, dl->range_hist); } +/*! Initialize LAPD datalink instance and allocate history + * \param[in] dl caller-allocated datalink structure + * \param[in] k maximum number of unacknowledged frames + * \param[in] v_range range of sequence numbers + * \param[in] maxf maximum frame size (after defragmentation) */ +void lapd_dl_init(struct lapd_datalink *dl, uint8_t k, uint8_t v_range, int maxf) +{ + lapd_dl_init2(dl, k, v_range, maxf, NULL); +} + +void lapd_dl_set_name(struct lapd_datalink *dl, const char *name) +{ + if (!name) + return; + osmo_talloc_replace_string(tall_lapd_ctx, &dl->name, name); +} + /* reset to IDLE state */ void lapd_dl_reset(struct lapd_datalink *dl) { - LOGP(DLLAPD, LOGL_INFO, "Resetting LAPDm instance\n"); + LOGDL(dl, LOGL_INFO, "Resetting LAPD instance\n"); /* enter idle state (and remove eventual cont_res) */ lapd_dl_newstate(dl, LAPD_STATE_IDLE); /* flush buffer */ @@ -322,11 +393,25 @@ void lapd_dl_reset(struct lapd_datalink *dl) lapd_stop_t203(dl); if (dl->state == LAPD_STATE_IDLE) return; - LOGP(DLLAPD, LOGL_INFO, "Resetting LAPDm instance (dl=%p)\n", dl); /* enter idle state (and remove eventual cont_res) */ lapd_dl_newstate(dl, LAPD_STATE_IDLE); } +/*! Set lapd_flags to change behaviour + * \param[in] dl \ref lapd_datalink instance + * \param[in] flags \ref lapd_flags */ +int lapd_dl_set_flags(struct lapd_datalink *dl, unsigned int flags) +{ + if (lapd_is_t200_started(dl) && (flags & LAPD_F_RTS) != (dl->lapd_flags & LAPD_F_RTS)) { + LOGDL(dl, LOGL_ERROR, "Changing RTS flag not allowed while T200 is running.\n"); + return -EINVAL; + } + + dl->lapd_flags = flags; + + return 0; +} + /* reset and de-allocate history buffer */ void lapd_dl_exit(struct lapd_datalink *dl) { @@ -339,6 +424,8 @@ void lapd_dl_exit(struct lapd_datalink *dl) /* free history buffer list */ talloc_free(dl->tx_hist); dl->tx_hist = NULL; + talloc_free(dl->name); + dl->name = NULL; } /*! Set the \ref lapdm_mode of a LAPDm entity */ @@ -389,9 +476,9 @@ static int mdl_error(uint8_t cause, struct lapd_msg_ctx *lctx) struct lapd_datalink *dl = lctx->dl; struct osmo_dlsap_prim dp; - LOGP(DLLAPD, LOGL_NOTICE, - "sending MDL-ERROR-IND cause %d from state %s (dl=%p)\n", - cause, lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_NOTICE, + "sending MDL-ERROR-IND cause %d from state %s\n", + cause, lapd_state_name(dl->state)); osmo_prim_init(&dp.oph, 0, PRIM_MDL_ERROR, PRIM_OP_INDICATION, NULL); dp.u.error_ind.cause = cause; return dl->send_dlsap(&dp, lctx); @@ -546,7 +633,7 @@ static int lapd_reestablish(struct lapd_datalink *dl) struct osmo_dlsap_prim dp; struct msgb *msg; - LOGP(DLLAPD, LOGL_DEBUG, "lapd reestablish (dl=%p)\n", dl); + LOGDL(dl, LOGL_DEBUG, "LAPD reestablish\n"); msg = lapd_msgb_alloc(0, "DUMMY"); osmo_prim_init(&dp.oph, 0, PRIM_DL_EST, PRIM_OP_REQUEST, msg); @@ -559,12 +646,13 @@ static void lapd_t200_cb(void *data) { struct lapd_datalink *dl = data; - LOGP(DLLAPD, LOGL_INFO, "Timeout T200 state=%s (dl=%p)\n", - lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "Timeout T200 state=%s\n", lapd_state_name(dl->state)); switch (dl->state) { case LAPD_STATE_SABM_SENT: /* 5.4.1.3 */ + /* increment re-transmission counter */ + dl->retrans_ctr++; if (dl->retrans_ctr >= dl->n200_est_rel + 1) { /* flush tx and send buffers */ lapd_dl_flush_tx(dl); @@ -583,18 +671,16 @@ static void lapd_t200_cb(void *data) } /* retransmit SABM command */ lapd_send_resend(dl); - /* increment re-transmission counter */ - dl->retrans_ctr++; /* restart T200 (PH-READY-TO-SEND) */ lapd_start_t200(dl); break; case LAPD_STATE_DISC_SENT: /* 5.4.4.3 */ + /* increment re-transmission counter */ + dl->retrans_ctr++; if (dl->retrans_ctr >= dl->n200_est_rel + 1) { /* send MDL ERROR INIDCATION to L3 */ mdl_error(MDL_CAUSE_T200_EXPIRED, &dl->lctx); - /* send RELEASE INDICATION to L3 */ - send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, &dl->lctx); /* flush tx and send buffers */ lapd_dl_flush_tx(dl); lapd_dl_flush_send(dl); @@ -603,12 +689,12 @@ static void lapd_t200_cb(void *data) /* NOTE: we must not change any other states or buffers * and queues, since we may reconnect after handover * failure. the buffered messages is replaced there */ + /* send RELEASE INDICATION to L3 */ + send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, &dl->lctx); break; } /* retransmit DISC command */ lapd_send_resend(dl); - /* increment re-transmission counter */ - dl->retrans_ctr++; /* restart T200 (PH-READY-TO-SEND) */ lapd_start_t200(dl); break; @@ -628,8 +714,7 @@ static void lapd_t200_cb(void *data) int length = dl->tx_hist[h].msg->len; struct lapd_msg_ctx nctx; - LOGP(DLLAPD, LOGL_INFO, "retransmit last frame" - " V(S)=%d (dl=%p)\n", vs, dl); + LOGDL(dl, LOGL_INFO, "retransmit last frame V(S)=%d\n", vs); /* Create I frame (segment) from tx_hist */ memcpy(&nctx, &dl->lctx, sizeof(nctx)); /* keep nctx.ldp */ @@ -660,8 +745,7 @@ static void lapd_t200_cb(void *data) } else if (dl->own_busy) { lapd_send_rnr(&dl->lctx, 1, 1); } else { - LOGP(DLLAPD, LOGL_INFO, "unhandled, " - "pls. fix (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "unhandled, pls. fix\n"); } } /* restart T200 (PH-READY-TO-SEND) */ @@ -672,14 +756,13 @@ static void lapd_t200_cb(void *data) /* reestablish */ if (!dl->reestablish) break; - LOGP(DLLAPD, LOGL_NOTICE, "N200+1 reached, performing " - "reestablishment. (dl=%p)\n", dl); + LOGDL(dl, LOGL_NOTICE, "N200+1 reached, performingreestablishment\n"); lapd_reestablish(dl); } break; default: - LOGP(DLLAPD, LOGL_INFO, "T200 expired in unexpected " - "dl->state %s (dl=%p)\n", lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "T200 expired in unexpected dl->state %s)\n", + lapd_state_name(dl->state)); } } @@ -688,12 +771,10 @@ static void lapd_t203_cb(void *data) { struct lapd_datalink *dl = data; - LOGP(DLLAPD, LOGL_INFO, "Timeout T203 state=%s (dl=%p)\n", - lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "Timeout T203 state=%s\n", lapd_state_name(dl->state)); if (dl->state != LAPD_STATE_MF_EST) { - LOGP(DLLAPD, LOGL_ERROR, "T203 fired outside MF EST state, " - "please fix! (dl=%p)\n", dl); + LOGDL(dl, LOGL_ERROR, "T203 fired outside MF EST state, please fix!\n"); return; } @@ -703,13 +784,11 @@ static void lapd_t203_cb(void *data) lapd_dl_newstate(dl, LAPD_STATE_TIMER_RECOV); /* transmit a supervisory command with P bit set to 1 as follows: */ if (!dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, - "transmit an RR poll command (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "transmit an RR poll command\n"); /* Send RR with P=1 */ lapd_send_rr(&dl->lctx, 1, 1); } else { - LOGP(DLLAPD, LOGL_INFO, - "transmit an RNR poll command (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "transmit an RNR poll command\n"); /* Send RNR with P=1 */ lapd_send_rnr(&dl->lctx, 1, 1); } @@ -717,12 +796,14 @@ static void lapd_t203_cb(void *data) lapd_start_t200(dl); } -/* 5.5.3.1: Common function to acknowlege frames up to the given N(R) value */ -static void lapd_acknowledge(struct lapd_msg_ctx *lctx) +/* 5.5.3.1: Common function to acknowlege frames up to the given N(R) value + * In case of a sequence error, the cause is returned with negative sign. */ +static int lapd_acknowledge(struct lapd_msg_ctx *lctx) { struct lapd_datalink *dl = lctx->dl; uint8_t nr = lctx->n_recv; - int s = 0, rej = 0, t200_reset = 0; + int s = 0, rej = 0; + bool t200_reset = false; int i, h; /* supervisory frame ? */ @@ -738,7 +819,7 @@ static void lapd_acknowledge(struct lapd_msg_ctx *lctx) if (dl->tx_hist[h].msg) { msgb_free(dl->tx_hist[h].msg); dl->tx_hist[h].msg = NULL; - LOGP(DLLAPD, LOGL_INFO, "ack frame %d\n", i); + LOGDL(dl, LOGL_INFO, "ack frame %d\n", i); } } @@ -747,9 +828,8 @@ static void lapd_acknowledge(struct lapd_msg_ctx *lctx) * link layer entity shall reset the timer T200 on * receipt of a valid I frame with N(R) higher than V(A), * or an REJ with an N(R) equal to V(A). */ - if ((!rej && nr != dl->v_ack) - || (rej && nr == dl->v_ack)) { - t200_reset = 1; + if ((!rej && nr != dl->v_ack) || (rej && nr == dl->v_ack)) { + t200_reset = true; lapd_stop_t200(dl); /* 5.5.3.1 Note 1 + 2 imply timer recovery cond. */ } @@ -757,10 +837,9 @@ static void lapd_acknowledge(struct lapd_msg_ctx *lctx) * N(R) is called valid, if and only if * (N(R)-V(A)) mod 8 <= (V(S)-V(A)) mod 8. */ - if (sub_mod(nr, dl->v_ack, dl->v_range) - > sub_mod(dl->v_send, dl->v_ack, dl->v_range)) { - LOGP(DLLAPD, LOGL_NOTICE, "N(R) sequence error (dl=%p)\n", dl); - mdl_error(MDL_CAUSE_SEQ_ERR, lctx); + if (sub_mod(nr, dl->v_ack, dl->v_range) > sub_mod(dl->v_send, dl->v_ack, dl->v_range)) { + LOGDL(dl, LOGL_NOTICE, "N(R) sequence error\n"); + return -MDL_CAUSE_SEQ_ERR; } } @@ -771,8 +850,7 @@ static void lapd_acknowledge(struct lapd_msg_ctx *lctx) * and if there are outstanding I frames, restart T200 */ if (t200_reset && !rej) { if (dl->tx_hist[sub_mod(dl->v_send, 1, dl->range_hist)].msg) { - LOGP(DLLAPD, LOGL_INFO, "start T200, due to unacked I " - "frame(s) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "start T200, due to unacked I frame(s)\n"); lapd_start_t200(dl); } } @@ -782,467 +860,493 @@ static void lapd_acknowledge(struct lapd_msg_ctx *lctx) /* Stop T203, if running */ lapd_stop_t203(dl); /* Start T203, if T200 is not running in MF EST state, if enabled */ - if (!osmo_timer_pending(&dl->t200) - && (dl->t203_sec || dl->t203_usec) - && (dl->state == LAPD_STATE_MF_EST)) { + if (!lapd_is_t200_started(dl) && (dl->t203_sec || dl->t203_usec) && (dl->state == LAPD_STATE_MF_EST)) lapd_start_t203(dl); - } + + return 0; } /* L1 -> L2 */ -/* Receive a LAPD U (Unnumbered) message from L1 */ -static int lapd_rx_u(struct msgb *msg, struct lapd_msg_ctx *lctx) +/* Receive a LAPD U SABM(E) message from L1 */ +static int lapd_rx_u_sabm(struct msgb *msg, struct lapd_msg_ctx *lctx) { struct lapd_datalink *dl = lctx->dl; int length = lctx->length; int rc = 0; uint8_t prim, op; - switch (lctx->s_u) { - case LAPD_U_SABM: - case LAPD_U_SABME: - prim = PRIM_DL_EST; - op = PRIM_OP_INDICATION; - - LOGP(DLLAPD, LOGL_INFO, "SABM(E) received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); - /* 5.7.1 */ - dl->seq_err_cond = 0; - /* G.2.2 Wrong value of the C/R bit */ - if (lctx->cr == dl->cr.rem2loc.resp) { - LOGP(DLLAPD, LOGL_ERROR, - "SABM response error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); - return -EINVAL; - } + prim = PRIM_DL_EST; + op = PRIM_OP_INDICATION; - /* G.4.5 If SABM is received with L>N201 or with M bit - * set, AN MDL-ERROR-INDICATION is sent to MM. - */ - if (lctx->more || length > lctx->n201) { - LOGP(DLLAPD, LOGL_ERROR, - "SABM too large error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); - return -EIO; - } + LOGDL(dl, LOGL_INFO, "SABM(E) received in state %s\n", lapd_state_name(dl->state)); + /* 5.7.1 */ + dl->seq_err_cond = 0; + /* G.2.2 Wrong value of the C/R bit */ + if (lctx->cr == dl->cr.rem2loc.resp) { + LOGDL(dl, LOGL_ERROR, "SABM response error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); + return -EINVAL; + } - switch (dl->state) { - case LAPD_STATE_IDLE: - break; - case LAPD_STATE_MF_EST: - LOGP(DLLAPD, LOGL_INFO, "SABM command, multiple " - "frame established state (dl=%p)\n", dl); - /* If link is lost on the remote side, we start over - * and send DL-ESTABLISH indication again. */ - /* Additionally, continue in case of content resoltion - * (GSM network). This happens, if the mobile has not - * yet received UA or another mobile (collision) tries - * to establish connection. The mobile must receive - * UA again. */ - /* 5.4.2.1 */ - if (!length) { - /* If no content resolution, this is a - * re-establishment. */ - LOGP(DLLAPD, LOGL_INFO, - "Remote reestablish (dl=%p)\n", dl); - break; - } - if (!dl->cont_res) { - LOGP(DLLAPD, LOGL_INFO, "SABM command not " - "allowed in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); - mdl_error(MDL_CAUSE_SABM_MF, lctx); - msgb_free(msg); - return 0; - } - /* Ignore SABM if content differs from first SABM. */ - if (dl->mode == LAPD_MODE_NETWORK && length) { -#ifdef TEST_CONTENT_RESOLUTION_NETWORK - dl->cont_res->data[0] ^= 0x01; -#endif - if (memcmp(dl->cont_res->data, msg->data, - length)) { - LOGP(DLLAPD, LOGL_INFO, "Another SABM " - "with different content - " - "ignoring! (dl=%p)\n", dl); - msgb_free(msg); - return 0; - } - } - /* send UA again */ - lapd_send_ua(lctx, length, msg->l3h); - msgb_free(msg); - return 0; - case LAPD_STATE_DISC_SENT: - /* 5.4.6.2 send DM with F=P */ - lapd_send_dm(lctx); - /* stop Timer T200 */ - lapd_stop_t200(dl); - msgb_free(msg); - return send_dl_simple(prim, op, lctx); - default: - /* collision: Send UA, but still wait for rx UA, then - * change to MF_EST state. - */ + /* G.4.5 If SABM is received with L>N201 or with M bit + * set, AN MDL-ERROR-INDICATION is sent to MM. + */ + if (lctx->more || length > lctx->n201) { + LOGDL(dl, LOGL_ERROR, "SABM too large error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); + return -EIO; + } + + switch (dl->state) { + case LAPD_STATE_IDLE: + break; + case LAPD_STATE_TIMER_RECOV: + LOGDL(dl, LOGL_INFO, "SABM command, timer recovery state\n"); + /* If link is lost on the remote side, we start over + * and send DL-ESTABLISH indication again. */ + /* 3GPP TS 44.006 8.6.3 "Procedures for re-establishment" */ + if (length) { /* check for contention resoultion */ - if (dl->tx_hist[0].msg && dl->tx_hist[0].msg->len) { - LOGP(DLLAPD, LOGL_NOTICE, "SABM not allowed " - "during contention resolution (state=%s, dl=%p)\n", - lapd_state_name(dl->state), dl); - mdl_error(MDL_CAUSE_SABM_INFO_NOTALL, lctx); - } - lapd_send_ua(lctx, length, msg->l3h); + LOGDL(dl, LOGL_ERROR, "SABM L>0 not expected in timer " + "recovery state\n"); + mdl_error(MDL_CAUSE_SABM_INFO_NOTALL, lctx); + lapd_send_dm(lctx); msgb_free(msg); return 0; } - /* save message context for further use */ - memcpy(&dl->lctx, lctx, sizeof(dl->lctx)); -#ifndef TEST_CONTENT_RESOLUTION_NETWORK - /* send UA response */ - lapd_send_ua(lctx, length, msg->l3h); -#endif - /* set Vs, Vr and Va to 0 */ - dl->v_send = dl->v_recv = dl->v_ack = 0; - /* clear tx_hist */ - lapd_dl_flush_hist(dl); - /* enter multiple-frame-established state */ - lapd_dl_newstate(dl, LAPD_STATE_MF_EST); - /* store content resolution data on network side - * Note: cont_res will be removed when changing state again, - * so it must be allocated AFTER lapd_dl_newstate(). */ - if (dl->mode == LAPD_MODE_NETWORK && length) { - dl->cont_res = lapd_msgb_alloc(length, "CONT RES"); - memcpy(msgb_put(dl->cont_res, length), msg->l3h, - length); - LOGP(DLLAPD, LOGL_NOTICE, - "Store content res. (dl=%p)\n", dl); - } - /* send notification to L3 */ - if (length == 0) { - /* 5.4.1.2 Normal establishment procedures */ - rc = send_dl_simple(prim, op, lctx); - msgb_free(msg); - } else { - /* 5.4.1.4 Contention resolution establishment */ - msgb_trim(msg, length); - rc = send_dl_l3(prim, op, lctx, msg); - } + /* re-establishment, continue below */ + lapd_stop_t200(dl); break; - case LAPD_U_DM: - LOGP(DLLAPD, LOGL_INFO, "DM received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); - /* G.2.2 Wrong value of the C/R bit */ - if (lctx->cr == dl->cr.rem2loc.cmd) { - LOGP(DLLAPD, LOGL_ERROR, - "DM command error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); - return -EINVAL; + case LAPD_STATE_MF_EST: + LOGDL(dl, LOGL_INFO, "SABM command, multiple frame established state\n"); + /* If link is lost on the remote side, we start over + * and send DL-ESTABLISH indication again. */ + /* Additionally, continue in case of content resoltion + * (GSM network). This happens, if the mobile has not + * yet received UA or another mobile (collision) tries + * to establish connection. The mobile must receive + * UA again. */ + /* 5.4.2.1 */ + if (!length) { + /* If no content resolution, this is a + * re-establishment. */ + LOGDL(dl, LOGL_INFO, "Remote reestablish\n"); + break; } - if (!lctx->p_f) { - /* 5.4.1.2 DM responses with the F bit set to "0" - * shall be ignored. - */ + if (!dl->cont_res) { + LOGDL(dl, LOGL_INFO, "SABM command not allowed in state %s\n", + lapd_state_name(dl->state)); + mdl_error(MDL_CAUSE_SABM_MF, lctx); msgb_free(msg); return 0; } - switch (dl->state) { - case LAPD_STATE_SABM_SENT: - break; - case LAPD_STATE_MF_EST: - if (lctx->p_f) { - LOGP(DLLAPD, LOGL_INFO, "unsolicited DM " - "response (dl=%p)\n", dl); - mdl_error(MDL_CAUSE_UNSOL_DM_RESP, lctx); - } else { - LOGP(DLLAPD, LOGL_INFO, "unsolicited DM " - "response, multiple frame established " - "state (dl=%p)\n", dl); - mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx); - /* reestablish */ - if (!dl->reestablish) { - msgb_free(msg); - return 0; - } - LOGP(DLLAPD, LOGL_NOTICE, "Performing " - "reestablishment. (dl=%p)\n", dl); - lapd_reestablish(dl); - } - msgb_free(msg); - return 0; - case LAPD_STATE_TIMER_RECOV: - /* FP = 0 (DM is normal in case PF = 1) */ - if (!lctx->p_f) { - LOGP(DLLAPD, LOGL_INFO, "unsolicited DM " - "response, multiple frame established " - "state (dl=%p)\n", dl); - mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx); + /* Ignore SABM if content differs from first SABM. */ + if (dl->mode == LAPD_MODE_NETWORK && length) { +#ifdef TEST_CONTENT_RESOLUTION_NETWORK + dl->cont_res->data[0] ^= 0x01; +#endif + if (memcmp(dl->cont_res->data, msg->data, + length)) { + LOGDL(dl, LOGL_INFO, "Another SABM with different content - " + "ignoring!\n"); msgb_free(msg); - /* reestablish */ - if (!dl->reestablish) - return 0; - LOGP(DLLAPD, LOGL_NOTICE, "Performing " - "reestablishment. (dl=%p)\n", dl); - return lapd_reestablish(dl); + return 0; } - break; - case LAPD_STATE_DISC_SENT: - /* stop Timer T200 */ - lapd_stop_t200(dl); - /* go to idle state */ - lapd_dl_flush_tx(dl); - lapd_dl_flush_send(dl); - lapd_dl_newstate(dl, LAPD_STATE_IDLE); - rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, lctx); - msgb_free(msg); - return 0; - case LAPD_STATE_IDLE: - /* 5.4.5 all other frame types shall be discarded */ - default: - LOGP(DLLAPD, LOGL_INFO, "unsolicited DM response! " - "(discarding) (dl=%p)\n", dl); - msgb_free(msg); - return 0; } - /* stop timer T200 */ + /* send UA again */ + lapd_send_ua(lctx, length, msg->l3h); + msgb_free(msg); + return 0; + case LAPD_STATE_DISC_SENT: + /* 5.4.6.2 send DM with F=P */ + lapd_send_dm(lctx); + /* stop Timer T200 */ lapd_stop_t200(dl); - /* go to idle state */ - lapd_dl_newstate(dl, LAPD_STATE_IDLE); - rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, lctx); msgb_free(msg); - break; - case LAPD_U_UI: - LOGP(DLLAPD, LOGL_INFO, "UI received (dl=%p)\n", dl); - /* G.2.2 Wrong value of the C/R bit */ - if (lctx->cr == dl->cr.rem2loc.resp) { - LOGP(DLLAPD, LOGL_ERROR, "UI indicates response " - "error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); - return -EINVAL; + return send_dl_simple(prim, op, lctx); + default: + /* collision: Send UA, but still wait for rx UA, then + * change to MF_EST state. + */ + /* check for contention resoultion */ + if (dl->tx_hist[0].msg && dl->tx_hist[0].msg->len) { + LOGDL(dl, LOGL_NOTICE, "SABM not allowed during contention " + "resolution (state=%s)\n", lapd_state_name(dl->state)); + mdl_error(MDL_CAUSE_SABM_INFO_NOTALL, lctx); } + lapd_send_ua(lctx, length, msg->l3h); + msgb_free(msg); + return 0; + } + /* save message context for further use */ + memcpy(&dl->lctx, lctx, sizeof(dl->lctx)); +#ifndef TEST_CONTENT_RESOLUTION_NETWORK + /* send UA response */ + lapd_send_ua(lctx, length, msg->l3h); +#endif + /* set Vs, Vr and Va to 0 */ + dl->v_send = dl->v_recv = dl->v_ack = 0; + /* clear tx_hist */ + lapd_dl_flush_hist(dl); + /* enter multiple-frame-established state */ + lapd_dl_newstate(dl, LAPD_STATE_MF_EST); + /* store content resolution data on network side + * Note: cont_res will be removed when changing state again, + * so it must be allocated AFTER lapd_dl_newstate(). */ + if (dl->mode == LAPD_MODE_NETWORK && length) { + dl->cont_res = lapd_msgb_alloc(length, "CONT RES"); + memcpy(msgb_put(dl->cont_res, length), msg->l3h, + length); + LOGDL(dl, LOGL_INFO, "Store content res.\n"); + } + /* send notification to L3 */ + if (length == 0) { + /* 5.4.1.2 Normal establishment procedures */ + rc = send_dl_simple(prim, op, lctx); + msgb_free(msg); + } else { + /* 5.4.1.4 Contention resolution establishment */ + msgb_trim(msg, length); + rc = send_dl_l3(prim, op, lctx, msg); + } + return rc; +} + +/* Receive a LAPD U DM message from L1 */ +static int lapd_rx_u_dm(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + int rc = 0; - /* G.4.5 If UI is received with L>N201 or with M bit - * set, AN MDL-ERROR-INDICATION is sent to MM. + LOGDL(dl, LOGL_INFO, "DM received in state %s\n", lapd_state_name(dl->state)); + /* G.2.2 Wrong value of the C/R bit */ + if (lctx->cr == dl->cr.rem2loc.cmd) { + LOGDL(dl, LOGL_ERROR, "DM command error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); + return -EINVAL; + } + if (!lctx->p_f) { + /* 5.4.1.2 DM responses with the F bit set to "0" + * shall be ignored. */ - if (length > lctx->n201 || lctx->more) { - LOGP(DLLAPD, LOGL_ERROR, "UI too large error " - "(%d > N201(%d) or M=%d) (dl=%p)\n", length, - lctx->n201, lctx->more, dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); - return -EIO; + msgb_free(msg); + return 0; + } + switch (dl->state) { + case LAPD_STATE_SABM_SENT: + break; + case LAPD_STATE_MF_EST: + if (lctx->p_f) { + LOGDL(dl, LOGL_INFO, "unsolicited DM response\n"); + mdl_error(MDL_CAUSE_UNSOL_DM_RESP, lctx); + } else { + LOGDL(dl, LOGL_INFO, "unsolicited DM response, " + "multiple frame established state\n"); + mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx); + /* reestablish */ + if (!dl->reestablish) { + msgb_free(msg); + return 0; + } + LOGDL(dl, LOGL_NOTICE, "Performing reestablishment\n"); + lapd_reestablish(dl); } - - /* do some length checks */ - if (length == 0) { - /* 5.3.3 UI frames received with the length indicator - * set to "0" shall be ignored - */ - LOGP(DLLAPD, LOGL_INFO, - "length=0 (discarding) (dl=%p)\n", dl); + msgb_free(msg); + return 0; + case LAPD_STATE_TIMER_RECOV: + /* FP = 0 (DM is normal in case PF = 1) */ + if (!lctx->p_f) { + LOGDL(dl, LOGL_INFO, "unsolicited DM response, multiple frame " + "established state\n"); + mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx); msgb_free(msg); - return 0; + /* reestablish */ + if (!dl->reestablish) + return 0; + LOGDL(dl, LOGL_NOTICE, "Performing reestablishment\n"); + return lapd_reestablish(dl); } - msgb_trim(msg, length); - rc = send_dl_l3(PRIM_DL_UNIT_DATA, PRIM_OP_INDICATION, lctx, - msg); break; - case LAPD_U_DISC: - prim = PRIM_DL_REL; - op = PRIM_OP_INDICATION; - - LOGP(DLLAPD, LOGL_INFO, "DISC received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); - /* flush tx and send buffers */ + case LAPD_STATE_DISC_SENT: + /* stop Timer T200 */ + lapd_stop_t200(dl); + /* go to idle state */ lapd_dl_flush_tx(dl); lapd_dl_flush_send(dl); - /* 5.7.1 */ - dl->seq_err_cond = 0; - /* G.2.2 Wrong value of the C/R bit */ - if (lctx->cr == dl->cr.rem2loc.resp) { - LOGP(DLLAPD, LOGL_ERROR, - "DISC response error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); - return -EINVAL; - } - if (length > 0 || lctx->more) { - /* G.4.4 If a DISC or DM frame is received with L>0 or - * with the M bit set to "1", an MDL-ERROR-INDICATION - * primitive with cause "U frame with incorrect - * parameters" is sent to the mobile management entity. - */ - LOGP(DLLAPD, LOGL_ERROR, - "U frame iwth incorrect parameters (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); - return -EIO; - } - switch (dl->state) { - case LAPD_STATE_IDLE: - LOGP(DLLAPD, LOGL_INFO, - "DISC in idle state (dl=%p)\n", dl); - /* send DM with F=P */ - msgb_free(msg); - return lapd_send_dm(lctx); - case LAPD_STATE_SABM_SENT: - LOGP(DLLAPD, LOGL_INFO, - "DISC in SABM state (dl=%p)\n", dl); - /* 5.4.6.2 send DM with F=P */ - lapd_send_dm(lctx); - /* stop Timer T200 */ - lapd_stop_t200(dl); - /* go to idle state */ - lapd_dl_newstate(dl, LAPD_STATE_IDLE); - msgb_free(msg); - return send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, - lctx); - case LAPD_STATE_MF_EST: - case LAPD_STATE_TIMER_RECOV: - LOGP(DLLAPD, LOGL_INFO, - "DISC in est state (dl=%p)\n", dl); - break; - case LAPD_STATE_DISC_SENT: - LOGP(DLLAPD, LOGL_INFO, - "DISC in disc state (dl=%p)\n", dl); - prim = PRIM_DL_REL; - op = PRIM_OP_CONFIRM; - break; - default: - lapd_send_ua(lctx, length, msg->l3h); - msgb_free(msg); - return 0; - } - /* send UA response */ - lapd_send_ua(lctx, length, msg->l3h); + lapd_dl_newstate(dl, LAPD_STATE_IDLE); + rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, lctx); + msgb_free(msg); + return 0; + case LAPD_STATE_IDLE: + /* 5.4.5 all other frame types shall be discarded */ + default: + LOGDL(dl, LOGL_INFO, "unsolicited DM response! (discarding)\n"); + msgb_free(msg); + return 0; + } + /* stop timer T200 */ + lapd_stop_t200(dl); + /* go to idle state */ + lapd_dl_newstate(dl, LAPD_STATE_IDLE); + rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, lctx); + msgb_free(msg); + return rc; +} + +/* Receive a LAPD U UI message from L1 */ +static int lapd_rx_u_ui(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + int length = lctx->length; + + LOGDL(dl, LOGL_INFO, "UI received\n"); + /* G.2.2 Wrong value of the C/R bit */ + if (lctx->cr == dl->cr.rem2loc.resp) { + LOGDL(dl, LOGL_ERROR, "UI indicates response error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); + return -EINVAL; + } + + /* G.4.5 If UI is received with L>N201 or with M bit + * set, AN MDL-ERROR-INDICATION is sent to MM. + */ + if (length > lctx->n201 || lctx->more) { + LOGDL(dl, LOGL_ERROR, "UI too large error (%d > N201(%d) or M=%d)\n", + length, lctx->n201, lctx->more); + msgb_free(msg); + mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); + return -EIO; + } + + /* do some length checks */ + if (length == 0) { + /* 5.3.3 UI frames received with the length indicator + * set to "0" shall be ignored + */ + LOGDL(dl, LOGL_INFO, "length=0 (discarding)\n"); + msgb_free(msg); + return 0; + } + msgb_trim(msg, length); + return send_dl_l3(PRIM_DL_UNIT_DATA, PRIM_OP_INDICATION, lctx, msg); +} + +/* Receive a LAPD U DISC message from L1 */ +static int lapd_rx_u_disc(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + int length = lctx->length; + int rc = 0; + uint8_t prim, op; + + prim = PRIM_DL_REL; + op = PRIM_OP_INDICATION; + + LOGDL(dl, LOGL_INFO, "DISC received in state %s\n", lapd_state_name(dl->state)); + /* flush tx and send buffers */ + lapd_dl_flush_tx(dl); + lapd_dl_flush_send(dl); + /* 5.7.1 */ + dl->seq_err_cond = 0; + /* G.2.2 Wrong value of the C/R bit */ + if (lctx->cr == dl->cr.rem2loc.resp) { + LOGDL(dl, LOGL_ERROR, "DISC response error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); + return -EINVAL; + } + if (length > 0 || lctx->more) { + /* G.4.4 If a DISC or DM frame is received with L>0 or + * with the M bit set to "1", an MDL-ERROR-INDICATION + * primitive with cause "U frame with incorrect + * parameters" is sent to the mobile management entity. + */ + LOGDL(dl, LOGL_ERROR, "U frame iwth incorrect parameters\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); + return -EIO; + } + switch (dl->state) { + case LAPD_STATE_IDLE: + LOGDL(dl, LOGL_INFO, "DISC in idle state\n"); + /* send DM with F=P */ + msgb_free(msg); + return lapd_send_dm(lctx); + case LAPD_STATE_SABM_SENT: + LOGDL(dl, LOGL_INFO, "DISC in SABM state\n"); + /* 5.4.6.2 send DM with F=P */ + lapd_send_dm(lctx); /* stop Timer T200 */ lapd_stop_t200(dl); - /* enter idle state, keep tx-buffer with UA response */ + /* go to idle state */ lapd_dl_newstate(dl, LAPD_STATE_IDLE); - /* send notification to L3 */ - rc = send_dl_simple(prim, op, lctx); msgb_free(msg); + return send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, + lctx); + case LAPD_STATE_MF_EST: + case LAPD_STATE_TIMER_RECOV: + LOGDL(dl, LOGL_INFO, "DISC in est state\n"); break; - case LAPD_U_UA: - LOGP(DLLAPD, LOGL_INFO, "UA received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); - /* G.2.2 Wrong value of the C/R bit */ - if (lctx->cr == dl->cr.rem2loc.cmd) { - LOGP(DLLAPD, LOGL_ERROR, "UA indicates command " - "error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); - return -EINVAL; - } + case LAPD_STATE_DISC_SENT: + LOGDL(dl, LOGL_INFO, "DISC in disc state\n"); + prim = PRIM_DL_REL; + op = PRIM_OP_CONFIRM; + break; + default: + lapd_send_ua(lctx, length, msg->l3h); + msgb_free(msg); + return 0; + } + /* send UA response */ + lapd_send_ua(lctx, length, msg->l3h); + /* stop Timer T200 */ + lapd_stop_t200(dl); + /* enter idle state, keep tx-buffer with UA response */ + lapd_dl_newstate(dl, LAPD_STATE_IDLE); + /* send notification to L3 */ + rc = send_dl_simple(prim, op, lctx); + msgb_free(msg); + return rc; +} - /* G.4.5 If UA is received with L>N201 or with M bit - * set, AN MDL-ERROR-INDICATION is sent to MM. - */ - if (lctx->more || length > lctx->n201) { - LOGP(DLLAPD, LOGL_ERROR, - "UA too large error (dl=%p)\n", dl); - msgb_free(msg); - mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); - return -EIO; - } +/* Receive a LAPD U UA message from L1 */ +static int lapd_rx_u_ua(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + int length = lctx->length; + int rc = 0; - if (!lctx->p_f) { - /* 5.4.1.2 A UA response with the F bit set to "0" - * shall be ignored. - */ - LOGP(DLLAPD, LOGL_INFO, - "F=0 (discarding) (dl=%p)\n", dl); - msgb_free(msg); - return 0; - } - switch (dl->state) { - case LAPD_STATE_SABM_SENT: - break; - case LAPD_STATE_MF_EST: - case LAPD_STATE_TIMER_RECOV: - LOGP(DLLAPD, LOGL_INFO, "unsolicited UA response! " - "(discarding) (dl=%p)\n", dl); - mdl_error(MDL_CAUSE_UNSOL_UA_RESP, lctx); - msgb_free(msg); - return 0; - case LAPD_STATE_DISC_SENT: - LOGP(DLLAPD, LOGL_INFO, - "UA in disconnect state (dl=%p)\n", dl); - /* stop Timer T200 */ - lapd_stop_t200(dl); + LOGDL(dl, LOGL_INFO, "UA received in state %s\n", lapd_state_name(dl->state)); + /* G.2.2 Wrong value of the C/R bit */ + if (lctx->cr == dl->cr.rem2loc.cmd) { + LOGDL(dl, LOGL_ERROR, "UA indicates command error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); + return -EINVAL; + } + + /* G.4.5 If UA is received with L>N201 or with M bit + * set, AN MDL-ERROR-INDICATION is sent to MM. + */ + if (lctx->more || length > lctx->n201) { + LOGDL(dl, LOGL_ERROR, "UA too large error\n"); + msgb_free(msg); + mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx); + return -EIO; + } + + if (!lctx->p_f) { + /* 5.4.1.2 A UA response with the F bit set to "0" + * shall be ignored. + */ + LOGDL(dl, LOGL_INFO, "F=0 (discarding)\n"); + msgb_free(msg); + return 0; + } + switch (dl->state) { + case LAPD_STATE_SABM_SENT: + break; + case LAPD_STATE_MF_EST: + case LAPD_STATE_TIMER_RECOV: + LOGDL(dl, LOGL_INFO, "unsolicited UA response! (discarding)\n"); + mdl_error(MDL_CAUSE_UNSOL_UA_RESP, lctx); + msgb_free(msg); + return 0; + case LAPD_STATE_DISC_SENT: + LOGDL(dl, LOGL_INFO, "UA in disconnect state\n"); + /* stop Timer T200 */ + lapd_stop_t200(dl); + /* go to idle state */ + lapd_dl_flush_tx(dl); + lapd_dl_flush_send(dl); + lapd_dl_newstate(dl, LAPD_STATE_IDLE); + rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, lctx); + msgb_free(msg); + return 0; + case LAPD_STATE_IDLE: + /* 5.4.5 all other frame types shall be discarded */ + default: + LOGDL(dl, LOGL_INFO, "unsolicited UA response! (discarding)\n"); + msgb_free(msg); + return 0; + } + LOGDL(dl, LOGL_INFO, "UA in SABM state\n"); + /* stop Timer T200 */ + lapd_stop_t200(dl); + /* compare UA with SABME if contention resolution is applied */ + if (dl->tx_hist[0].msg->len) { + if (length != (dl->tx_hist[0].msg->len) + || !!memcmp(dl->tx_hist[0].msg->data, msg->l3h, + length)) { + LOGDL(dl, LOGL_INFO, "**** UA response mismatches ****\n"); /* go to idle state */ lapd_dl_flush_tx(dl); lapd_dl_flush_send(dl); lapd_dl_newstate(dl, LAPD_STATE_IDLE); - rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, lctx); - msgb_free(msg); - return 0; - case LAPD_STATE_IDLE: - /* 5.4.5 all other frame types shall be discarded */ - default: - LOGP(DLLAPD, LOGL_INFO, "unsolicited UA response! " - "(discarding) (dl=%p)\n", dl); + rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, lctx); msgb_free(msg); return 0; } - LOGP(DLLAPD, LOGL_INFO, "UA in SABM state (dl=%p)\n", dl); - /* stop Timer T200 */ - lapd_stop_t200(dl); - /* compare UA with SABME if contention resolution is applied */ - if (dl->tx_hist[0].msg->len) { - if (length != (dl->tx_hist[0].msg->len) - || !!memcmp(dl->tx_hist[0].msg->data, msg->l3h, - length)) { - LOGP(DLLAPD, LOGL_INFO, "**** UA response " - "mismatches **** (dl=%p)\n", dl); - rc = send_dl_simple(PRIM_DL_REL, - PRIM_OP_INDICATION, lctx); - msgb_free(msg); - /* go to idle state */ - lapd_dl_flush_tx(dl); - lapd_dl_flush_send(dl); - lapd_dl_newstate(dl, LAPD_STATE_IDLE); - return 0; - } - } - /* set Vs, Vr and Va to 0 */ - dl->v_send = dl->v_recv = dl->v_ack = 0; - /* clear tx_hist */ - lapd_dl_flush_hist(dl); - /* enter multiple-frame-established state */ - lapd_dl_newstate(dl, LAPD_STATE_MF_EST); - /* send outstanding frames, if any (resume / reconnect) */ - lapd_send_i(lctx, __LINE__); - /* send notification to L3 */ - rc = send_dl_simple(PRIM_DL_EST, PRIM_OP_CONFIRM, lctx); - msgb_free(msg); - break; + } + /* set Vs, Vr and Va to 0 */ + dl->v_send = dl->v_recv = dl->v_ack = 0; + /* clear tx_hist */ + lapd_dl_flush_hist(dl); + /* enter multiple-frame-established state */ + lapd_dl_newstate(dl, LAPD_STATE_MF_EST); + /* send outstanding frames, if any (resume / reconnect) */ + lapd_send_i(dl, __LINE__, false); + /* send notification to L3 */ + rc = send_dl_simple(PRIM_DL_EST, PRIM_OP_CONFIRM, lctx); + msgb_free(msg); + return rc; +} + +/* Receive a LAPD U FRMR message from L1 */ +static int lapd_rx_u_frmr(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + + LOGDL(dl, LOGL_NOTICE, "Frame reject received\n"); + /* send MDL ERROR INIDCATION to L3 */ + mdl_error(MDL_CAUSE_FRMR, lctx); + msgb_free(msg); + /* reestablish */ + if (!dl->reestablish) + return 0; + LOGDL(dl, LOGL_NOTICE, "Performing reestablishment\n"); + return lapd_reestablish(dl); +} + +/* Receive a LAPD U (Unnumbered) message from L1 */ +static int lapd_rx_u(struct msgb *msg, struct lapd_msg_ctx *lctx) +{ + switch (lctx->s_u) { + case LAPD_U_SABM: + case LAPD_U_SABME: + return lapd_rx_u_sabm(msg, lctx); + case LAPD_U_DM: + return lapd_rx_u_dm(msg, lctx); + case LAPD_U_UI: + return lapd_rx_u_ui(msg, lctx); + case LAPD_U_DISC: + return lapd_rx_u_disc(msg, lctx); + case LAPD_U_UA: + return lapd_rx_u_ua(msg, lctx); case LAPD_U_FRMR: - LOGP(DLLAPD, LOGL_NOTICE, - "Frame reject received (dl=%p)\n", dl); - /* send MDL ERROR INIDCATION to L3 */ - mdl_error(MDL_CAUSE_FRMR, lctx); - msgb_free(msg); - /* reestablish */ - if (!dl->reestablish) - break; - LOGP(DLLAPD, LOGL_NOTICE, - "Performing reestablishment. (dl=%p)\n", dl); - rc = lapd_reestablish(dl); - break; + return lapd_rx_u_frmr(msg, lctx); default: /* G.3.1 */ - LOGP(DLLAPD, LOGL_NOTICE, - "Unnumbered frame not allowed. (dl=%p)\n", dl); + LOGDL(lctx->dl, LOGL_NOTICE, "Unnumbered frame not allowed\n"); msgb_free(msg); mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); return -EINVAL; } - return rc; } /* Receive a LAPD S (Supervisory) message from L1 */ @@ -1256,8 +1360,7 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) * with the M bit set to "1", an MDL-ERROR-INDICATION * primitive with cause "S frame with incorrect * parameters" is sent to the mobile management entity. */ - LOGP(DLLAPD, LOGL_ERROR, - "S frame with incorrect parameters (dl=%p)\n", dl); + LOGDL(dl, LOGL_ERROR, "S frame with incorrect parameters\n"); msgb_free(msg); mdl_error(MDL_CAUSE_SFRM_INC_PARAM, lctx); return -EIO; @@ -1267,8 +1370,7 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) && lctx->p_f && dl->state != LAPD_STATE_TIMER_RECOV) { /* 5.4.2.2: Inidcate error on supervisory reponse F=1 */ - LOGP(DLLAPD, LOGL_NOTICE, - "S frame response with F=1 error (dl=%p)\n", dl); + LOGDL(dl, LOGL_NOTICE, "S frame response with F=1 error\n"); mdl_error(MDL_CAUSE_UNSOL_SPRV_RESP, lctx); } @@ -1281,15 +1383,13 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) /* fall though */ case LAPD_STATE_SABM_SENT: case LAPD_STATE_DISC_SENT: - LOGP(DLLAPD, LOGL_NOTICE, - "S frame ignored in this state (dl=%p)\n", dl); + LOGDL(dl, LOGL_NOTICE, "S frame ignored in this state\n"); msgb_free(msg); return 0; } switch (lctx->s_u) { case LAPD_S_RR: - LOGP(DLLAPD, LOGL_INFO, "RR received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "RR received in state %s\n", lapd_state_name(dl->state)); /* 5.5.3.1: Acknowlege all tx frames up the the N(R)-1 */ lapd_acknowledge(lctx); @@ -1297,10 +1397,8 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) if (lctx->cr == dl->cr.rem2loc.cmd && lctx->p_f) { if (!dl->own_busy && !dl->seq_err_cond) { - LOGP(DLLAPD, LOGL_INFO, "RR frame command " - "with polling bit set and we are not " - "busy, so we reply with RR frame " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RR frame command with polling bit set and " + "we are not busy, so we reply with RR frame response\n"); lapd_send_rr(lctx, 1, 0); /* NOTE: In case of sequence error condition, * the REJ frame has been transmitted when @@ -1308,18 +1406,15 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) * done here */ } else if (dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, "RR frame command " - "with polling bit set and we are busy, " - "so we reply with RR frame response (dl=%p)\n", - dl); + LOGDL(dl, LOGL_INFO, "RR frame command with polling bit set and " + "we are busy, so we reply with RR frame response\n"); lapd_send_rnr(lctx, 1, 0); } } else if (lctx->cr == dl->cr.rem2loc.resp && lctx->p_f && dl->state == LAPD_STATE_TIMER_RECOV) { - LOGP(DLLAPD, LOGL_INFO, "RR response with F==1, " - "and we are in timer recovery state, so " - "we leave that state (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RR response with F==1, and we are in timer recovery " + "state, so we leave that state\n"); /* V(S) to the N(R) in the RR frame */ dl->v_send = lctx->n_recv; /* stop Timer T200 */ @@ -1328,56 +1423,52 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) lapd_dl_newstate(dl, LAPD_STATE_MF_EST); } /* Send message, if possible due to acknowledged data */ - lapd_send_i(lctx, __LINE__); + lapd_send_i(dl, __LINE__, false); break; case LAPD_S_RNR: - LOGP(DLLAPD, LOGL_INFO, "RNR received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "RNR received in state %s\n", lapd_state_name(dl->state)); /* 5.5.3.1: Acknowlege all tx frames up the the N(R)-1 */ lapd_acknowledge(lctx); /* 5.5.5 */ /* Set peer receiver busy condition */ dl->peer_busy = 1; + /* Flush pending messages in TX queue. */ + lapd_dl_flush_tx_queue(dl); + /* stop Timer T200 */ + lapd_stop_t200(dl); if (lctx->p_f) { if (lctx->cr == dl->cr.rem2loc.cmd) { if (!dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, "RNR poll " - "command and we are not busy, " - "so we reply with RR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RNR poll command and we are not busy, " + "so we reply with RR final response\n"); /* Send RR with F=1 */ lapd_send_rr(lctx, 1, 0); } else { - LOGP(DLLAPD, LOGL_INFO, "RNR poll " - "command and we are busy, so " - "we reply with RNR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RNR poll command and we are busy, so " + "we reply with RNR final response\n"); /* Send RNR with F=1 */ lapd_send_rnr(lctx, 1, 0); } } else if (dl->state == LAPD_STATE_TIMER_RECOV) { - LOGP(DLLAPD, LOGL_INFO, "RNR poll response " - "and we in timer recovery state, so " - "we leave that state (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RNR poll response and we in timer recovery " + "state, so we leave that state\n"); /* 5.5.7 Clear timer recovery condition */ lapd_dl_newstate(dl, LAPD_STATE_MF_EST); /* V(S) to the N(R) in the RNR frame */ dl->v_send = lctx->n_recv; } } else - LOGP(DLLAPD, LOGL_INFO, "RNR not polling/final state " - "received (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "RNR not polling/final state received\n"); /* Send message, if possible due to acknowledged data */ - lapd_send_i(lctx, __LINE__); + lapd_send_i(dl, __LINE__, false); break; case LAPD_S_REJ: - LOGP(DLLAPD, LOGL_INFO, "REJ received in state %s (dl=%p)\n", - lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "REJ received in state %s\n", lapd_state_name(dl->state)); /* 5.5.3.1: Acknowlege all tx frames up the the N(R)-1 */ lapd_acknowledge(lctx); @@ -1387,17 +1478,16 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) dl->peer_busy = 0; /* V(S) and V(A) to the N(R) in the REJ frame */ dl->v_send = dl->v_ack = lctx->n_recv; + /* Flush pending messages in TX queue. */ + lapd_dl_flush_tx_queue(dl); /* stop Timer T200 */ lapd_stop_t200(dl); /* 5.5.3.2 */ if (lctx->cr == dl->cr.rem2loc.cmd && lctx->p_f) { if (!dl->own_busy && !dl->seq_err_cond) { - LOGP(DLLAPD, LOGL_INFO, "REJ poll " - "command not in timer recovery " - "state and not in own busy " - "condition received, so we " - "respond with RR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ poll command not in timer recovery " + "state and not in own busy condition received, so we " + "respond with RR final response\n"); lapd_send_rr(lctx, 1, 0); /* NOTE: In case of sequence error * condition, the REJ frame has been @@ -1406,33 +1496,28 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) * here */ } else if (dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, "REJ poll " - "command not in timer recovery " - "state and in own busy " - "condition received, so we " - "respond with RNR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ poll command not in timer recovery " + "state and in own busy condition received, so we " + "respond with RNR final response\n"); lapd_send_rnr(lctx, 1, 0); } } else - LOGP(DLLAPD, LOGL_INFO, "REJ response or not " - "polling command not in timer recovery " - "state received (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ response or not polling command not " + "in timer recovery state received\n"); /* send MDL ERROR INIDCATION to L3 */ if (lctx->cr == dl->cr.rem2loc.resp && lctx->p_f) { - LOGP(DLLAPD, LOGL_ERROR, - "unsolicited supervisory response! (dl=%p)\n", - dl); + LOGDL(dl, LOGL_ERROR, "unsolicited supervisory response!\n"); mdl_error(MDL_CAUSE_UNSOL_SPRV_RESP, lctx); } } else if (lctx->cr == dl->cr.rem2loc.resp && lctx->p_f) { - LOGP(DLLAPD, LOGL_INFO, "REJ poll response in timer " - "recovery state received (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ poll response in timer recovery state received\n"); /* Clear an existing peer receiver busy condition */ dl->peer_busy = 0; /* V(S) and V(A) to the N(R) in the REJ frame */ dl->v_send = dl->v_ack = lctx->n_recv; + /* Flush pending messages in TX queue. */ + lapd_dl_flush_tx_queue(dl); /* stop Timer T200 */ lapd_stop_t200(dl); /* 5.5.7 Clear timer recovery condition */ @@ -1442,15 +1527,14 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) dl->peer_busy = 0; /* V(S) and V(A) to the N(R) in the REJ frame */ dl->v_send = dl->v_ack = lctx->n_recv; + /* Flush pending messages in TX queue. */ + lapd_dl_flush_tx_queue(dl); /* 5.5.3.2 */ if (lctx->cr == dl->cr.rem2loc.cmd && lctx->p_f) { if (!dl->own_busy && !dl->seq_err_cond) { - LOGP(DLLAPD, LOGL_INFO, "REJ poll " - "command in timer recovery " - "state and not in own busy " - "condition received, so we " - "respond with RR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ poll command in timer recovery " + "state and not in own busy condition received, so we " + "respond with RR final response\n"); lapd_send_rr(lctx, 1, 0); /* NOTE: In case of sequence error * condition, the REJ frame has been @@ -1459,30 +1543,25 @@ static int lapd_rx_s(struct msgb *msg, struct lapd_msg_ctx *lctx) * here */ } else if (dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, "REJ poll " - "command in timer recovery " - "state and in own busy " - "condition received, so we " - "respond with RNR final " - "response (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ poll command in timer recovery " + "state and in own busy condition received, so we " + "respond with RNR final response\n"); lapd_send_rnr(lctx, 1, 0); } } else - LOGP(DLLAPD, LOGL_INFO, "REJ response or not " - "polling command in timer recovery " - "state received (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "REJ response or not polling command in " + "timer recovery state received\n"); } /* FIXME: 5.5.4.2 2) */ - /* Send message, if possible due to acknowledged data */ - lapd_send_i(lctx, __LINE__); + /* Send message, if possible due to acknowledged data and new V(S) and V(A). */ + lapd_send_i(dl, __LINE__, false); break; default: /* G.3.1 */ - LOGP(DLLAPD, LOGL_ERROR, - "Supervisory frame not allowed. (dl=%p)\n", dl); + LOGDL(dl, LOGL_ERROR, "Supervisory frame not allowed\n"); msgb_free(msg); mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); return -EINVAL; @@ -1499,14 +1578,16 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) uint8_t ns = lctx->n_send; int length = lctx->length; int rc; + bool i_frame_in_queue = false; + int mdl_cause = 0; - LOGP(DLLAPD, LOGL_INFO, "I received in state %s on SAPI(%u) (dl=%p)\n", - lapd_state_name(dl->state), lctx->sapi, dl); + LOGDL(dl, LOGL_INFO, "I received in state %s on SAPI(%u)\n", + lapd_state_name(dl->state), lctx->sapi); /* G.2.2 Wrong value of the C/R bit */ if (lctx->cr == dl->cr.rem2loc.resp) { - LOGP(DLLAPD, LOGL_ERROR, - "I frame response not allowed (dl=%p state %s)\n", dl, lapd_state_name(dl->state)); + LOGDL(dl, LOGL_ERROR, "I frame response not allowed (state %s)\n", + lapd_state_name(dl->state)); msgb_free(msg); mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx); return -EINVAL; @@ -1517,8 +1598,8 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) * to a numerical value L>N201 or L=0, an MDL-ERROR-INDICATION * primitive with cause "I frame with incorrect length" * is sent to the mobile management entity. */ - LOGP(DLLAPD, LOGL_ERROR, - "I frame length not allowed (dl=%p state %s)\n", dl, lapd_state_name(dl->state)); + LOGDL(dl, LOGL_ERROR, "I frame length not allowed (state %s)\n", + lapd_state_name(dl->state)); msgb_free(msg); mdl_error(MDL_CAUSE_IFRM_INC_LEN, lctx); return -EIO; @@ -1529,8 +1610,8 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) * cause "I frame with incorrect use of M bit" is sent to the * mobile management entity. */ if (lctx->more && length < lctx->n201) { - LOGP(DLLAPD, LOGL_ERROR, - "I frame with M bit too short (dl=%p state %s)\n", dl, lapd_state_name(dl->state)); + LOGDL(dl, LOGL_ERROR, "I frame with M bit too short (state %s)\n", + lapd_state_name(dl->state)); msgb_free(msg); mdl_error(MDL_CAUSE_IFRM_INC_MBITS, lctx); return -EIO; @@ -1545,21 +1626,19 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) /* fall though */ case LAPD_STATE_SABM_SENT: case LAPD_STATE_DISC_SENT: - LOGP(DLLAPD, LOGL_NOTICE, - "I frame ignored in state %s (dl=%p)\n", lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_NOTICE, "I frame ignored in state %s\n", lapd_state_name(dl->state)); msgb_free(msg); return 0; } /* 5.7.1: N(s) sequence error */ if (ns != dl->v_recv) { - LOGP(DLLAPD, LOGL_NOTICE, "N(S) sequence error: N(S)=%u, " - "V(R)=%u (dl=%p state %s)\n", ns, dl->v_recv, dl, lapd_state_name(dl->state)); + LOGDL(dl, LOGL_NOTICE, "N(S) sequence error: N(S)=%u, V(R)=%u (state %s)\n", + ns, dl->v_recv, lapd_state_name(dl->state)); /* discard data */ msgb_free(msg); - if (dl->seq_err_cond != 1) { - /* FIXME: help me understand what exactly todo here - */ + /* Send reject, but suppress second reject if LAPD_F_DROP_2ND_REJ flag is set. */ + if (dl->seq_err_cond != 1 || !(dl->lapd_flags & LAPD_F_DROP_2ND_REJ)) { dl->seq_err_cond = 1; lapd_send_rej(lctx, lctx->p_f); } else { @@ -1577,10 +1656,12 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) } /* Even if N(s) sequence error, acknowledge to N(R)-1 */ /* 5.5.3.1: Acknowlege all transmitted frames up the N(R)-1 */ - lapd_acknowledge(lctx); /* V(A) is also set here */ + mdl_cause = lapd_acknowledge(lctx); /* V(A) is also set here */ + if (mdl_cause < 0) + mdl_error(-mdl_cause, lctx); /* Send message, if possible due to acknowledged data */ - lapd_send_i(lctx, __LINE__); + lapd_send_i(dl, __LINE__, false); return 0; } @@ -1588,18 +1669,23 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) /* Increment receiver state */ dl->v_recv = inc_mod(dl->v_recv, dl->v_range); - LOGP(DLLAPD, LOGL_INFO, "incrementing V(R) to %u (dl=%p)\n", - dl->v_recv, dl); + LOGDL(dl, LOGL_INFO, "incrementing V(R) to %u\n", dl->v_recv); + + /* Update all pending frames in the queue to the new V(R) state. */ + if (dl->update_pending_frames) { + rc = dl->update_pending_frames(lctx); + if (!rc) + i_frame_in_queue = true; + } /* 5.5.3.1: Acknowlege all transmitted frames up the the N(R)-1 */ - lapd_acknowledge(lctx); /* V(A) is also set here */ + mdl_cause = lapd_acknowledge(lctx); /* V(A) is also set here */ /* Only if we are not in own receiver busy condition */ if (!dl->own_busy) { /* if the frame carries a complete segment */ if (!lctx->more && !dl->rcv_buffer) { - LOGP(DLLAPD, LOGL_INFO, - "message in single I frame (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "message in single I frame\n"); /* send a DATA INDICATION to L3 */ msgb_trim(msg, length); rc = send_dl_l3(PRIM_DL_DATA, PRIM_OP_INDICATION, lctx, @@ -1607,50 +1693,51 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) } else { /* create rcv_buffer */ if (!dl->rcv_buffer) { - LOGP(DLLAPD, LOGL_INFO, "message in multiple " - "I frames (first message) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "message in multiple I frames (first message)\n"); dl->rcv_buffer = lapd_msgb_alloc(dl->maxf, "LAPD RX"); dl->rcv_buffer->l3h = dl->rcv_buffer->data; } /* concat. rcv_buffer */ if (msgb_l3len(dl->rcv_buffer) + length > dl->maxf) { - LOGP(DLLAPD, LOGL_NOTICE, "Received frame " - "overflow! (dl=%p)\n", dl); + LOGDL(dl, LOGL_NOTICE, "Received frame overflow!\n"); } else { memcpy(msgb_put(dl->rcv_buffer, length), msg->l3h, length); } /* if the last segment was received */ if (!lctx->more) { - LOGP(DLLAPD, LOGL_INFO, "message in multiple " - "I frames (last message) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "message in multiple I frames (last message)\n"); rc = send_dl_l3(PRIM_DL_DATA, PRIM_OP_INDICATION, lctx, dl->rcv_buffer); dl->rcv_buffer = NULL; } else - LOGP(DLLAPD, LOGL_INFO, "message in multiple " - "I frames (next message) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "message in multiple I frames (next message)\n"); msgb_free(msg); } + /* the L3 or higher (called in-line above via send_dl_l3) might have destroyed the + * data link meanwhile. See OS#1761 */ + if (dl->state == LAPD_STATE_NULL) + return 0; } else - LOGP(DLLAPD, LOGL_INFO, "I frame ignored during own receiver " - "busy condition\n"); + LOGDL(dl, LOGL_INFO, "I frame ignored during own receiver busy condition\n"); + + /* Indicate sequence error, if exists. */ + if (mdl_cause < 0) + mdl_error(-mdl_cause, lctx); /* Check for P bit */ if (lctx->p_f) { /* 5.5.2.1 */ /* check if we are not in own receiver busy */ if (!dl->own_busy) { - LOGP(DLLAPD, LOGL_INFO, - "we are not busy, send RR (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "we are not busy, send RR\n"); /* Send RR with F=1 */ rc = lapd_send_rr(lctx, 1, 0); } else { - LOGP(DLLAPD, LOGL_INFO, - "we are busy, send RNR (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "we are busy, send RNR\n"); /* Send RNR with F=1 */ rc = lapd_send_rnr(lctx, 1, 0); } @@ -1659,32 +1746,24 @@ static int lapd_rx_i(struct msgb *msg, struct lapd_msg_ctx *lctx) /* check if we are not in own receiver busy */ if (!dl->own_busy) { /* NOTE: V(R) is already set above */ - rc = lapd_send_i(lctx, __LINE__); - - /* if update_pending_iframe returns 0 it updated - * the lapd header of an iframe in the tx queue */ - if (rc && dl->update_pending_frames) - rc = dl->update_pending_frames(lctx); - - if (rc) { - LOGP(DLLAPD, LOGL_INFO, "we are not busy and " - "have no pending data, send RR (dl=%p)\n", - dl); + rc = lapd_send_i(dl, __LINE__, false); + if (rc && !i_frame_in_queue) { + LOGDL(dl, LOGL_INFO, "we are not busy and have no pending data, " + "send RR\n"); /* Send RR with F=0 */ return lapd_send_rr(lctx, 0, 0); } /* all I or one RR is sent, we are done */ return 0; } else { - LOGP(DLLAPD, LOGL_INFO, - "we are busy, send RNR (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "we are busy, send RNR\n"); /* Send RNR with F=0 */ rc = lapd_send_rnr(lctx, 0, 0); } } /* Send message, if possible due to acknowledged data */ - lapd_send_i(lctx, __LINE__); + lapd_send_i(dl, __LINE__, false); return rc; } @@ -1705,14 +1784,38 @@ int lapd_ph_data_ind(struct msgb *msg, struct lapd_msg_ctx *lctx) rc = lapd_rx_i(msg, lctx); break; default: - LOGP(DLLAPD, LOGL_NOTICE, - "unknown LAPD format (dl=%p)\n", lctx->dl); + LOGDL(lctx->dl, LOGL_NOTICE, "unknown LAPD format 0x%02x\n", lctx->format); msgb_free(msg); rc = -EINVAL; } return rc; } +/*! Enqueue next LAPD frame and run pending T200. (Must be called when frame is ready to send.) + * The caller (LAPDm code) calls this function before it sends the next frame. + * If there is no frame in the TX queue, LAPD will enqueue next I-frame, if possible. + * If the T200 is pending, it is changed to running state. + * \param[in] lctx LAPD context + * \param[out] rc set to 1, if timer T200 state changed to running, set to 0, if not. */ +int lapd_ph_rts_ind(struct lapd_msg_ctx *lctx) +{ + struct lapd_datalink *dl = lctx->dl; + + /* If there is no pending frame, try to enqueue next I frame. */ + if (llist_empty(&dl->tx_queue) && (dl->state == LAPD_STATE_MF_EST || dl->state == LAPD_STATE_TIMER_RECOV)) { + /* Send an I frame, if there are pending outgoing messages. */ + lapd_send_i(dl, __LINE__, true); + } + + /* Run T200 at RTS, if pending. Tell caller that is has been started. (rc = 1) */ + if (dl->t200_rts == LAPD_T200_RTS_PENDING) { + dl->t200_rts = LAPD_T200_RTS_RUNNING; + return 1; + } + + return 0; +} + /* L3 -> L2 */ /* send unit data */ @@ -1736,6 +1839,20 @@ static int lapd_udata_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) return dl->send_ph_data_req(&nctx, msg); } +static void msg_to_tx_hist(struct lapd_history *tx_hist, const struct msgb *msg, int length, int more) +{ + tx_hist->msg = lapd_msgb_alloc(msg->len, "HIST"); + tx_hist->more = more; + msgb_put(tx_hist->msg, msg->len); + if (length) + memcpy(tx_hist->msg->data, msg->l3h, msg->len); +} + +static void msg_to_tx_hist0(struct lapd_datalink *dl, const struct msgb *msg) +{ + return msg_to_tx_hist(&dl->tx_hist[0], msg, msg->len, 0); +} + /* request link establishment */ static int lapd_est_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) { @@ -1744,11 +1861,9 @@ static int lapd_est_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) struct lapd_msg_ctx nctx; if (msg->len) - LOGP(DLLAPD, LOGL_INFO, "perform establishment with content " - "(SABM) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "perform establishment with content (SABM)\n"); else - LOGP(DLLAPD, LOGL_INFO, - "perform normal establishm. (SABM), (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "perform normal establishm. (SABM)\n"); /* Flush send-queue */ /* Clear send-buffer */ @@ -1776,11 +1891,8 @@ static int lapd_est_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) nctx.more = 0; /* Transmit-buffer carries exactly one segment */ - dl->tx_hist[0].msg = lapd_msgb_alloc(msg->len, "HIST"); - msgb_put(dl->tx_hist[0].msg, msg->len); - if (msg->len) - memcpy(dl->tx_hist[0].msg->data, msg->l3h, msg->len); - dl->tx_hist[0].more = 0; + msg_to_tx_hist0(dl, msg); + /* set Vs to 0, because it is used as index when resending SABM */ dl->v_send = 0; @@ -1803,29 +1915,26 @@ static int lapd_data_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) struct msgb *msg = dp->oph.msg; if (msgb_l3len(msg) == 0) { - LOGP(DLLAPD, LOGL_ERROR, - "writing an empty message is not possible. (dl=%p)\n", dl); + LOGDL(dl, LOGL_ERROR, "writing an empty message is not possible\n"); msgb_free(msg); return -1; } - LOGP(DLLAPD, LOGL_INFO, - "writing message to send-queue: l3len: %d (dl=%p)\n", - msgb_l3len(msg), dl); + LOGDL(dl, LOGL_INFO, "writing message to send-queue: l3len: %d\n", msgb_l3len(msg)); /* Write data into the send queue */ msgb_enqueue(&dl->send_queue, msg); /* Send message, if possible */ - lapd_send_i(&dl->lctx, __LINE__); + lapd_send_i(dl, __LINE__, false); return 0; } /* Send next I frame from queued/buffered data */ -static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) +static int lapd_send_i(struct lapd_datalink *dl, int line, bool rts) { - struct lapd_datalink *dl = lctx->dl; + struct lapd_msg_ctx *lctx = &dl->lctx; uint8_t k = dl->k; uint8_t h; struct msgb *msg; @@ -1833,20 +1942,26 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) int rc = - 1; /* we sent nothing */ struct lapd_msg_ctx nctx; + if (!rts) + LOGDL(dl, LOGL_INFO, "%s() called from line %d\n", __func__, line); - LOGP(DLLAPD, LOGL_INFO, - "%s() called from line %d (dl=%p)\n", __func__, line, dl); + if ((dl->lapd_flags & LAPD_F_RTS) && !llist_empty(&dl->tx_queue)) { + if (!rts) + LOGDL(dl, LOGL_INFO, "There is a frame in the TX queue, not checking for sending I frame.\n"); + return rc; + } next_frame: if (dl->peer_busy) { - LOGP(DLLAPD, LOGL_INFO, "peer busy, not sending (dl=%p)\n", dl); + if (!rts) + LOGDL(dl, LOGL_INFO, "Peer busy, not sending.\n"); return rc; } if (dl->state == LAPD_STATE_TIMER_RECOV) { - LOGP(DLLAPD, LOGL_INFO, - "timer recovery, not sending (dl=%p)\n", dl); + if (!rts) + LOGDL(dl, LOGL_INFO, "Timer recovery, not sending.\n"); return rc; } @@ -1857,9 +1972,9 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) * of the error recovery procedures as described in subclauses 5.5.4 and * 5.5.7. */ if (dl->v_send == add_mod(dl->v_ack, k, dl->v_range)) { - LOGP(DLLAPD, LOGL_INFO, "k frames outstanding, not sending " - "more (k=%u V(S)=%u V(A)=%u) (dl=%p)\n", k, dl->v_send, - dl->v_ack, dl); + if (!rts) + LOGDL(dl, LOGL_INFO, "k frames outstanding, not sending more. (k=%u V(S)=%u V(A)=%u)\n", + k, dl->v_send, dl->v_ack); return rc; } @@ -1875,8 +1990,7 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) /* No more data to be sent */ if (!dl->send_buffer) return rc; - LOGP(DLLAPD, LOGL_INFO, "get message from " - "send-queue (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "get message from send-queue\n"); } /* How much is left in the send-buffer? */ @@ -1885,10 +1999,9 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) length = left; if (length > lctx->n201) length = lctx->n201; - LOGP(DLLAPD, LOGL_INFO, "msg-len %d sent %d left %d N201 %d " - "length %d first byte %02x (dl=%p)\n", - msgb_l3len(dl->send_buffer), dl->send_out, left, - lctx->n201, length, dl->send_buffer->l3h[0], dl); + LOGDL(dl, LOGL_INFO, "msg-len %d sent %d left %d N201 %d length %d " + "first byte %02x\n", msgb_l3len(dl->send_buffer), dl->send_out, left, + lctx->n201, length, dl->send_buffer->l3h[0]); /* If message in send-buffer is completely sent */ if (left == 0) { msgb_free(dl->send_buffer); @@ -1896,14 +2009,14 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) goto next_message; } - LOGP(DLLAPD, LOGL_INFO, "send I frame %sV(S)=%d (dl=%p)\n", - (left > length) ? "segment " : "", dl->v_send, dl); + LOGDL(dl, LOGL_INFO, "send I frame %sV(S)=%d\n", + (left > length) ? "segment " : "", dl->v_send); /* Create I frame (segment) and transmit-buffer content */ msg = lapd_msgb_alloc(length, "LAPD I"); msg->l3h = msgb_put(msg, length); /* assemble message */ - memcpy(&nctx, &dl->lctx, sizeof(nctx)); + memcpy(&nctx, lctx, sizeof(nctx)); /* keep nctx.ldp */ /* keep nctx.sapi */ /* keep nctx.tei */ @@ -1921,23 +2034,19 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) memcpy(msg->l3h, dl->send_buffer->l3h + dl->send_out, length); /* store in tx_hist */ - dl->tx_hist[h].msg = lapd_msgb_alloc(msg->len, "HIST"); - msgb_put(dl->tx_hist[h].msg, msg->len); - if (length) - memcpy(dl->tx_hist[h].msg->data, msg->l3h, msg->len); - dl->tx_hist[h].more = nctx.more; + msg_to_tx_hist(&dl->tx_hist[h], msg, length, nctx.more); + /* Add length to track how much is already in the tx buffer */ dl->send_out += length; } else { - LOGP(DLLAPD, LOGL_INFO, "resend I frame from tx buffer " - "V(S)=%d (dl=%p)\n", dl->v_send, dl); + LOGDL(dl, LOGL_INFO, "resend I frame from tx buffer V(S)=%d\n", dl->v_send); /* Create I frame (segment) from tx_hist */ length = dl->tx_hist[h].msg->len; msg = lapd_msgb_alloc(length, "LAPD I resend"); msg->l3h = msgb_put(msg, length); /* assemble message */ - memcpy(&nctx, &dl->lctx, sizeof(nctx)); + memcpy(&nctx, lctx, sizeof(nctx)); /* keep nctx.ldp */ /* keep nctx.sapi */ /* keep nctx.tei */ @@ -1959,7 +2068,7 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) /* If timer T200 is not running at the time right before transmitting a * frame, when the PH-READY-TO-SEND primitive is received from the * physical layer., it shall be set. */ - if (!osmo_timer_pending(&dl->t200)) { + if (!lapd_is_t200_started(dl)) { /* stop Timer T203, if running */ lapd_stop_t203(dl); /* start Timer T200 */ @@ -1968,7 +2077,11 @@ static int lapd_send_i(struct lapd_msg_ctx *lctx, int line) dl->send_ph_data_req(&nctx, msg); - rc = 0; /* we sent something */ + /* When using RTS, we send only one frame. */ + if ((dl->lapd_flags & LAPD_F_RTS)) + return 0; + + rc = 0; /* We sent an I frame, so sending RR frame is not required. */ goto next_frame; } @@ -1978,16 +2091,15 @@ static int lapd_susp_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) struct lapd_datalink *dl = lctx->dl; struct msgb *msg = dp->oph.msg; - LOGP(DLLAPD, LOGL_INFO, "perform suspension (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "perform suspension\n"); /* put back the send-buffer to the send-queue (first position) */ if (dl->send_buffer) { - LOGP(DLLAPD, LOGL_INFO, "put frame in sendbuffer back to " - "queue (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "put frame in sendbuffer back to queue\n"); llist_add(&dl->send_buffer->list, &dl->send_queue); dl->send_buffer = NULL; } else - LOGP(DLLAPD, LOGL_INFO, "no frame in sendbuffer (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "no frame in sendbuffer\n"); /* Clear transmit buffer, but keep send buffer */ lapd_dl_flush_tx(dl); @@ -2007,9 +2119,7 @@ static int lapd_res_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) struct msgb *msg = dp->oph.msg; struct lapd_msg_ctx nctx; - LOGP(DLLAPD, LOGL_INFO, - "perform re-establishment (SABM) length=%d (dl=%p)\n", - msg->len, dl); + LOGDL(dl, LOGL_INFO, "perform re-establishment (SABM) length=%d\n", msg->len); /* be sure that history is empty */ lapd_dl_flush_hist(dl); @@ -2050,11 +2160,8 @@ static int lapd_res_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) nctx.length = 0; nctx.more = 0; - dl->tx_hist[0].msg = lapd_msgb_alloc(msg->len, "HIST"); - msgb_put(dl->tx_hist[0].msg, msg->len); - if (msg->len) - memcpy(dl->tx_hist[0].msg->data, msg->l3h, msg->len); - dl->tx_hist[0].more = 0; + msg_to_tx_hist0(dl, msg); + /* set Vs to 0, because it is used as index when resending SABM */ dl->v_send = 0; @@ -2079,7 +2186,7 @@ static int lapd_rel_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) /* local release */ if (dp->u.rel_req.mode) { - LOGP(DLLAPD, LOGL_INFO, "perform local release (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "perform local release\n"); msgb_free(msg); /* stop Timer T200 */ lapd_stop_t200(dl); @@ -2099,7 +2206,7 @@ static int lapd_rel_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) /* flush tx_hist */ lapd_dl_flush_hist(dl); - LOGP(DLLAPD, LOGL_INFO, "perform normal release (DISC) (dl=%p)\n", dl); + LOGDL(dl, LOGL_INFO, "perform normal release (DISC)\n"); /* Push LAPD header on msgb */ /* assemble message */ @@ -2114,11 +2221,8 @@ static int lapd_rel_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) nctx.length = 0; nctx.more = 0; - dl->tx_hist[0].msg = lapd_msgb_alloc(msg->len, "HIST"); - msgb_put(dl->tx_hist[0].msg, msg->len); - if (msg->len) - memcpy(dl->tx_hist[0].msg->data, msg->l3h, msg->len); - dl->tx_hist[0].more = 0; + msg_to_tx_hist0(dl, msg); + /* set Vs to 0, because it is used as index when resending DISC */ dl->v_send = 0; @@ -2224,22 +2328,20 @@ int lapd_recv_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx) } } if (!supported) { - LOGP(DLLAPD, LOGL_NOTICE, - "Message %u/%u unsupported. (dl=%p)\n", dp->oph.primitive, - dp->oph.operation, dl); + LOGDL(dl, LOGL_NOTICE, "Message %u/%u unsupported\n", + dp->oph.primitive, dp->oph.operation); msgb_free(msg); return 0; } if (i == L2DOWNSLLEN) { - LOGP(DLLAPD, LOGL_NOTICE, "Message %u/%u unhandled at this " - "state %s. (dl=%p)\n", dp->oph.primitive, - dp->oph.operation, lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_NOTICE, "Message %u/%u unhandled at this state %s\n", + dp->oph.primitive, dp->oph.operation, lapd_state_name(dl->state)); msgb_free(msg); return 0; } - LOGP(DLLAPD, LOGL_INFO, "Message %s received in state %s (dl=%p)\n", - l2downstatelist[i].name, lapd_state_name(dl->state), dl); + LOGDL(dl, LOGL_INFO, "Message %s received in state %s\n", + l2downstatelist[i].name, lapd_state_name(dl->state)); rc = l2downstatelist[i].rout(dp, lctx); diff --git a/src/isdn/libosmoisdn.map b/src/isdn/libosmoisdn.map new file mode 100644 index 00000000..8f34f0d8 --- /dev/null +++ b/src/isdn/libosmoisdn.map @@ -0,0 +1,52 @@ +LIBOSMOISDN_1.0 { +global: + +tall_lapd_ctx; +lapd_dl_exit; +lapd_dl_init; +lapd_dl_init2; +lapd_dl_set_name; +lapd_dl_reset; +lapd_dl_set_flags; +lapd_msgb_alloc; +lapd_ph_data_ind; +lapd_ph_rts_ind; +lapd_recv_dlsap; +lapd_set_mode; +lapd_state_names; +lapd_t200_timeout; + +osmo_i460_demux_in; +osmo_i460_mux_enqueue; +osmo_i460_mux_out; +osmo_i460_subchan_add; +osmo_i460_subchan_del; +osmo_i460_subchan_count; +osmo_i460_ts_init; + +osmo_v110_decode_frame; +osmo_v110_encode_frame; +osmo_v110_ubit_dump; +osmo_v110_e1e2e3; +osmo_v110_sync_ra1_get_user_data_chunk_bitlen; +osmo_v110_sync_ra1_get_user_data_rate; +osmo_v110_sync_ra1_get_intermediate_rate; +osmo_v110_sync_ra1_user_to_ir; +osmo_v110_sync_ra1_ir_to_user; + +osmo_v110_ta_alloc; +osmo_v110_ta_free; +osmo_v110_ta_set_timer_val_ms; +osmo_v110_ta_frame_in; +osmo_v110_ta_frame_out; +osmo_v110_ta_sync_ind; +osmo_v110_ta_desync_ind; +osmo_v110_ta_get_status; +osmo_v110_ta_get_circuit; +osmo_v110_ta_set_circuit; + +osmo_v110_ta_circuit_names; +osmo_v110_ta_circuit_descs; + +local: *; +}; diff --git a/src/isdn/v110.c b/src/isdn/v110.c new file mode 100644 index 00000000..b4bc4e30 --- /dev/null +++ b/src/isdn/v110.c @@ -0,0 +1,590 @@ +/* V.110 frames according to ITU-T V.110 + * + * This code implements the following functionality: + * - parsing/encoding of osmo_v110_decoded_frame from/to actual 80-bit V.110 frame + * - synchronous rate adapting of user bit rate to V.110 D-bits as per Table 6 + * + * It is (at least initially) a very "naive" implementation, as it first and foremost + * aims to be functional and correct, rather than efficient in any way. Hence it + * operates on unpacked bits (ubit_t, 1 bit per byte), and has various intermediate + * representations and indirect function calls. If needed, a more optimized variant + * can always be developed later on. + */ + +/* (C) 2022 by Harald Welte <laforge@osmocom.org> + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + */ + +#include <stdint.h> +#include <errno.h> + +#include <osmocom/core/bits.h> + +#include <osmocom/isdn/v110.h> + +/************************************************************************* + * V.110 frame decoding/encoding (ubits <-> struct with D/S/X/E bits) + *************************************************************************/ + +/*! Decode a 80-bit V.110 frame present as 80 ubits into a struct osmo_v110_decoded_frame. + * \param[out] fr caller-allocated output data structure, filled by this function + * \param[in] ra_bits One V.110 frame as 80 unpacked bits. + * \param[in] n_bits number of unpacked bits provided in ra_bits + * \returns 0 in case of success; negative on error. */ +int osmo_v110_decode_frame(struct osmo_v110_decoded_frame *fr, const ubit_t *ra_bits, size_t n_bits) +{ + if (n_bits < 80) + return -EINVAL; + + /* X1 .. X2 */ + fr->x_bits[0] = ra_bits[2 * 8 + 7]; + fr->x_bits[1] = ra_bits[7 * 8 + 7]; + + /* S1, S3, S4, S6, S8, S9 */ + fr->s_bits[0] = ra_bits[1 * 8 + 7]; + fr->s_bits[2] = ra_bits[3 * 8 + 7]; + fr->s_bits[3] = ra_bits[4 * 8 + 7]; + fr->s_bits[5] = ra_bits[6 * 8 + 7]; + fr->s_bits[7] = ra_bits[8 * 8 + 7]; + fr->s_bits[8] = ra_bits[9 * 8 + 7]; + + /* E1 .. E7 */ + memcpy(fr->e_bits, ra_bits + 5 * 8 + 1, 7); + + /* D-bits */ + memcpy(fr->d_bits + 0 * 6, ra_bits + 1 * 8 + 1, 6); + memcpy(fr->d_bits + 1 * 6, ra_bits + 2 * 8 + 1, 6); + memcpy(fr->d_bits + 2 * 6, ra_bits + 3 * 8 + 1, 6); + memcpy(fr->d_bits + 3 * 6, ra_bits + 4 * 8 + 1, 6); + + memcpy(fr->d_bits + 4 * 6, ra_bits + 6 * 8 + 1, 6); + memcpy(fr->d_bits + 5 * 6, ra_bits + 7 * 8 + 1, 6); + memcpy(fr->d_bits + 6 * 6, ra_bits + 8 * 8 + 1, 6); + memcpy(fr->d_bits + 7 * 6, ra_bits + 9 * 8 + 1, 6); + + return 0; +} + +/*! Encode a struct osmo_v110_decoded_frame into an 80-bit V.110 frame as ubits. + * \param[out] ra_bits caller-provided output buffer at leat 80 ubits large + * \param[in] n_bits length of ra_bits. Must be at least 80. + * \param[in] input data structure + * \returns number of bits written to ra_bits */ +int osmo_v110_encode_frame(ubit_t *ra_bits, size_t n_bits, const struct osmo_v110_decoded_frame *fr) +{ + if (n_bits < 80) + return -ENOSPC; + + /* alignment pattern */ + memset(ra_bits+0, 0, 8); + for (int i = 1; i < 10; i++) + ra_bits[i*8] = 1; + + /* X1 .. X2 */ + ra_bits[2 * 8 + 7] = fr->x_bits[0]; + ra_bits[7 * 8 + 7] = fr->x_bits[1]; + + /* S1, S3, S4, S6, S8, S9 */ + ra_bits[1 * 8 + 7] = fr->s_bits[0]; + ra_bits[3 * 8 + 7] = fr->s_bits[2]; + ra_bits[4 * 8 + 7] = fr->s_bits[3]; + ra_bits[6 * 8 + 7] = fr->s_bits[5]; + ra_bits[8 * 8 + 7] = fr->s_bits[7]; + ra_bits[9 * 8 + 7] = fr->s_bits[8]; + + /* E1 .. E7 */ + memcpy(ra_bits + 5 * 8 + 1, fr->e_bits, 7); + + /* D-bits */ + memcpy(ra_bits + 1 * 8 + 1, fr->d_bits + 0 * 6, 6); + memcpy(ra_bits + 2 * 8 + 1, fr->d_bits + 1 * 6, 6); + memcpy(ra_bits + 3 * 8 + 1, fr->d_bits + 2 * 6, 6); + memcpy(ra_bits + 4 * 8 + 1, fr->d_bits + 3 * 6, 6); + + memcpy(ra_bits + 6 * 8 + 1, fr->d_bits + 4 * 6, 6); + memcpy(ra_bits + 7 * 8 + 1, fr->d_bits + 5 * 6, 6); + memcpy(ra_bits + 8 * 8 + 1, fr->d_bits + 6 * 6, 6); + memcpy(ra_bits + 9 * 8 + 1, fr->d_bits + 7 * 6, 6); + + return 10 * 8; +} + +/*! Print a encoded V.110 frame in the same table-like structure as the spec. + * \param outf output FILE stream to which to dump + * \param[in] fr unpacked bits to dump + * \param[in] in_len length of unpacked bits available at fr. */ +void osmo_v110_ubit_dump(FILE *outf, const ubit_t *fr, size_t in_len) +{ + if (in_len < 80) + fprintf(outf, "short input data\n"); + + for (unsigned int octet = 0; octet < 10; octet++) { + fprintf(outf, "%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", + fr[octet * 8 + 0], fr[octet * 8 + 1], fr[octet * 8 + 2], fr[octet * 8 + 3], + fr[octet * 8 + 4], fr[octet * 8 + 5], fr[octet * 8 + 6], fr[octet * 8 + 7]); + } +} + +/************************************************************************* + * RA1 synchronous rate adaptation + *************************************************************************/ + +/* I actually couldn't find any reference as to the value of F(ill) bits */ +#define F 1 + +/*! E1/E2/E3 bit values as per Table 5/V.110 */ +const ubit_t osmo_v110_e1e2e3[_NUM_OSMO_V110_SYNC_RA1][3] = { + [OSMO_V110_SYNC_RA1_600] = { 1, 0, 0 }, + [OSMO_V110_SYNC_RA1_1200] = { 0, 1, 0 }, + [OSMO_V110_SYNC_RA1_2400] = { 1, 1, 0 }, + [OSMO_V110_SYNC_RA1_4800] = { 0, 1, 1 }, + [OSMO_V110_SYNC_RA1_7200] = { 1, 0, 1 }, + [OSMO_V110_SYNC_RA1_9600] = { 0, 1, 1 }, + [OSMO_V110_SYNC_RA1_12000] = { 0, 0, 1 }, + [OSMO_V110_SYNC_RA1_14400] = { 1, 0, 1 }, + [OSMO_V110_SYNC_RA1_19200] = { 0, 1, 1 }, + [OSMO_V110_SYNC_RA1_24000] = { 0, 0, 1 }, + [OSMO_V110_SYNC_RA1_28800] = { 1, 0, 1 }, + [OSMO_V110_SYNC_RA1_38400] = { 0, 1, 1 }, +}; + +/*! Adapt from 6 synchronous 600bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 6. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_600_to_IR8000(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + if (in_len != 6) + return -EINVAL; + + /* Table 6a / V.110 */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_600); + for (int i = 0; i < 6; i++) + memset(fr->d_bits + i*8, d_in[i], 8); + + return 0; +} + +static int v110_adapt_IR8000_to_600(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + if (out_len < 6) + return -ENOSPC; + + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_600)) + return -EINVAL; + + for (int i = 0; i < 6; i++) { + /* we only use one of the bits, not some kind of consistency check or majority vote */ + d_out[i] = fr->d_bits[i*8]; + } + + return 6; +} + +/*! Adapt from 12 synchronous 1200bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 12. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_1200_to_IR8000(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + if (in_len != 12) + return -EINVAL; + + /* Table 6b / V.110 */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_1200); + for (int i = 0; i < 12; i++) + memset(fr->d_bits + i*4, d_in[i], 4); + + return 0; +} + +static int v110_adapt_IR8000_to_1200(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + if (out_len < 12) + return -ENOSPC; + + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_1200)) + return -EINVAL; + + for (int i = 0; i < 12; i++) { + /* we only use one of the bits, not some kind of consistency check or majority vote */ + d_out[i] = fr->d_bits[i*4]; + } + + return 12; +} + +/*! Adapt from 24 synchronous 2400bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 24. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_2400_to_IR8000(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + if (in_len != 24) + return -EINVAL; + + /* Table 6c / V.110 */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_2400); + for (int i = 0; i < 24; i++) { + fr->d_bits[i*2 + 0] = d_in[i]; + fr->d_bits[i*2 + 1] = d_in[i]; + } + + return 0; +} + +static int v110_adapt_IR8000_to_2400(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + if (out_len < 24) + return -ENOSPC; + + /* Table 6c / V.110 */ + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_2400)) + return -EINVAL; + + for (int i = 0; i < 24; i++) { + /* we only use one of the bits, not some kind of consistency check or majority vote */ + d_out[i] = fr->d_bits[i*2]; + } + + return 24; +} + +/*! Adapt from 36 synchronous N x 3600bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 36. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_Nx3600_to_IR(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + int d_idx = 0; + + if (in_len != 36) + return -EINVAL; + + /* Table 6d / V.110 (7200 is one of Nx3600) */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_7200); + + memcpy(fr->d_bits + d_idx, d_in + 0, 10); d_idx += 10; /* D1..D10 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 10, 2); d_idx += 2; /* D11..D12 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 12, 2); d_idx += 2; /* D13..D14 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 14, 14); d_idx += 14; /* D15..D28 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 28, 2); d_idx += 2; /* D29..D30 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 30, 2); d_idx += 2; /* D31..D32 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 32, 4); d_idx += 4; /* D33..D36 */ + + OSMO_ASSERT(d_idx == 48); + + return 0; +} + +static int v110_adapt_IR_to_Nx3600(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + int d_idx = 0; + + if (out_len < 36) + return -ENOSPC; + + /* Table 6d / V.110 (7200 is one of Nx3600) */ + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_7200)) + return -EINVAL; + + memcpy(d_out + 0, fr->d_bits + d_idx, 10); d_idx += 10; /* D1..D10 */ + d_idx += 2; + memcpy(d_out + 10, fr->d_bits + d_idx, 2); d_idx += 2; /* D11..D12 */ + d_idx += 2; + memcpy(d_out + 12, fr->d_bits + d_idx, 2); d_idx += 2; /* D13..D14 */ + d_idx += 2; + memcpy(d_out + 14, fr->d_bits + d_idx, 14); d_idx += 14;/* D15..D28 */ + d_idx += 2; + memcpy(d_out + 28, fr->d_bits + d_idx, 2); d_idx += 2; /* D29..D30 */ + d_idx += 2; + memcpy(d_out + 30, fr->d_bits + d_idx, 2); d_idx += 2; /* D31..D32 */ + d_idx += 2; + memcpy(d_out + 32, fr->d_bits + d_idx, 4); d_idx += 4; /* D33..D36 */ + + OSMO_ASSERT(d_idx == 48); + + return 36; +} + + +/*! Adapt from 48 synchronous N x 4800bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 48. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_Nx4800_to_IR(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + if (in_len != 48) + return -EINVAL; + + /* Table 6e / V.110 (4800 is one of Nx4800) */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_4800); + + memcpy(fr->d_bits, d_in, 48); + + return 0; +} + +static int v110_adapt_IR_to_Nx4800(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + if (out_len < 48) + return -ENOSPC; + + /* Table 6e / V.110 (4800 is one of Nx4800) */ + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_4800)) + return -EINVAL; + + memcpy(d_out, fr->d_bits, 48); + + return 48; +} + +/*! Adapt from 30 synchronous N x 12000bit/s input bits to a decoded V.110 frame. + * \param[out] fr caller-allocated output frame to which E+D bits are stored + * \param[in] d_in input user bits + * \param[in] in_len number of bits in d_in. Must be 30. + * \returns 0 on success; negative in case of error. */ +static int v110_adapt_Nx12000_to_IR(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len) +{ + int d_idx = 0; + + if (in_len != 30) + return -EINVAL; + + /* Table 6f / V.110 (12000 is one of Nx12000) */ + osmo_v110_e1e2e3_set(fr->e_bits, OSMO_V110_SYNC_RA1_12000); + + memcpy(fr->d_bits + d_idx, d_in + 0, 10); d_idx += 10; /* D1..D10 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 10, 2); d_idx += 2; /* D11..D12 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 12, 2); d_idx += 2; /* D13..D14 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + fr->d_bits[d_idx++] = d_in[14]; /* D15 */ + memset(fr->d_bits + d_idx, F, 3); d_idx += 3; + memcpy(fr->d_bits + d_idx, d_in + 15, 10); d_idx += 10; /* D16..D25 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 25, 2); d_idx += 2; /* D26..D27 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + memcpy(fr->d_bits + d_idx, d_in + 27, 2); d_idx += 2; /* D28..D29 */ + memset(fr->d_bits + d_idx, F, 2); d_idx += 2; + fr->d_bits[d_idx++] = d_in[29]; /* D30 */ + memset(fr->d_bits + d_idx, F, 3); d_idx += 3; + + OSMO_ASSERT(d_idx == 48); + + return 0; +} + +static int v110_adapt_IR_to_Nx12000(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr) +{ + int d_idx = 0; + + if (out_len < 30) + return -ENOSPC; + + /* Table 6f / V.110 (12000 is one of Nx12000) */ + if (osmo_v110_e1e2e3_cmp(fr->e_bits, OSMO_V110_SYNC_RA1_12000)) + return -EINVAL; + + memcpy(d_out + 0, fr->d_bits + d_idx, 10); d_idx += 10; /* D1..D10 */ + d_idx += 2; + memcpy(d_out + 10, fr->d_bits + d_idx, 2); d_idx += 2; /* D11..D12 */ + d_idx += 2; + memcpy(d_out + 12, fr->d_bits + d_idx, 2); d_idx += 2; /* D13..D14 */ + d_idx += 2; + d_out[14] = fr->d_bits[d_idx++]; /* D15 */ + d_idx += 3; + memcpy(d_out + 15, fr->d_bits + d_idx, 10); d_idx += 10;/* D16..D25 */ + d_idx += 2; + memcpy(d_out + 25, fr->d_bits + d_idx, 2); d_idx += 2; /* D26..D27 */ + d_idx += 2; + memcpy(d_out + 27, fr->d_bits + d_idx, 2); d_idx += 2; /* D28..D29 */ + d_idx += 2; + d_out[29] = fr->d_bits[d_idx++]; /* D30 */ + d_idx += 3; + + OSMO_ASSERT(d_idx == 48); + + return 30; +} + +/* definition of a synchronous V.110 RA1 rate adaptation. There is one for each supported tuple + * of user data rate and intermediate rate (IR). */ +struct osmo_v110_sync_ra1 { + unsigned int data_rate; + unsigned int intermediate_rate; + unsigned int user_data_chunk_bits; + /*! RA1 function in user bitrate -> intermediate rate direction */ + int (*adapt_user_to_ir)(struct osmo_v110_decoded_frame *fr, const ubit_t *d_in, size_t in_len); + /*! RA1 function in intermediate rate -> user bitrate direction */ + int (*adapt_ir_to_user)(ubit_t *d_out, size_t out_len, const struct osmo_v110_decoded_frame *fr); +}; + +/* all of the synchronous data signalling rates; see Table 1/V.110 */ +static const struct osmo_v110_sync_ra1 osmo_v110_sync_ra1_def[_NUM_OSMO_V110_SYNC_RA1] = { + [OSMO_V110_SYNC_RA1_600] = { + .data_rate = 600, + .intermediate_rate = 8000, + .user_data_chunk_bits = 6, + .adapt_user_to_ir = v110_adapt_600_to_IR8000, + .adapt_ir_to_user = v110_adapt_IR8000_to_600, + }, + [OSMO_V110_SYNC_RA1_1200] = { + .data_rate = 1200, + .intermediate_rate = 8000, + .user_data_chunk_bits = 12, + .adapt_user_to_ir = v110_adapt_1200_to_IR8000, + .adapt_ir_to_user = v110_adapt_IR8000_to_1200, + }, + [OSMO_V110_SYNC_RA1_2400] = { + .data_rate = 2400, + .intermediate_rate = 8000, + .user_data_chunk_bits = 24, + .adapt_user_to_ir = v110_adapt_2400_to_IR8000, + .adapt_ir_to_user = v110_adapt_IR8000_to_2400, + }, + [OSMO_V110_SYNC_RA1_4800] = { + .data_rate = 4800, + .intermediate_rate = 8000, + .user_data_chunk_bits = 48, + .adapt_user_to_ir = v110_adapt_Nx4800_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx4800, + }, + [OSMO_V110_SYNC_RA1_7200] = { + .data_rate = 7200, + .intermediate_rate = 16000, + .user_data_chunk_bits = 36, + .adapt_user_to_ir = v110_adapt_Nx3600_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx3600, + }, + [OSMO_V110_SYNC_RA1_9600] = { + .data_rate = 9600, + .intermediate_rate = 16000, + .user_data_chunk_bits = 48, + .adapt_user_to_ir = v110_adapt_Nx4800_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx4800, + }, + [OSMO_V110_SYNC_RA1_12000] = { + .data_rate = 12000, + .intermediate_rate = 32000, + .user_data_chunk_bits = 30, + .adapt_user_to_ir = v110_adapt_Nx12000_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx12000, + }, + [OSMO_V110_SYNC_RA1_14400] = { + .data_rate = 14400, + .intermediate_rate = 32000, + .user_data_chunk_bits = 36, + .adapt_user_to_ir = v110_adapt_Nx3600_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx3600, + }, + [OSMO_V110_SYNC_RA1_19200] = { + .data_rate = 19200, + .intermediate_rate = 32000, + .user_data_chunk_bits = 48, + .adapt_user_to_ir = v110_adapt_Nx4800_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx4800, + }, + [OSMO_V110_SYNC_RA1_24000] = { + .data_rate = 24000, + .intermediate_rate = 64000, + .user_data_chunk_bits = 30, + .adapt_user_to_ir = v110_adapt_Nx12000_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx12000, + }, + [OSMO_V110_SYNC_RA1_28800] = { + .data_rate = 28800, + .intermediate_rate = 64000, + .user_data_chunk_bits = 36, + .adapt_user_to_ir = v110_adapt_Nx3600_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx3600, + }, + [OSMO_V110_SYNC_RA1_38400] = { + .data_rate = 38400, + .intermediate_rate = 64000, + .user_data_chunk_bits = 48, + .adapt_user_to_ir = v110_adapt_Nx4800_to_IR, + .adapt_ir_to_user = v110_adapt_IR_to_Nx4800, + }, +}; + +/*! obtain the size (in number of bits) of the user data bits in one V.110 + * frame for specified RA1 rate */ +int osmo_v110_sync_ra1_get_user_data_chunk_bitlen(enum osmo_v100_sync_ra1_rate rate) +{ + if (rate < 0 || rate >= _NUM_OSMO_V110_SYNC_RA1) + return -EINVAL; + + return osmo_v110_sync_ra1_def[rate].user_data_chunk_bits; +} + +/*! obtain the user data rate (in bits/s) for specified RA1 rate */ +int osmo_v110_sync_ra1_get_user_data_rate(enum osmo_v100_sync_ra1_rate rate) +{ + if (rate < 0 || rate >= _NUM_OSMO_V110_SYNC_RA1) + return -EINVAL; + + return osmo_v110_sync_ra1_def[rate].data_rate; +} + +/*! obtain the intermediate rate (in bits/s) for specified RA1 rate */ +int osmo_v110_sync_ra1_get_intermediate_rate(enum osmo_v100_sync_ra1_rate rate) +{ + if (rate < 0 || rate >= _NUM_OSMO_V110_SYNC_RA1) + return -EINVAL; + + return osmo_v110_sync_ra1_def[rate].intermediate_rate; +} + +/*! perform V.110 RA1 function in user rate -> intermediate rate direction. + * \param[in] rate specification of the user bitrate + * \param[out] fr caller-allocated output buffer for the [decoded] V.110 frame generated + * \param[in] d_in input user data (unpacked bits) + * \param[in] in_len length of user input data (in number of bits) + * \returns 0 on success; negative in case of error */ +int osmo_v110_sync_ra1_user_to_ir(enum osmo_v100_sync_ra1_rate rate, struct osmo_v110_decoded_frame *fr, + const ubit_t *d_in, size_t in_len) +{ + if (rate < 0 || rate >= _NUM_OSMO_V110_SYNC_RA1) + return -EINVAL; + + return osmo_v110_sync_ra1_def[rate].adapt_user_to_ir(fr, d_in, in_len); +} + +/*! perform V.110 RA1 function in intermediate rate -> user rate direction. + * \param[in] rate specification of the user bitrate + * \param[out] d_out caller-allocated output user data (unpacked bits) + * \param[out] out_len length of d_out output buffer + * \param[in] fr [decoded] V.110 frame used as input + * \returns number of unpacked bits written to d_out on success; negative in case of error */ +int osmo_v110_sync_ra1_ir_to_user(enum osmo_v100_sync_ra1_rate rate, ubit_t *d_out, size_t out_len, + const struct osmo_v110_decoded_frame *fr) +{ + if (rate < 0 || rate >= _NUM_OSMO_V110_SYNC_RA1) + return -EINVAL; + + return osmo_v110_sync_ra1_def[rate].adapt_ir_to_user(d_out, out_len, fr); +} diff --git a/src/isdn/v110_ta.c b/src/isdn/v110_ta.c new file mode 100644 index 00000000..6730b20c --- /dev/null +++ b/src/isdn/v110_ta.c @@ -0,0 +1,881 @@ +/*! \file v110_ta.c + * TA (Terminal Adapter) implementation as per ITU-T V.110. */ +/* + * (C) 2022 by Harald Welte <laforge@gnumonks.org> + * (C) 2023 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * Initial (Work-in-Progress) implementation by Harald Welte, + * completed and co-authored by Vadim Yanitskiy. + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + +#include <stdbool.h> +#include <stdint.h> +#include <errno.h> + +#include <osmocom/core/logging.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/bits.h> +#include <osmocom/core/tdef.h> +#include <osmocom/core/fsm.h> + +#include <osmocom/isdn/v110.h> +#include <osmocom/isdn/v110_ta.h> + +#define S(x) (1 << (x)) + +#define V24_FLAGMASK_IS_ON(flags, circuit) \ + (((flags) & S(circuit)) != 0) + +#define V24_FLAGMASK_IS_OFF(flags, circuit) \ + (((flags) & S(circuit)) == 0) + +#define V24_FLAGMASK_SET_ON(flags, circuit) \ + (flags) |= S(circuit) + +#define V24_FLAGMASK_SET_OFF(flags, circuit) \ + (flags) &= ~S(circuit) + +/* inverse logic: ON = binary 0; OFF = binary 1 */ +#define V110_SX_BIT_ON 0 +#define V110_SX_BIT_OFF 1 + +const struct value_string osmo_v110_ta_circuit_names[] = { + { OSMO_V110_TA_C_105, "105/RTS" }, + { OSMO_V110_TA_C_106, "106/CTS" }, + { OSMO_V110_TA_C_107, "107/DSR" }, + { OSMO_V110_TA_C_108, "108/DTR" }, + { OSMO_V110_TA_C_109, "109/DCD" }, + { OSMO_V110_TA_C_133, "133" }, + { 0, NULL } +}; + +const struct value_string osmo_v110_ta_circuit_descs[] = { + { OSMO_V110_TA_C_105, "Request to Send" }, + { OSMO_V110_TA_C_106, "Clear to Send" }, + { OSMO_V110_TA_C_107, "Data Set Ready" }, + { OSMO_V110_TA_C_108, "Data Terminal Ready" }, + { OSMO_V110_TA_C_109, "Data Carrier Detect" }, + { OSMO_V110_TA_C_133, "Ready for receiving" }, + { 0, NULL } +}; + +static const struct osmo_tdef v110_ta_tdef[] = { + { .T = OSMO_V110_TA_TIMER_X1, + .unit = OSMO_TDEF_MS, .default_val = 3000, /* suggested in 7.1.5 e) */ + .desc = "ITU-T V.110 7.1.5 Loss of frame synchronization: sync recovery timer" }, + { .T = OSMO_V110_TA_TIMER_T1, + .unit = OSMO_TDEF_MS, .default_val = 10000, /* suggested in 7.1.2.2 */ + .desc = "ITU-T V.110 7.1.2 Connect TA to line: sync establishment timer" }, + { .T = OSMO_V110_TA_TIMER_T2, + .unit = OSMO_TDEF_MS, .default_val = 5000, /* suggested in 7.1.4.1 */ + .desc = "ITU-T V.110 7.1.4 Disconnect mode: disconnect confirmation timer" }, + { /* end of list */ } +}; + +/********************************************************************************* + * V.110 TERMINAL ADAPTER FSM + *********************************************************************************/ + +enum v110_ta_fsm_state { + V110_TA_ST_IDLE_READY, /* 7.1.1 Idle (or ready) state */ + V110_TA_ST_CON_TA_TO_LINE, /* 7.1.2 Connect TA to line state */ + V110_TA_ST_DATA_TRANSFER, /* 7.1.3 Data transfer state */ + V110_TA_ST_DISCONNECTING, /* 7.1.4 Disconnect mode */ + V110_TA_ST_RESYNCING, /* 7.1.5 Re-synchronizing state */ +}; + +enum v110_ta_fsm_event { + V110_TA_EV_RX_FRAME_IND, /* a V.110 frame was received by the lower layer */ + V110_TA_EV_TX_FRAME_RTS, /* a V.110 frame is to be sent by the lower layer */ + V110_TA_EV_V24_STATUS_CHG, /* V.24 flag-mask has been updated by TE */ + V110_TA_EV_SYNC_IND, /* the lower layer has synchronized to the frame clock */ + V110_TA_EV_DESYNC_IND, /* the lower layer has lost frame clock synchronization */ + V110_TA_EV_TIMEOUT, /* generic event for handling a timeout condition */ +}; + +static const struct value_string v110_ta_fsm_event_names[] = { + { V110_TA_EV_RX_FRAME_IND, "RX_FRAME_IND" }, + { V110_TA_EV_TX_FRAME_RTS, "TX_FRAME_RTS" }, + { V110_TA_EV_V24_STATUS_CHG, "V24_STATUS_CHG" }, + { V110_TA_EV_SYNC_IND, "SYNC_IND" }, + { V110_TA_EV_DESYNC_IND, "DESYNC_IND" }, + { V110_TA_EV_TIMEOUT, "TIMEOUT" }, + { 0, NULL } +}; + +enum v110_ta_d_bit_mode { + V110_TA_DBIT_M_ALL_ZERO = 0, /* set all bits to binary '0' */ + V110_TA_DBIT_M_ALL_ONE = 1, /* set all bits to binary '1' */ + V110_TA_DBIT_M_FORWARD, /* forward D-bits to/from DTE */ +}; + +struct v110_ta_state { + /*! V.24 status flags shared between DTE (user) and DCE (TA, us) */ + unsigned int v24_flags; + struct { + /* what kind of D-bits to transmit in V.110 frames */ + enum v110_ta_d_bit_mode d_bit_mode; + /* what to put in S-bits of transmitted V.110 frames */ + ubit_t s_bits; + /* what to put in X-bits of transmitted V.110 frames */ + ubit_t x_bits; + } tx; + struct { + enum v110_ta_d_bit_mode d_bit_mode; + } rx; +}; + +struct osmo_v110_ta { + const char *name; + struct osmo_tdef *Tdefs; + struct osmo_fsm_inst *fi; + struct osmo_v110_ta_cfg *cfg; + struct v110_ta_state state; +}; + +static inline bool v110_df_x_bits_are(const struct osmo_v110_decoded_frame *df, ubit_t cmp) +{ + return (df->x_bits[0] == cmp) && (df->x_bits[1] == cmp); +} + +static inline bool v110_df_s_bits_are(const struct osmo_v110_decoded_frame *df, ubit_t cmp) +{ + /* ITU-T Table 2/V.110 (see also 5.1.2.3) defines the following S-bits: + * S1, S3, S4, S6, S8, S9 (6 bits total). However, fr->s_bits[] contains + * 9 (MAX_S_BITS) bits, including the undefined bits S2, S5, S7. + * Hence we must skip those undefined bits. */ + static const uint8_t sbit_map[] = { 0, 2, 3, 5, 7, 8 }; + + for (unsigned int i = 0; i < ARRAY_SIZE(sbit_map); i++) { + uint8_t idx = sbit_map[i]; + if (df->s_bits[idx] != cmp) + return false; + } + + return true; +} + +static inline bool v110_df_d_bits_are(const struct osmo_v110_decoded_frame *df, ubit_t cmp) +{ + for (unsigned int i = 0; i < MAX_D_BITS; i++) { + if (df->d_bits[i] != cmp) + return false; + } + + return true; +} + +/* handle one V.110 frame and forward user bits to the application */ +static void v110_ta_handle_frame(const struct osmo_v110_ta *ta, + const struct osmo_v110_decoded_frame *df) +{ + const struct osmo_v110_ta_cfg *cfg = ta->cfg; + const struct v110_ta_state *ts = &ta->state; + ubit_t user_bits[MAX_D_BITS]; + int num_user_bits; + int rc; + + switch (ts->rx.d_bit_mode) { + case V110_TA_DBIT_M_ALL_ZERO: + case V110_TA_DBIT_M_ALL_ONE: + /* generate as many user bits as needed for the configured rate */ + num_user_bits = osmo_v110_sync_ra1_get_user_data_chunk_bitlen(cfg->rate); + OSMO_ASSERT(num_user_bits > 0); + /* set them all to binary '0' or binary '1' */ + memset(&user_bits[0], (int)ts->rx.d_bit_mode, num_user_bits); + cfg->rx_cb(cfg->priv, &user_bits[0], num_user_bits); + break; + case V110_TA_DBIT_M_FORWARD: + rc = osmo_v110_sync_ra1_ir_to_user(cfg->rate, &user_bits[0], sizeof(user_bits), df); + if (rc > 0) + cfg->rx_cb(cfg->priv, &user_bits[0], rc); + /* XXX else: indicate an error somehow? */ + break; + } +} + +/* build one V.110 frame to transmit */ +static void v110_ta_build_frame(const struct osmo_v110_ta *ta, + struct osmo_v110_decoded_frame *df) +{ + const struct osmo_v110_ta_cfg *cfg = ta->cfg; + const struct v110_ta_state *ts = &ta->state; + ubit_t user_bits[MAX_D_BITS]; + int num_user_bits; + int rc; + + /* E-bits (E1/E2/E3 may be overwritten below) */ + memset(df->e_bits, 1, sizeof(df->e_bits)); + /* S-bits */ + memset(df->s_bits, ts->tx.s_bits, sizeof(df->s_bits)); + /* X-bits */ + memset(df->x_bits, ts->tx.x_bits, sizeof(df->x_bits)); + + /* D-bits */ + switch (ts->tx.d_bit_mode) { + case V110_TA_DBIT_M_ALL_ZERO: + case V110_TA_DBIT_M_ALL_ONE: + /* set them all to binary '0' or binary '1' */ + memset(df->d_bits, (int)ts->tx.d_bit_mode, sizeof(df->d_bits)); + break; + case V110_TA_DBIT_M_FORWARD: + /* how many user bits to retrieve */ + num_user_bits = osmo_v110_sync_ra1_get_user_data_chunk_bitlen(cfg->rate); + OSMO_ASSERT(num_user_bits > 0); + /* retrieve user bits from the application */ + cfg->tx_cb(cfg->priv, &user_bits[0], num_user_bits); + /* convert user bits to intermediate rate (store to df) */ + rc = osmo_v110_sync_ra1_user_to_ir(cfg->rate, df, &user_bits[0], num_user_bits); + OSMO_ASSERT(rc == 0); + break; + } +} + +static void v110_ta_flags_update(struct osmo_v110_ta *ta, unsigned int v24_flags) +{ + struct osmo_v110_ta_cfg *cfg = ta->cfg; + + if (ta->state.v24_flags == v24_flags) + return; + if (cfg->status_update_cb != NULL) + cfg->status_update_cb(cfg->priv, v24_flags); + ta->state.v24_flags = v24_flags; +} + +static const struct osmo_tdef_state_timeout v110_ta_fsm_timeouts[32] = { + [V110_TA_ST_RESYNCING] = { .T = OSMO_V110_TA_TIMER_X1 }, + [V110_TA_ST_CON_TA_TO_LINE] = { .T = OSMO_V110_TA_TIMER_T1 }, + [V110_TA_ST_DISCONNECTING] = { .T = OSMO_V110_TA_TIMER_T2 }, +}; + +#define v110_ta_fsm_state_chg(state) \ + osmo_tdef_fsm_inst_state_chg(fi, state, \ + v110_ta_fsm_timeouts, \ + ((struct osmo_v110_ta *)(fi->priv))->Tdefs, \ + 0) + +/* ITU-T V.110 Section 7.1.1 */ +static void v110_ta_fsm_idle_ready_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + unsigned int v24_flags = ta->state.v24_flags; + + /* 7.1.1.2 During the idle (or ready) state the TA will transmit continuous binary 1s into the B-channel */ + ts->tx.d_bit_mode = V110_TA_DBIT_M_ALL_ONE; /* circuit 103: continuous binary '1' */ + ts->tx.s_bits = V110_SX_BIT_OFF; /* OFF is binary '1' */ + ts->tx.x_bits = V110_SX_BIT_OFF; /* OFF is binary '1' */ + + /* 7.1.1.3 During the idle (or ready) state the TA (DCE) will transmit the following toward the DTE: */ + /* - circuit 104: continuous binary '1' */ + ts->rx.d_bit_mode = V110_TA_DBIT_M_ALL_ONE; + /* - circuits 107, 106, 109 = OFF */ + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_106); + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_107); + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_109); + v110_ta_flags_update(ta, v24_flags); +} + +/* ITU-T V.110 Section 7.1.1 */ +static void v110_ta_fsm_idle_ready(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + + switch (event) { + case V110_TA_EV_V24_STATUS_CHG: + /* When the TA is to be switched to the data mode, circuit 108 must be ON */ + if (V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_108)) { + /* 7.12.2: Start timer T1 when switching to CON_TA_LINE */ + v110_ta_fsm_state_chg(V110_TA_ST_CON_TA_TO_LINE); + } + break; + case V110_TA_EV_RX_FRAME_IND: + v110_ta_handle_frame(ta, (const struct osmo_v110_decoded_frame *)data); + break; + case V110_TA_EV_TX_FRAME_RTS: + v110_ta_build_frame(ta, (struct osmo_v110_decoded_frame *)data); + break; + default: + OSMO_ASSERT(0); + } +} + +/* ITU-T V.110 Section 7.1.2 */ +static void v110_ta_fsm_connect_ta_to_line_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + + /* 7.1.2.1 Switching to the data mode causes the TA to transmit the following towards the ISDN: */ + /* a) frame synchronization pattern as described in 5.1.3.1 and 5.2.1 (done by the API user) */ + /* b) circuit 103: continuous binary '1' */ + ts->tx.d_bit_mode = V110_TA_DBIT_M_ALL_ONE; + /* c) status bits S = OFF and X = OFF */ + ts->tx.s_bits = V110_SX_BIT_OFF; /* OFF is binary '1' */ + ts->tx.x_bits = V110_SX_BIT_OFF; /* OFF is binary '1' */ + + /* 7.1.2.2 ... the receiver in the TA will begin to search for the frame synchronization + * pattern in the received bit stream (see 5.1.3.1 and 5.2.1) and start timer T1. */ + OSMO_ASSERT(fi->T == OSMO_V110_TA_TIMER_T1); +} + +/* ITU-T V.110 Section 7.1.2 */ +static void v110_ta_fsm_connect_ta_to_line(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + + switch (event) { + case V110_TA_EV_V24_STATUS_CHG: + /* If circuit 108 is OFF, we go back to IDLE/READY */ + if (V24_FLAGMASK_IS_OFF(ts->v24_flags, OSMO_V110_TA_C_108)) + v110_ta_fsm_state_chg(V110_TA_ST_IDLE_READY); + break; + case V110_TA_EV_SYNC_IND: + /* 7.1.2.3 When the receiver recognizes the frame synchronization pattern, it causes the S- + * and X-bits in the transmitted frames to be turned ON (provided that circuit 108 is ON). */ + OSMO_ASSERT(V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_108)); + ts->tx.s_bits = V110_SX_BIT_ON; + ts->tx.x_bits = V110_SX_BIT_ON; + break; + case V110_TA_EV_RX_FRAME_IND: + { + const struct osmo_v110_decoded_frame *df = data; + unsigned int v24_flags = ta->state.v24_flags; + + /* 7.1.2.4 When the receiver recognizes that the status of bits S and X are ON */ + if (v110_df_s_bits_are(df, V110_SX_BIT_ON) && + v110_df_x_bits_are(df, V110_SX_BIT_ON)) { + /* ... it will perform the following functions: */ + /* a) Turn ON circuit 107 toward the DTE and stop timer T1 */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_107); + osmo_timer_del(&fi->timer); + /* b) Then, circuit 103 may be connected to the data bits in the frame; however, the + * DTE must maintain a binary 1 condition on circuit 103 until circuit 106 is turned + * ON in the next portion of the sequence. */ + /* c) Turn ON circuit 109 and connect the data bits to circuit 104. */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_109); + ts->rx.d_bit_mode = V110_TA_DBIT_M_FORWARD; + /* d) After an interval of N bits (see 6.3), it will turn ON circuit 106. */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_106); + ts->tx.d_bit_mode = V110_TA_DBIT_M_FORWARD; + v110_ta_flags_update(ta, v24_flags); + /* Circuit 106 transitioning from OFF to ON will cause the transmitted data to + * transition from binary 1 to the data mode. */ + v110_ta_fsm_state_chg(V110_TA_ST_DATA_TRANSFER); + } + + v110_ta_handle_frame(ta, df); + break; + } + case V110_TA_EV_TX_FRAME_RTS: + v110_ta_build_frame(ta, (struct osmo_v110_decoded_frame *)data); + break; + case V110_TA_EV_TIMEOUT: + v110_ta_fsm_state_chg(V110_TA_ST_IDLE_READY); + break; + default: + OSMO_ASSERT(0); + } +} + +/* ITU-T V.110 Section 7.1.3 */ +static void v110_ta_fsm_data_transfer_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + unsigned int v24_flags = ta->state.v24_flags; + + /* 7.1.3.1 While in the data transfer state, the following circuit conditions exist: + * a): 105, 107, 108, and 109 are in the ON condition */ + /* XXX: OSMO_ASSERT(V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_105)); */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_107); + /* XXX: OSMO_ASSERT(V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_108)); */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_109); + /* b) data is being transmitted on circuit 103 and received on circuit 104 */ + ts->rx.d_bit_mode = V110_TA_DBIT_M_FORWARD; + ts->tx.d_bit_mode = V110_TA_DBIT_M_FORWARD; + /* c) circuits 133 (when implemented) and 106 are in the ON condition unless local out-of-band + * flow control is being used, either or both circuits may be in the ON or the OFF condition. */ + if (!ta->cfg->flow_ctrl.end_to_end) { + /* XXX: OSMO_ASSERT(V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_133)); */ + V24_FLAGMASK_SET_ON(v24_flags, OSMO_V110_TA_C_106); + } + v110_ta_flags_update(ta, v24_flags); + + /* 7.1.3.2 While in the data transfer state, the following status bit conditions exist: */ + /* a) status bits S in both directions are in the ON condition; */ + ts->tx.s_bits = V110_SX_BIT_ON; + /* b) status bits X in both directions are in the ON condition unless end-to-end flow control + * is being used, in which case status bit X in either or both directions may be in the + * ON or the OFF condition. */ + if (!ta->cfg->flow_ctrl.end_to_end) + ts->tx.x_bits = V110_SX_BIT_ON; +} + +/* ITU-T V.110 Section 7.1.3 */ +static void v110_ta_fsm_data_transfer(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + + /* 7.1.3.3 While in the data transfer state: */ + /* a) the S status bits shall *not* be mapped to/from the interchange circuits */ + /* b) the X status bits shall *not* be mapped according to Table 3, + * unless end-to-end flow control is implemented */ + /* TODO: if (ta->cfg->flow_ctrl.end_to_end) { ... } */ + + switch (event) { + case V110_TA_EV_V24_STATUS_CHG: + /* 7.1.4.1 At the completion of the data transfer phase, the local DTE will indicate a + * disconnect request by turning OFF circuit 108 */ + if (V24_FLAGMASK_IS_ON(ts->v24_flags, OSMO_V110_TA_C_108)) + break; + v110_ta_fsm_state_chg(V110_TA_ST_DISCONNECTING); + break; + case V110_TA_EV_DESYNC_IND: + v110_ta_fsm_state_chg(V110_TA_ST_RESYNCING); + break; + case V110_TA_EV_TX_FRAME_RTS: + v110_ta_build_frame(ta, (struct osmo_v110_decoded_frame *)data); + break; + case V110_TA_EV_RX_FRAME_IND: + { + const struct osmo_v110_decoded_frame *df = data; + unsigned int v24_flags = ta->state.v24_flags; + + /* 7.1.4.2 ... this TA will recognize the transition of the status bits S from + * ON to OFF and the data bits from data to binary 0 as a disconnect request */ + if (v110_df_s_bits_are(df, V110_SX_BIT_OFF) && v110_df_d_bits_are(df, 0)) { + /* ... and it will turn OFF circuits 107 and 109. */ + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_107); + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_109); + v110_ta_flags_update(ta, v24_flags); + /* DTE should respond by turning OFF circuit 108 */ + break; /* XXX: shall we forward D-bits to DTE anyway? */ + } + + v110_ta_handle_frame(ta, df); + break; + } + default: + OSMO_ASSERT(0); + } +} + +/* ITU-T V.110 Section 7.1.4 */ +static void v110_ta_fsm_disconnect_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + unsigned int v24_flags = ta->state.v24_flags; + + /* 7.1.4.1 At the completion of the data transfer phase, the local DTE will indicate a + * disconnect request by turning OFF circuit 108. This will cause the following to occur: */ + /* a) the status bits S in the frame toward ISDN will turn OFF, status bits X are kept ON */ + ts->tx.s_bits = V110_SX_BIT_OFF; + /* b) circuit 106 will be turned OFF */ + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_106); + v110_ta_flags_update(ta, v24_flags); + /* c) the data bits in the frame will be set to binary 0. */ + ts->tx.d_bit_mode = V110_TA_DBIT_M_ALL_ZERO; + + /* To guard against the failure of the remote TA to respond to the disconnect request, + * the local TA may start a timer T2 (suggested value 5 s) which is stopped by the + * reception or transmission of any D-channel clearing message (DISCONNECT, RELEASE, + * RELEASE COMPLETE). */ + OSMO_ASSERT(fi->T == OSMO_V110_TA_TIMER_T2); +} + +/* ITU-T V.110 Section 7.1.4 */ +static void v110_ta_fsm_disconnect(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + + switch (event) { + case V110_TA_EV_V24_STATUS_CHG: + break; /* nothing to do */ + case V110_TA_EV_TX_FRAME_RTS: + v110_ta_build_frame(ta, (struct osmo_v110_decoded_frame *)data); + break; + case V110_TA_EV_RX_FRAME_IND: + { + const struct osmo_v110_decoded_frame *df = data; + + /* 7.1.4.3 The TA at the station that originated the disconnect request will + * recognize reception of S = OFF or the loss of framing signals as a disconnect + * acknowledgement and turn OFF circuits 107 and 109. */ + if (v110_df_s_bits_are(df, V110_SX_BIT_OFF)) { + /* circuits 107 and 109 set to off in .onenter() */ + v110_ta_fsm_state_chg(V110_TA_ST_IDLE_READY); + } + + v110_ta_handle_frame(ta, df); + break; + } + case V110_TA_EV_DESYNC_IND: + case V110_TA_EV_TIMEOUT: + /* circuits 107 and 109 set to off in .onenter() */ + v110_ta_fsm_state_chg(V110_TA_ST_IDLE_READY); + break; + default: + OSMO_ASSERT(0); + } +} + +/* ITU-T V.110 Section 7.1.5 */ +static void v110_ta_fsm_resyncing_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + + /* 7.1.5 In the event of loss of frame synchronization, the (local) TA should + * attempt to resynchronize as follows: */ + /* a) Place circuit 104 in binary 1 condition (passes from the data mode) */ + ts->rx.d_bit_mode = V110_TA_DBIT_M_ALL_ONE; + /* b) Turn OFF status bit X in the transmitted frame */ + ts->tx.x_bits = V110_SX_BIT_OFF; + + /* guard timeout, see 7.1.5 e) */ + OSMO_ASSERT(fi->T == OSMO_V110_TA_TIMER_X1); +} + +/* ITU-T V.110 Section 7.1.5 */ +static void v110_ta_fsm_resyncing(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_v110_ta *ta = (struct osmo_v110_ta *)fi->priv; + struct v110_ta_state *ts = &ta->state; + unsigned int v24_flags = ta->state.v24_flags; + + switch (event) { + case V110_TA_EV_V24_STATUS_CHG: + break; /* TODO: handle circuit 108 being set to OFF? */ + case V110_TA_EV_TX_FRAME_RTS: + v110_ta_build_frame(ta, (struct osmo_v110_decoded_frame *)data); + break; + case V110_TA_EV_SYNC_IND: + /* f) If resynchronization is achieved, the local TA should turn ON status bit X */ + ts->tx.x_bits = V110_SX_BIT_ON; + v110_ta_fsm_state_chg(V110_TA_ST_DATA_TRANSFER); + break; + case V110_TA_EV_TIMEOUT: + /* e) If after an interval of X1 seconds the local TA cannot attain synchronization, + * it should send a disconnect request by turning OFF all of the status bits for several + * (at least three) frames with data bits set to binary 0 and then disconnect by turning + * OFF circuit 107 and transferring to the disconnected mode as discussed in 7.1.4.2. */ + ts->tx.s_bits = V110_SX_BIT_OFF; + ts->tx.x_bits = V110_SX_BIT_OFF; + ts->tx.d_bit_mode = V110_TA_DBIT_M_ALL_ZERO; + /* TODO: actually Tx those frames (delay state transition) */ + V24_FLAGMASK_SET_OFF(v24_flags, OSMO_V110_TA_C_107); + v110_ta_flags_update(ta, v24_flags); + v110_ta_fsm_state_chg(V110_TA_ST_DISCONNECTING); + break; + default: + OSMO_ASSERT(0); + } +} + +static int v110_ta_timer_cb(struct osmo_fsm_inst *fi) +{ + osmo_fsm_inst_dispatch(fi, V110_TA_EV_TIMEOUT, NULL); + return 0; +} + +static const struct osmo_fsm_state v110_ta_states[] = { + [V110_TA_ST_IDLE_READY] = { + .name = "IDLE_READY", + .in_event_mask = S(V110_TA_EV_V24_STATUS_CHG) + | S(V110_TA_EV_TX_FRAME_RTS) + | S(V110_TA_EV_RX_FRAME_IND), + .out_state_mask = S(V110_TA_ST_IDLE_READY) + | S(V110_TA_ST_CON_TA_TO_LINE), + .action = &v110_ta_fsm_idle_ready, + .onenter = &v110_ta_fsm_idle_ready_onenter, + }, + [V110_TA_ST_CON_TA_TO_LINE] = { + .name = "CONNECT_TA_TO_LINE", + .in_event_mask = S(V110_TA_EV_V24_STATUS_CHG) + | S(V110_TA_EV_TIMEOUT) + | S(V110_TA_EV_SYNC_IND) + | S(V110_TA_EV_TX_FRAME_RTS) + | S(V110_TA_EV_RX_FRAME_IND), + .out_state_mask = S(V110_TA_ST_DATA_TRANSFER) + | S(V110_TA_ST_IDLE_READY), + .action = &v110_ta_fsm_connect_ta_to_line, + .onenter = &v110_ta_fsm_connect_ta_to_line_onenter, + }, + [V110_TA_ST_DATA_TRANSFER] = { + .name = "DATA_TRANSFER", + .in_event_mask = S(V110_TA_EV_V24_STATUS_CHG) + | S(V110_TA_EV_DESYNC_IND) + | S(V110_TA_EV_TX_FRAME_RTS) + | S(V110_TA_EV_RX_FRAME_IND), + .out_state_mask = S(V110_TA_ST_RESYNCING) + | S(V110_TA_ST_DISCONNECTING), + .action = &v110_ta_fsm_data_transfer, + .onenter = &v110_ta_fsm_data_transfer_onenter, + }, + [V110_TA_ST_DISCONNECTING] = { + .name = "DISCONNECTING", + .in_event_mask = S(V110_TA_EV_V24_STATUS_CHG) + | S(V110_TA_EV_TIMEOUT) + | S(V110_TA_EV_TX_FRAME_RTS) + | S(V110_TA_EV_RX_FRAME_IND) + | S(V110_TA_EV_DESYNC_IND), + .out_state_mask = S(V110_TA_ST_IDLE_READY), + .action = &v110_ta_fsm_disconnect, + .onenter = &v110_ta_fsm_disconnect_onenter, + }, + [V110_TA_ST_RESYNCING] = { + .name = "RESYNCING", + .in_event_mask = S(V110_TA_EV_V24_STATUS_CHG) + | S(V110_TA_EV_TIMEOUT) + | S(V110_TA_EV_TX_FRAME_RTS) + | S(V110_TA_EV_SYNC_IND), + .out_state_mask = S(V110_TA_ST_IDLE_READY) + | S(V110_TA_ST_DATA_TRANSFER), + .action = &v110_ta_fsm_resyncing, + .onenter = &v110_ta_fsm_resyncing_onenter, + }, +}; + +static struct osmo_fsm osmo_v110_ta_fsm = { + .name = "V110-TA", + .states = v110_ta_states, + .num_states = ARRAY_SIZE(v110_ta_states), + .timer_cb = v110_ta_timer_cb, + .log_subsys = DLGLOBAL, + .event_names = v110_ta_fsm_event_names, +}; + +static __attribute__((constructor)) void on_dso_load(void) +{ + OSMO_ASSERT(osmo_fsm_register(&osmo_v110_ta_fsm) == 0); +} + +/*! Allocate a V.110 TA (Terminal Adapter) instance. + * \param[in] ctx parent talloc context. + * \param[in] name name of the TA instance. + * \param[in] cfg initial configuration of the TA instance. + * \returns pointer to allocated TA instance; NULL on error. */ +struct osmo_v110_ta *osmo_v110_ta_alloc(void *ctx, const char *name, + const struct osmo_v110_ta_cfg *cfg) +{ + struct osmo_v110_ta *ta; + + OSMO_ASSERT(cfg != NULL); + OSMO_ASSERT(cfg->rx_cb != NULL); + OSMO_ASSERT(cfg->tx_cb != NULL); + + /* local (TE-TA) flow control is not implemented */ + if (cfg->flow_ctrl.local != OSMO_V110_LOCAL_FLOW_CTRL_NONE) { + LOGP(DLGLOBAL, LOGL_ERROR, "Local (TE-TA) flow control is not implemented\n"); + return NULL; + } + + ta = talloc_zero(ctx, struct osmo_v110_ta); + if (ta == NULL) + return NULL; + + ta->name = talloc_strdup(ta, name); + ta->cfg = talloc_memdup(ta, cfg, sizeof(*cfg)); + if (ta->name == NULL || ta->cfg == NULL) + goto exit_free; + + ta->Tdefs = talloc_memdup(ta, v110_ta_tdef, sizeof(v110_ta_tdef)); + if (ta->Tdefs == NULL) + goto exit_free; + osmo_tdefs_reset(ta->Tdefs); /* apply default values */ + + ta->fi = osmo_fsm_inst_alloc(&osmo_v110_ta_fsm, ta, ta, LOGL_DEBUG, name); + if (ta->fi == NULL) + goto exit_free; + + /* perform a loop transition to init the internal state */ + osmo_fsm_inst_state_chg(ta->fi, V110_TA_ST_IDLE_READY, 0, 0); + + return ta; + +exit_free: + if (ta->fi != NULL) + osmo_fsm_inst_free(ta->fi); + talloc_free(ta); + return NULL; +} + +/*! Release memory taken by the given V.110 TA instance. + * \param[in] ta TA instance to be free()d. */ +void osmo_v110_ta_free(struct osmo_v110_ta *ta) +{ + if (ta == NULL) + return; + if (ta->fi != NULL) + osmo_fsm_inst_free(ta->fi); + talloc_free(ta); /* also free()s name and cfg */ +} + +/*! Configure a timer of the given V.110 TA instance. + * \param[in] ta TA instance to be configured. + * \param[in] timer a timer to be configured. + * \param[in] val_ms the new timeout value to set (in milliseconds). + * \returns 0 in case of success; negative on error. */ +int osmo_v110_ta_set_timer_val_ms(struct osmo_v110_ta *ta, + enum osmo_v110_ta_timer timer, + unsigned long val_ms) +{ + return osmo_tdef_set(ta->Tdefs, (int)timer, val_ms, OSMO_TDEF_MS); +} + +/*! Feed a [decoded] V.110 frame into the given TA instance. + * + * This function, like its _out counterpart, is intended to be used by the lower layers + * receiving V.110 frames over some medium. The caller of this function is responsible + * for finding the synchronization pattern (if needed), aligning to the frame boundaries, + * and decoding frames using osmo_v110_decode_frame() or osmo_csd_*_decode_frame(). + * + * Bits E1/E2/E3 are expected to be set by the caller (if not being transmitted + * over the medium) in accordance with the configured synchronous user rate. + * + * Bits D1..D48 are passed to the bit rate adaption function RA1. The resulting output + * is then passed to the upper layer (application) via the configured .rx_cb(). Though, + * in certain states of the TA's FSM, bits D1..D48 are ignored and the upper layer + * gets a sequence of binary '0' or '1'. + * + * \param[in] ta TA instance to feed the given frame into. + * \param[in] in pointer to a [decoded] V.110 frame. + * \returns 0 in case of success; negative on error. */ +int osmo_v110_ta_frame_in(struct osmo_v110_ta *ta, const struct osmo_v110_decoded_frame *in) +{ + return osmo_fsm_inst_dispatch(ta->fi, V110_TA_EV_RX_FRAME_IND, (void *)in); +} + +/*! Pull a [decoded] V.110 frame out of the given TA instance. + * + * This function, like its _in counterpart, is intended to be used by the lower layers + * transmitting V.110 frames over some medium. The caller of this function is responsible + * for encoding the output frame using osmo_v110_encode_frame() or osmo_csd_*_encode_frame(). + * + * Bits E1/E2/E3 are set in accordance with the configured synchronous user rate. + * Bits E4/E5/E6/E7 are unconditionally set to binary '1'. + * + * Bits D1..D48 are set depending on the state of TA's FSM: + * + * - In data transfer mode, the user bits are obtained from the upper layer (application) + * via the configured .tx_cb(), and then passed to the bit rate adaption function RA1, + * which generates bits D1..D48. + * - In other modes, bits D1..D48 are all set to binary '0' or '1'. + * + * \param[in] ta TA instance to pull a frame from. + * \param[out] out where to store a [decoded] V.110 frame. + * \returns 0 in case of success; negative on error. */ +int osmo_v110_ta_frame_out(struct osmo_v110_ta *ta, struct osmo_v110_decoded_frame *out) +{ + return osmo_fsm_inst_dispatch(ta->fi, V110_TA_EV_TX_FRAME_RTS, (void *)out); +} + +/*! Indicate a synchronization establishment event. + * + * This function is intended to be called when the lower layer + * achieves synchronization to the frame clock. + * + * \param[in] ta TA instance to indicate the event to. + * \returns 0 in case of success; negative on error. */ +int osmo_v110_ta_sync_ind(struct osmo_v110_ta *ta) +{ + return osmo_fsm_inst_dispatch(ta->fi, V110_TA_EV_SYNC_IND, NULL); +} + +/*! Indicate a synchronization loss event. + * + * This function is intended to be called when the lower layer + * experiences a loss of synchronization with the frame clock. + * + * \param[in] ta TA instance to indicate the event to. + * \returns 0 in case of success; negative on error. */ +int osmo_v110_ta_desync_ind(struct osmo_v110_ta *ta) +{ + return osmo_fsm_inst_dispatch(ta->fi, V110_TA_EV_DESYNC_IND, NULL); +} + +/*! Get the V.24 status bit-mask of the given TA instance. + * \param[in] ta TA instance to get the circuit bit-mask. + * \returns bitmask of OSMO_V110_TA_C_*. */ +unsigned int osmo_v110_ta_get_status(const struct osmo_v110_ta *ta) +{ + return ta->state.v24_flags; +} + +/*! Set the V.24 status bit-mask of the given TA instance. + * \param[in] ta TA instance to update the circuit state. + * \param[in] status bit-mask of OSMO_V110_TA_C_*. + * \returns 0 on success; negative on error. */ +static int v110_ta_set_status(struct osmo_v110_ta *ta, unsigned int status) +{ + const unsigned int old_status = ta->state.v24_flags; + int rc = 0; + + ta->state.v24_flags = status; + if (status != old_status) + rc = osmo_fsm_inst_dispatch(ta->fi, V110_TA_EV_V24_STATUS_CHG, NULL); + + return rc; +} + +/*! Get state of a V.24 circuit of the given TA instance. + * \param[in] ta TA instance to get the circuit state. + * \param[in] circuit a V.24 circuit, one of OSMO_V110_TA_C_*. + * \returns circuit state: active (true) or inactive (false). */ +bool osmo_v110_ta_get_circuit(const struct osmo_v110_ta *ta, + enum osmo_v110_ta_circuit circuit) +{ + return V24_FLAGMASK_IS_ON(ta->state.v24_flags, circuit); +} + +/*! Activate/deactivate a V.24 circuit of the given TA instance. + * \param[in] ta TA instance to update the circuit state. + * \param[in] circuit a V.24 circuit, one of OSMO_V110_TA_C_* (DTE->DCE). + * \param[in] active activate (true) or deactivate (false) the circuit. + * \returns 0 on success; negative on error. */ +int osmo_v110_ta_set_circuit(struct osmo_v110_ta *ta, + enum osmo_v110_ta_circuit circuit, bool active) +{ + unsigned int status = ta->state.v24_flags; + + /* permit setting only DTE->DCE circuits */ + switch (circuit) { + case OSMO_V110_TA_C_105: + case OSMO_V110_TA_C_108: + case OSMO_V110_TA_C_133: + break; + default: + LOGPFSML(ta->fi, LOGL_ERROR, + "Setting circuit %s is not permitted (wrong direction?)\n", + osmo_v110_ta_circuit_name(circuit)); + return -EACCES; + } + + if (active) + V24_FLAGMASK_SET_ON(status, circuit); + else + V24_FLAGMASK_SET_OFF(status, circuit); + + return v110_ta_set_status(ta, status); +} diff --git a/src/pseudotalloc/Makefile.am b/src/pseudotalloc/Makefile.am index 3c78bba8..030b281b 100644 --- a/src/pseudotalloc/Makefile.am +++ b/src/pseudotalloc/Makefile.am @@ -1,4 +1,5 @@ -AM_CFLAGS = -Wall -I. $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)/include +AM_CPPFLAGS = -I. -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall if ENABLE_PSEUDOTALLOC diff --git a/src/pseudotalloc/pseudotalloc.c b/src/pseudotalloc/pseudotalloc.c index 25425e57..895c6dd0 100644 --- a/src/pseudotalloc/pseudotalloc.c +++ b/src/pseudotalloc/pseudotalloc.c @@ -55,21 +55,21 @@ void talloc_set_name_const(const void *ptr, const char *name) { } -char *talloc_strdup(const void *context, const char *p) +void *talloc_memdup(const void *ctx, const void *p, size_t size) { - char *ptr; - size_t len; + void *ptr; - if (!p) - return NULL; - len = strlen(p); + ptr = talloc_size(ctx, size); + if (ptr && p) + memcpy(ptr, p, size); + return ptr; +} - ptr = talloc_size(context, len+1); - if (!ptr) +char *talloc_strdup(const void *ctx, const char *p) +{ + if (!p) return NULL; - memcpy(ptr, p, len+1); - - return ptr; + return talloc_memdup(ctx, p, strlen(p) + 1); } void *talloc_pool(const void *context, size_t size) diff --git a/src/pseudotalloc/talloc.h b/src/pseudotalloc/talloc.h index d257a981..38935991 100644 --- a/src/pseudotalloc/talloc.h +++ b/src/pseudotalloc/talloc.h @@ -50,7 +50,8 @@ int _talloc_free(void *ptr, const char *location); void *talloc_named_const(const void *context, size_t size, const char *name); void *talloc_named(const void *context, size_t size, const char *fmt, ...); void talloc_set_name_const(const void *ptr, const char *name); -char *talloc_strdup(const void *t, const char *p); +void *talloc_memdup(const void *ctx, const void *p, size_t size); +char *talloc_strdup(const void *ctx, const char *p); void *talloc_pool(const void *context, size_t size); #define talloc_array(ctx, type, count) (type *)_talloc_array(ctx, sizeof(type), count, #type) void *_talloc_array(const void *ctx, size_t el_size, unsigned count, const char *name); diff --git a/src/select.c b/src/select.c deleted file mode 100644 index b997122e..00000000 --- a/src/select.c +++ /dev/null @@ -1,398 +0,0 @@ -/*! \file select.c - * select filedescriptor handling. - * Taken from: - * userspace logging daemon for the iptables ULOG target - * of the linux 2.4 netfilter subsystem. */ -/* - * (C) 2000-2009 by Harald Welte <laforge@gnumonks.org> - * All Rights Reserverd. - * - * SPDX-License-Identifier: GPL-2.0+ - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 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 General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - * MA 02110-1301, USA. - */ - -#include <fcntl.h> -#include <stdio.h> -#include <unistd.h> -#include <string.h> -#include <stdbool.h> -#include <errno.h> - -#include <osmocom/core/select.h> -#include <osmocom/core/linuxlist.h> -#include <osmocom/core/timer.h> -#include <osmocom/core/logging.h> -#include <osmocom/core/talloc.h> -#include <osmocom/core/utils.h> - -#include "../config.h" - -#ifdef HAVE_SYS_SELECT_H -#include <sys/select.h> - -/*! \addtogroup select - * @{ - * select() loop abstraction - * - * \file select.c */ - -/* keep a set of file descriptors per-thread, so that each thread can have its own - * distinct set of file descriptors to interact with */ -static __thread int maxfd = 0; -static __thread struct llist_head osmo_fds; /* TLS cannot use LLIST_HEAD() */ -static __thread int unregistered_count; - -/*! Set up an osmo-fd. Will not register it. - * \param[inout] ofd Osmo FD to be set-up - * \param[in] fd OS-level file descriptor number - * \param[in] when bit-mask of OSMO_FD_{READ,WRITE,EXECEPT} - * \param[in] cb Call-back function to be called - * \param[in] data Private context pointer - * \param[in] priv_nr Private number - */ -void osmo_fd_setup(struct osmo_fd *ofd, int fd, unsigned int when, - int (*cb)(struct osmo_fd *fd, unsigned int what), - void *data, unsigned int priv_nr) -{ - ofd->fd = fd; - ofd->when = when; - ofd->cb = cb; - ofd->data = data; - ofd->priv_nr = priv_nr; -} - -/*! Check if a file descriptor is already registered - * \param[in] fd osmocom file descriptor to be checked - * \returns true if registered; otherwise false - */ -bool osmo_fd_is_registered(struct osmo_fd *fd) -{ - struct osmo_fd *entry; - llist_for_each_entry(entry, &osmo_fds, list) { - if (entry == fd) { - return true; - } - } - - return false; -} - -/*! Register a new file descriptor with select loop abstraction - * \param[in] fd osmocom file descriptor to be registered - * \returns 0 on success; negative in case of error - */ -int osmo_fd_register(struct osmo_fd *fd) -{ - int flags; - - /* make FD nonblocking */ - flags = fcntl(fd->fd, F_GETFL); - if (flags < 0) - return flags; - flags |= O_NONBLOCK; - flags = fcntl(fd->fd, F_SETFL, flags); - if (flags < 0) - return flags; - - /* set close-on-exec flag */ - flags = fcntl(fd->fd, F_GETFD); - if (flags < 0) - return flags; - flags |= FD_CLOEXEC; - flags = fcntl(fd->fd, F_SETFD, flags); - if (flags < 0) - return flags; - - /* Register FD */ - if (fd->fd > maxfd) - maxfd = fd->fd; - -#ifdef BSC_FD_CHECK - if (osmo_fd_is_registered(fd)) { - fprintf(stderr, "Adding a osmo_fd that is already in the list.\n"); - return 0; - } -#endif - - llist_add_tail(&fd->list, &osmo_fds); - - return 0; -} - -/*! Unregister a file descriptor from select loop abstraction - * \param[in] fd osmocom file descriptor to be unregistered - */ -void osmo_fd_unregister(struct osmo_fd *fd) -{ - /* Note: when fd is inside the osmo_fds list (not registered before) - * this function will crash! If in doubt, check file descriptor with - * osmo_fd_is_registered() */ - unregistered_count++; - llist_del(&fd->list); -} - -/*! Close a file descriptor, mark it as closed + unregister from select loop abstraction - * \param[in] fd osmocom file descriptor to be unregistered + closed - * - * If \a fd is registered, we unregister it from the select() loop - * abstraction. We then close the fd and set it to -1, as well as - * unsetting any 'when' flags */ -void osmo_fd_close(struct osmo_fd *fd) -{ - if (osmo_fd_is_registered(fd)) - osmo_fd_unregister(fd); - if (fd->fd != -1) - close(fd->fd); - fd->fd = -1; - fd->when = 0; -} - -/*! Populate the fd_sets and return the highest fd number - * \param[in] _rset The readfds to populate - * \param[in] _wset The wrtiefds to populate - * \param[in] _eset The errorfds to populate - * - * \returns The highest file descriptor seen or 0 on an empty list - */ -inline int osmo_fd_fill_fds(void *_rset, void *_wset, void *_eset) -{ - fd_set *readset = _rset, *writeset = _wset, *exceptset = _eset; - struct osmo_fd *ufd; - int highfd = 0; - - llist_for_each_entry(ufd, &osmo_fds, list) { - if (ufd->when & OSMO_FD_READ) - FD_SET(ufd->fd, readset); - - if (ufd->when & OSMO_FD_WRITE) - FD_SET(ufd->fd, writeset); - - if (ufd->when & OSMO_FD_EXCEPT) - FD_SET(ufd->fd, exceptset); - - if (ufd->fd > highfd) - highfd = ufd->fd; - } - - return highfd; -} - -inline int osmo_fd_disp_fds(void *_rset, void *_wset, void *_eset) -{ - struct osmo_fd *ufd, *tmp; - int work = 0; - fd_set *readset = _rset, *writeset = _wset, *exceptset = _eset; - -restart: - unregistered_count = 0; - llist_for_each_entry_safe(ufd, tmp, &osmo_fds, list) { - int flags = 0; - - if (FD_ISSET(ufd->fd, readset)) { - flags |= OSMO_FD_READ; - FD_CLR(ufd->fd, readset); - } - - if (FD_ISSET(ufd->fd, writeset)) { - flags |= OSMO_FD_WRITE; - FD_CLR(ufd->fd, writeset); - } - - if (FD_ISSET(ufd->fd, exceptset)) { - flags |= OSMO_FD_EXCEPT; - FD_CLR(ufd->fd, exceptset); - } - - if (flags) { - work = 1; - /* make sure to clear any log context before processing the next incoming message - * as part of some file descriptor callback. This effectively prevents "context - * leaking" from processing of one message into processing of the next message as part - * of one iteration through the list of file descriptors here. See OS#3813 */ - log_reset_context(); - ufd->cb(ufd, flags); - } - /* ugly, ugly hack. If more than one filedescriptor was - * unregistered, they might have been consecutive and - * llist_for_each_entry_safe() is no longer safe */ - /* this seems to happen with the last element of the list as well */ - if (unregistered_count >= 1) - goto restart; - } - - return work; -} - -static int _osmo_select_main(int polling) -{ - fd_set readset, writeset, exceptset; - int rc; - struct timeval no_time = {0, 0}; - - FD_ZERO(&readset); - FD_ZERO(&writeset); - FD_ZERO(&exceptset); - - /* prepare read and write fdsets */ - osmo_fd_fill_fds(&readset, &writeset, &exceptset); - - if (!polling) - osmo_timers_prepare(); - rc = select(maxfd+1, &readset, &writeset, &exceptset, polling ? &no_time : osmo_timers_nearest()); - if (rc < 0) - return 0; - - /* fire timers */ - osmo_timers_update(); - - OSMO_ASSERT(osmo_ctx->select); - - /* call registered callback functions */ - return osmo_fd_disp_fds(&readset, &writeset, &exceptset); -} - -/*! select main loop integration - * \param[in] polling should we pollonly (1) or block on select (0) - * \returns 0 if no fd handled; 1 if fd handled; negative in case of error - */ -int osmo_select_main(int polling) -{ - int rc = _osmo_select_main(polling); -#ifndef EMBEDDED - if (talloc_total_size(osmo_ctx->select) != 0) { - osmo_panic("You cannot use the 'select' volatile " - "context if you don't use osmo_select_main_ctx()!\n"); - } -#endif - return rc; -} - -#ifndef EMBEDDED -/*! select main loop integration with temporary select-dispatch talloc context - * \param[in] polling should we pollonly (1) or block on select (0) - * \returns 0 if no fd handled; 1 if fd handled; negative in case of error - */ -int osmo_select_main_ctx(int polling) -{ - int rc = _osmo_select_main(polling); - /* free all the children of the volatile 'select' scope context */ - talloc_free_children(osmo_ctx->select); - return rc; -} -#endif - -/*! find an osmo_fd based on the integer fd - * \param[in] fd file descriptor to use as search key - * \returns \ref osmo_fd for \ref fd; NULL in case it doesn't exist */ -struct osmo_fd *osmo_fd_get_by_fd(int fd) -{ - struct osmo_fd *ofd; - - llist_for_each_entry(ofd, &osmo_fds, list) { - if (ofd->fd == fd) - return ofd; - } - return NULL; -} - -/*! initialize the osmocom select abstraction for the current thread */ -void osmo_select_init(void) -{ - INIT_LLIST_HEAD(&osmo_fds); -} - -/* ensure main thread always has pre-initialized osmo_fds */ -static __attribute__((constructor)) void on_dso_load_select(void) -{ - osmo_select_init(); -} - -#ifdef HAVE_SYS_TIMERFD_H -#include <sys/timerfd.h> - -/*! disable the osmocom-wrapped timerfd */ -int osmo_timerfd_disable(struct osmo_fd *ofd) -{ - const struct itimerspec its_null = { - .it_value = { 0, 0 }, - .it_interval = { 0, 0 }, - }; - return timerfd_settime(ofd->fd, 0, &its_null, NULL); -} - -/*! schedule the osmcoom-wrapped timerfd to occur first at \a first, then periodically at \a interval - * \param[in] ofd Osmocom wrapped timerfd - * \param[in] first Relative time at which the timer should first execute (NULL = \a interval) - * \param[in] interval Time interval at which subsequent timer shall fire - * \returns 0 on success; negative on error */ -int osmo_timerfd_schedule(struct osmo_fd *ofd, const struct timespec *first, - const struct timespec *interval) -{ - struct itimerspec its; - - if (ofd->fd < 0) - return -EINVAL; - - /* first expiration */ - if (first) - its.it_value = *first; - else - its.it_value = *interval; - /* repeating interval */ - its.it_interval = *interval; - - return timerfd_settime(ofd->fd, 0, &its, NULL); -} - -/*! setup osmocom-wrapped timerfd - * \param[inout] ofd Osmocom-wrapped timerfd on which to operate - * \param[in] cb Call-back function called when timerfd becomes readable - * \param[in] data Opaque data to be passed on to call-back - * \returns 0 on success; negative on error - * - * We simply initialize the data structures here, but do not yet - * schedule the timer. - */ -int osmo_timerfd_setup(struct osmo_fd *ofd, int (*cb)(struct osmo_fd *, unsigned int), void *data) -{ - ofd->cb = cb; - ofd->data = data; - ofd->when = OSMO_FD_READ; - - if (ofd->fd < 0) { - int rc; - - ofd->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK); - if (ofd->fd < 0) - return ofd->fd; - - rc = osmo_fd_register(ofd); - if (rc < 0) { - close(ofd->fd); - ofd->fd = -1; - return rc; - } - } - return 0; -} - -#endif /* HAVE_SYS_TIMERFD_H */ - - -/*! @} */ - -#endif /* _HAVE_SYS_SELECT_H */ diff --git a/src/sim/Makefile.am b/src/sim/Makefile.am index 1a8e5084..0f6be576 100644 --- a/src/sim/Makefile.am +++ b/src/sim/Makefile.am @@ -1,27 +1,32 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=1:1:1 +LIBVERSION=3:2:1 -AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)/include -AM_CFLAGS = -fPIC -Wall $(PCSC_CFLAGS) $(TALLOC_CFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -fPIC -Wall $(TALLOC_CFLAGS) AM_LDFLAGS = $(COVERAGE_LDFLAGS) -if ENABLE_PCSC -# FIXME: only build the PC/SC dependent part conditional, but always build other parts - noinst_HEADERS = sim_int.h gsm_int.h +if !EMBEDDED lib_LTLIBRARIES = libosmosim.la -libosmosim_la_SOURCES = core.c reader.c reader_pcsc.c class_tables.c \ +libosmosim_la_SOURCES = core.c reader.c class_tables.c \ card_fs_sim.c card_fs_usim.c card_fs_uicc.c \ - card_fs_isim.c card_fs_tetra.c -libosmosim_la_LDFLAGS = -version-info $(LIBVERSION) + card_fs_isim.c card_fs_hpsim.c card_fs_tetra.c +libosmosim_la_LDFLAGS = \ + -version-info $(LIBVERSION) \ + -no-undefined \ + $(NULL) libosmosim_la_LIBADD = \ - $(top_builddir)/src/libosmocore.la \ + $(top_builddir)/src/core/libosmocore.la \ $(top_builddir)/src/gsm/libosmogsm.la \ - $(TALLOC_LIBS) \ - $(PCSC_LIBS) - + $(TALLOC_LIBS) +if ENABLE_PCSC +AM_CFLAGS += $(PCSC_CFLAGS) +libosmosim_la_SOURCES += reader_pcsc.c +libosmosim_la_LIBADD += $(PCSC_LIBS) endif + +endif # !EMBEDDED diff --git a/src/sim/card_fs_hpsim.c b/src/sim/card_fs_hpsim.c new file mode 100644 index 00000000..2c115b75 --- /dev/null +++ b/src/sim/card_fs_hpsim.c @@ -0,0 +1,72 @@ +/*! \file card_fs_hpsim.c + * 3GPP HPSIM specific structures / routines. */ +/* + * (C) 2020 by Harald Welte <laforge@gnumonks.org> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + */ + + +#include <errno.h> +#include <string.h> + +#include <osmocom/sim/sim.h> +#include <osmocom/core/talloc.h> +#include <osmocom/gsm/gsm48.h> + +#include "sim_int.h" +#include "gsm_int.h" + +/* TS 31.104 Version 15.0.0 Release 15 / Chapter 7.1.3 */ +const struct osim_card_sw ts31_104_sw[] = { + { + 0x9862, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, + .u.str = "Security management - Authentication error, incorrect MAC", + }, + OSIM_CARD_SW_LAST +}; + +/* TS 31.104 Version 15.0.0 Release 15 / Chapter 4.2 */ +static const struct osim_file_desc hpsim_ef_in_adf_hpsim[] = { + EF_LIN_FIX_N(0x6F06, 0x06, "EF.ARR", 0, 1, 256, + "Access Rule TLV data objects"), + EF_TRANSP_N(0x6F07, 0x07, "EF.IMST", 0, 9, 9, + "IMSI"), + EF_TRANSP_N(0x6FAD, 0x03, "EF_AD", 0, 4, 8, + "Administrative Data"), +}; + +/* Annex E - TS 101 220 */ +static const uint8_t adf_hpsim_aid[] = { 0xA0, 0x00, 0x00, 0x00, 0x87, 0x10, 0x0A }; + +struct osim_card_app_profile *osim_aprof_hpsim(void *ctx) +{ + struct osim_card_app_profile *aprof; + struct osim_file_desc *iadf; + + aprof = talloc_zero(ctx, struct osim_card_app_profile); + aprof->name = "3GPP HPSIM"; + aprof->sw = ts31_104_sw; + aprof->aid_len = sizeof(adf_hpsim_aid); + memcpy(aprof->aid, adf_hpsim_aid, aprof->aid_len); + + /* ADF.HPSIM with its EF siblings */ + iadf = alloc_adf_with_ef(aprof, adf_hpsim_aid, sizeof(adf_hpsim_aid), "ADF.HPSIM", + hpsim_ef_in_adf_hpsim, ARRAY_SIZE(hpsim_ef_in_adf_hpsim)); + aprof->adf = iadf; + + return aprof; +} diff --git a/src/sim/card_fs_isim.c b/src/sim/card_fs_isim.c index e6ba0d09..1a38da2f 100644 --- a/src/sim/card_fs_isim.c +++ b/src/sim/card_fs_isim.c @@ -1,7 +1,7 @@ /*! \file card_fs_isim.c * 3GPP ISIM specific structures / routines. */ /* - * (C) 2014 by Harald Welte <laforge@gnumonks.org> + * (C) 2014-2020 by Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -34,22 +30,19 @@ #include "sim_int.h" #include "gsm_int.h" -/* TS 31.103 Version 11.2.0 Release 11 / Chapoter 7.1.3 */ +/* TS 31.103 Version 15.5.0 Release 15 / Chapter 7.1.3 */ const struct osim_card_sw ts31_103_sw[] = { { 0x9862, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, .u.str = "Security management - Authentication error, incorrect MAC", + }, { + 0x9864, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, + .u.str = "Security management - Authentication error, security context not supported", }, OSIM_CARD_SW_LAST }; -static const struct osim_card_sw *isim_card_sws[] = { - ts31_103_sw, - ts102221_uicc_sw, - NULL -}; - -/* TS 31.103 Version 11.2.0 Release 11 / Chapoter 4.2 */ +/* TS 31.103 Version 15.5.0 Release 15 / Chapter 4.2 */ static const struct osim_file_desc isim_ef_in_adf_isim[] = { EF_TRANSP_N(0x6F02, 0x02, "EF.IMPI", 0, 1, 256, "IMS private user identity"), @@ -81,28 +74,34 @@ static const struct osim_file_desc isim_ef_in_adf_isim[] = { "Short message service parameters"), EF_LIN_FIX_N(0x6FE7, SFI_NONE, "EF.UICCIARI", F_OPTIONAL, 1, 256, "UICC IARI"), + EF_TRANSP_N(0x6FF7, SFI_NONE, "EF_FromPreferred", F_OPTIONAL, 1, 1, + "From Preferred"), + EF_TRANSP_N(0x6FF8, SFI_NONE, "EF_IMSConfigData", F_OPTIONAL, 3, 128, + "IMS Configuration Data"), + EF_TRANSP_N(0x6FFC, SFI_NONE, "EF_XCAPConfigData", F_OPTIONAL, 1, 128, + "XCAP Configuration Data"), + EF_LIN_FIX_N(0x6FFA, SFI_NONE, "EF_WebRTCURI", F_OPTIONAL, 3, 128, + "WebRTC URI"), }; /* Annex E - TS 101 220 */ static const uint8_t adf_isim_aid[] = { 0xA0, 0x00, 0x00, 0x00, 0x87, 0x10, 0x04 }; -struct osim_card_profile *osim_cprof_isim(void *ctx) +struct osim_card_app_profile *osim_aprof_isim(void *ctx) { - struct osim_card_profile *cprof; - struct osim_file_desc *mf; - - cprof = talloc_zero(ctx, struct osim_card_profile); - cprof->name = "3GPP ISIM"; - cprof->sws = isim_card_sws; - - mf = alloc_df(cprof, 0x3f00, "MF"); + struct osim_card_app_profile *aprof; + struct osim_file_desc *iadf; - cprof->mf = mf; + aprof = talloc_zero(ctx, struct osim_card_app_profile); + aprof->name = "3GPP ISIM"; + aprof->sw = ts31_103_sw; + aprof->aid_len = sizeof(adf_isim_aid); + memcpy(aprof->aid, adf_isim_aid, aprof->aid_len); /* ADF.USIM with its EF siblings */ - add_adf_with_ef(mf, adf_isim_aid, sizeof(adf_isim_aid), - "ADF.ISIM", isim_ef_in_adf_isim, - ARRAY_SIZE(isim_ef_in_adf_isim)); + iadf = alloc_adf_with_ef(aprof, adf_isim_aid, sizeof(adf_isim_aid), "ADF.ISIM", + isim_ef_in_adf_isim, ARRAY_SIZE(isim_ef_in_adf_isim)); + aprof->adf = iadf; - return cprof; + return aprof; } diff --git a/src/sim/card_fs_sim.c b/src/sim/card_fs_sim.c index 3f541f7b..f07a2370 100644 --- a/src/sim/card_fs_sim.c +++ b/src/sim/card_fs_sim.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <errno.h> @@ -431,7 +427,7 @@ int osim_int_cprof_add_gsm(struct osim_file_desc *mf) add_df_with_ef(gsm, 0x5F33, "DF.ACeS", NULL, 0); add_df_with_ef(gsm, 0x5F3C, "DF.MExE", sim_ef_in_mexe, ARRAY_SIZE(sim_ef_in_mexe)); - add_df_with_ef(gsm, 0x5F40, "DF.EIA/TIA-533", NULL, 0); + add_df_with_ef(gsm, 0x5F40, "DF.EIA-TIA-533", NULL, 0); add_df_with_ef(gsm, 0x5F60, "DF.CTS", NULL, 0); add_df_with_ef(gsm, 0x5F70, "DF.SoLSA", sim_ef_in_solsa, ARRAY_SIZE(sim_ef_in_solsa)); diff --git a/src/sim/card_fs_tetra.c b/src/sim/card_fs_tetra.c index 12853a52..597e38b7 100644 --- a/src/sim/card_fs_tetra.c +++ b/src/sim/card_fs_tetra.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <errno.h> diff --git a/src/sim/card_fs_uicc.c b/src/sim/card_fs_uicc.c index af6061cf..87def0e4 100644 --- a/src/sim/card_fs_uicc.c +++ b/src/sim/card_fs_uicc.c @@ -1,7 +1,7 @@ /*! \file card_fs_uicc.c * ETSI UICC specific structures / routines. */ /* - * (C) 2012 by Harald Welte <laforge@gnumonks.org> + * (C) 2012-2020 by Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -17,16 +17,16 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <osmocom/sim/sim.h> +#include <osmocom/core/talloc.h> #include <osmocom/gsm/tlv.h> +#include "sim_int.h" +#include "gsm_int.h" + /* TS 102 221 V10.0.0 / 10.2.1 */ const struct osim_card_sw ts102221_uicc_sw[] = { { @@ -171,6 +171,23 @@ const struct osim_card_sw ts102221_uicc_sw[] = { OSIM_CARD_SW_LAST }; +static const struct osim_card_sw *uicc_card_sws[] = { + ts102221_uicc_sw, + NULL +}; + +/* TS 102 221 Chapter 13.1 */ +static const struct osim_file_desc uicc_ef_in_mf[] = { + EF_LIN_FIX_N(0x2f00, SFI_NONE, "EF.DIR", 0, 1, 32, + "Application directory"), + EF_TRANSP_N(0x2FE2, SFI_NONE, "EF.ICCID", 0, 10, 10, + "ICC Identification"), + EF_TRANSP_N(0x2F05, SFI_NONE, "EF.PL", 0, 2, 20, + "Preferred Languages"), + EF_LIN_FIX_N(0x2F06, SFI_NONE, "EF.ARR", F_OPTIONAL, 1, 256, + "Access Rule Reference"), +}; + const struct value_string ts102221_fcp_vals[14] = { { UICC_FCP_T_FCP, "File control parameters" }, { UICC_FCP_T_FILE_SIZE, "File size" }, @@ -209,3 +226,39 @@ const struct tlv_definition ts102221_fcp_tlv_def = { /* Annex E - TS 101 220 */ static const uint8_t __attribute__((__unused__)) adf_uicc_aid[] = { 0xA0, 0x00, 0x00, 0x00, 0x87, 0x10, 0x01 }; + +struct osim_card_profile *osim_cprof_uicc(void *ctx, bool have_df_gsm) +{ + struct osim_card_profile *cprof; + struct osim_file_desc *mf; + int rc; + + cprof = talloc_zero(ctx, struct osim_card_profile); + cprof->name = "3GPP UICC"; + cprof->sws = uicc_card_sws; // FIXME: extend later + + mf = alloc_df(cprof, 0x3f00, "MF"); + + cprof->mf = mf; + + /* Core UICC Files */ + add_filedesc(mf, uicc_ef_in_mf, ARRAY_SIZE(uicc_ef_in_mf)); + + /* DF.TELECOM hierarchy as sub-directory of MF */ + rc = osim_int_cprof_add_telecom(mf); + if (rc != 0) { + talloc_free(cprof); + return NULL; + } + + if (have_df_gsm) { + /* DF.GSM as sub-directory of MF */ + rc = osim_int_cprof_add_gsm(mf); + if (rc != 0) { + talloc_free(cprof); + return NULL; + } + } + + return cprof; +} diff --git a/src/sim/card_fs_usim.c b/src/sim/card_fs_usim.c index 9e9fc878..8cff3fc3 100644 --- a/src/sim/card_fs_usim.c +++ b/src/sim/card_fs_usim.c @@ -1,7 +1,7 @@ /*! \file card_fs_usim.c * 3GPP USIM specific structures / routines. */ /* - * (C) 2012-2014 by Harald Welte <laforge@gnumonks.org> + * (C) 2012-2020 by Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -15,10 +15,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -32,7 +28,7 @@ #include "sim_int.h" #include "gsm_int.h" -/* TS 31.102 Version 7.7.0 / Chapter 7.3 */ +/* TS 31.102 Version 15.7.0 Release 15 / Chapter 7.3 */ const struct osim_card_sw ts31_102_sw[] = { { 0x9862, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, @@ -43,29 +39,17 @@ const struct osim_card_sw ts31_102_sw[] = { }, { 0x9865, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, .u.str = "Security management - Key freshness error", + }, { + 0x9866, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, + .u.str = "Security management - Authentication error, no memory space available", + }, { + 0x9867, 0xffff, SW_TYPE_STR, SW_CLS_ERROR, + .u.str = "Security management - Authentication error, no memory space available in EF_MUK", }, OSIM_CARD_SW_LAST }; -static const struct osim_card_sw *usim_card_sws[] = { - ts31_102_sw, - ts102221_uicc_sw, - NULL -}; - -/* TS 102 221 Chapter 13.1 */ -static const struct osim_file_desc uicc_ef_in_mf[] = { - EF_LIN_FIX_N(0x2f00, SFI_NONE, "EF.DIR", 0, 1, 32, - "Application directory"), - EF_TRANSP_N(0x2FE2, SFI_NONE, "EF.ICCID", 0, 10, 10, - "ICC Identification"), - EF_TRANSP_N(0x2F05, SFI_NONE, "EF.PL", 0, 2, 20, - "Preferred Languages"), - EF_LIN_FIX_N(0x2F06, SFI_NONE, "EF.ARR", F_OPTIONAL, 1, 256, - "Access Rule Reference"), -}; - -/* 31.102 Chapter 4.4.3 */ +/* 31.102 Version 15.7.0 Release 15 / Chapter 4.4.3 */ static const struct osim_file_desc usim_ef_in_df_gsm_access[] = { EF_TRANSP_N(0x4f20, 0x01, "EF.Kc", 0, 9, 9, "Ciphering Key Kc"), @@ -77,7 +61,7 @@ static const struct osim_file_desc usim_ef_in_df_gsm_access[] = { "Investigation Scan"), }; -/* 31.102 Chapter 4.2 */ +/* 31.102 Version 15.7.0 Release 15 / Chapter 4.2 */ static const struct osim_file_desc usim_ef_in_adf_usim[] = { EF_TRANSP(0x6F05, 0x02, "EF.LI", 0, 2, 16, "Language Indication", &gsm_lp_decode, NULL), @@ -161,7 +145,7 @@ static const struct osim_file_desc usim_ef_in_adf_usim[] = { "Key for hidden phone book entries"), EF_LIN_FIX_N(0x6F4D, SFI_NONE, "EF.BDN", F_OPTIONAL, 15, 32, "Barred Dialling Numbers"), - EF_LIN_FIX_N(0x6F4E, SFI_NONE, "EF.EXT4", F_OPTIONAL, 13, 13, + EF_LIN_FIX_N(0x6F55, SFI_NONE, "EF.EXT4", F_OPTIONAL, 13, 13, "Extension 4"), EF_LIN_FIX_N(0x6F58, SFI_NONE, "EF.CMI", F_OPTIONAL, 2, 16, "Comparison Method Information"), @@ -261,6 +245,39 @@ static const struct osim_file_desc usim_ef_in_adf_usim[] = { "UICC IARI"), EF_TRANSP_N(0x6FEC, SFI_NONE, "EF.PWS", F_OPTIONAL, 1, 32, "Public Warning System"), + EF_LIN_FIX_N(0x6FED, SFI_NONE, "EF_FDNURI", F_OPTIONAL, 1, 128, + "Fixed Dialling Numbers URI"), + EF_LIN_FIX_N(0x6FEE, SFI_NONE, "EF_BDNURI", F_OPTIONAL, 1, 128, + "Barred Dialling Numbers URI"), + EF_LIN_FIX_N(0x6FEF, SFI_NONE, "EF_SDNURI", F_OPTIONAL, 1, 128, + "Service Dialling Numbers URI"), + EF_LIN_FIX_N(0x6FF0, SFI_NONE, "EF_IWL", F_OPTIONAL, 18, 32, + "IMEI(SV) White Lists"), + EF_CYCLIC_N(0x6FF1, SFI_NONE, "EF_IPS", F_OPTIONAL, 4, 4, + "IMEI(SV) Pairing Status"), + EF_LIN_FIX_N(0x6FF2, SFI_NONE, "EF_IPD", F_OPTIONAL, 10, 16, + "IMEI(SV) of Pairing Device"), + EF_TRANSP_N(0x6FF3, SFI_NONE, "EF_ePDGId", F_OPTIONAL, 1, 128, + "Home ePDG Identifier"), + EF_TRANSP_N(0x6FF4, SFI_NONE, "EF_ePDGSelection", F_OPTIONAL, 1, 128, + "ePDG Selection Information"), + EF_TRANSP_N(0x6FF5, SFI_NONE, "EF_ePDGIdEm", F_OPTIONAL, 1, 128, + "Emergency ePDG Identifier"), + EF_TRANSP_N(0x6FF6, SFI_NONE, "EF_ePDGSelectionEm", F_OPTIONAL, 1, 128, + "ePDG Selection Information for Emergency Services"), + EF_TRANSP_N(0x6FF7, SFI_NONE, "EF_FromPreferred", F_OPTIONAL, 1, 1, + "From Preferred"), + EF_TRANSP_N(0x6FF8, SFI_NONE, "EF_IMSConfigData", F_OPTIONAL, 3, 128, + "IMS Configuration Data"), + /* EF TVCONFIG (TV Configuration) has no fixed FID */ + EF_TRANSP_N(0x6FF9, SFI_NONE, "EF_3GPPPSDATAOFF", F_OPTIONAL, 4, 4, + "3GPP PS Data Off"), + EF_LIN_FIX_N(0x6FFA, SFI_NONE, "EF_3GPPPSDATAOFFservicelist", F_OPTIONAL, 1, 128, + "3GPP PS Data Off Service List"), + EF_TRANSP_N(0x6FFC, SFI_NONE, "EF_XCAPConfigData", F_OPTIONAL, 1, 128, + "XCAP Configuration Data"), + EF_TRANSP_N(0x6FFD, SFI_NONE, "EF_EARFCNList", F_OPTIONAL, 1, 128, + "EARFCN list for MTC/NB-IOT UEs"), }; @@ -327,30 +344,86 @@ static const struct osim_file_desc usim_ef_in_df_hnb[] = { "Oprator Home NodeB Name"), }; -/* Annex E - TS 101 220 */ -static const uint8_t adf_usim_aid[] = { 0xA0, 0x00, 0x00, 0x00, 0x87, 0x10, 0x02 }; +/* 31.102 Chapter 4.4.8 */ +static const struct osim_file_desc usim_ef_in_df_prose[] = { + EF_LIN_FIX_N(0x4F01, 0x01, "EF.PROSE_MON", F_OPTIONAL, 1, 64, + "ProSe Monitoring Parameters"), + EF_LIN_FIX_N(0x4F02, 0x02, "EF.PROSE_ANN", F_OPTIONAL, 1, 64, + "ProSe Announcing Parameters"), + EF_LIN_FIX_N(0x4F03, 0x03, "EF.PROSEFUNC", F_OPTIONAL, 1, 64, + "HPLMN ProSe Function"), + EF_TRANSP_N(0x4F04, 0x04, "EF.PROSE_RADIO_COM", F_OPTIONAL, 1, 128, + "ProSe Direct Communication Radio Parameters"), + EF_TRANSP_N(0x4F05, 0x05, "EF.PROSE_RADIO_MON", F_OPTIONAL, 1, 128, + "ProSe Direct Discovery Monitoring Radio Parameters"), + EF_TRANSP_N(0x4F06, 0x06, "EF.PROSE_RADIO_ANN", F_OPTIONAL, 1, 128, + "ProSe Direct Discovery Announcing Radio Parameters"), + EF_LIN_FIX_N(0x4F07, 0x07, "EF.PROSE_POLICY", F_OPTIONAL, 1, 64, + "ProSe Direct Discovery Announcing Radio Parameters"), + EF_LIN_FIX_N(0x4F08, 0x08, "EF.PROSE_PLMN", F_OPTIONAL, 1, 64, + "ProSe PLMN Parametes"), + EF_TRANSP_N(0x4F09, 0x09, "EF.PROSE_GC", F_OPTIONAL, 8, 64, + "ProSe Direct Discovery Announcing Radio Parameters"), + EF_TRANSP_N(0x4F10, 0x10, "EF.PST", F_OPTIONAL, 1, 2, + "ProSe Service Table"), + EF_TRANSP_N(0x4F11, 0x11, "EF.PROSE_UIRC", F_OPTIONAL, 1, 128, + "ProSe UsageInformationReportingConfiguration"), + EF_LIN_FIX_N(0x4F12, 0x12, "EF.PROSE_GM_DISCOVERY", F_OPTIONAL, 1, 64, + "ProSe Group Member Discovery Parameters"), + EF_LIN_FIX_N(0x4F13, 0x13, "EF.PROSE_RELAY", F_OPTIONAL, 1, 64, + "ProSe Relay Parameters"), + EF_TRANSP_N(0x4F14, 0x14, "EF.PROSE_RELAY_DISCOVERY", F_OPTIONAL, 5, 64, + "ProSe Relay Discovery Parameters"), +}; -struct osim_card_profile *osim_cprof_usim(void *ctx) -{ - struct osim_card_profile *cprof; - struct osim_file_desc *mf, *uadf; - int rc; +/* 31.102 Chapter 4.4.9 */ +static const struct osim_file_desc usim_ef_in_df_acdc[] = { + EF_TRANSP_N(0x4F01, 0x01, "EF.ACDC_LIST", F_OPTIONAL, 1, 128, + "ACDC List"), +}; - cprof = talloc_zero(ctx, struct osim_card_profile); - cprof->name = "3GPP USIM"; - cprof->sws = usim_card_sws; +/* 31.102 Chapter 4.4.11 */ +static const struct osim_file_desc usim_ef_in_df_5gs[] = { + EF_TRANSP_N(0x4F01, 0x01, "EF.5GS3GPPLOCI", F_OPTIONAL, 20, 20, + "5GS 3GPP location information"), + EF_TRANSP_N(0x4F02, 0x02, "EF.5GSN3GPPLOCI", F_OPTIONAL, 20, 20, + "5GS non-3GPP location information"), + EF_LIN_FIX_N(0x4F03, 0x03, "EF.5GS3GPPNSC", F_OPTIONAL, 57, 57, + "5GS 3GPP Access NAS Security Context"), + EF_LIN_FIX_N(0x4F04, 0x04, "EF.5GSN3GPPNSC", F_OPTIONAL, 57, 57, + "5GS non-3GPP Access NAS Security Context"), + EF_TRANSP_N(0x4F05, 0x05, "EF.5GAUTHKEYS", F_OPTIONAL, 68, 68, + "5GS authentication keys"), + EF_TRANSP_N(0x4F06, 0x06, "EF.UAC_AIC", F_OPTIONAL, 4, 4, + "UAC Access Identities Configuration"), + EF_TRANSP_N(0x4F07, 0x07, "EF.SUCI_Calc_Info", F_OPTIONAL, 2, 64, + "Subscription Concealed Identifier Calculation Information"), + EF_LIN_FIX_N(0x4F08, 0x08, "EF.OPL5G", F_OPTIONAL, 10, 10, + "5GS Operator PLMN List"), + EF_TRANSP_N(0x4F09, 0x09, "EF.NSI", F_OPTIONAL, 1, 64, + "Network Specific Identifier"), + EF_TRANSP_N(0x4F0A, 0x0A, "EF.Routing_Indicator", F_OPTIONAL, 4, 4, + "Routing Indicator"), +}; - mf = alloc_df(cprof, 0x3f00, "MF"); +/* Annex E - TS 101 220 */ +static const uint8_t adf_usim_aid[] = { 0xA0, 0x00, 0x00, 0x00, 0x87, 0x10, 0x02 }; - cprof->mf = mf; +struct osim_card_app_profile *osim_aprof_usim(void *ctx) +{ + struct osim_card_app_profile *aprof; + struct osim_file_desc *uadf; - /* Core UICC Files */ - add_filedesc(mf, uicc_ef_in_mf, ARRAY_SIZE(uicc_ef_in_mf)); + aprof = talloc_zero(ctx, struct osim_card_app_profile); + aprof->name = "3GPP USIM"; + aprof->sw = ts31_102_sw; + aprof->aid_len = sizeof(adf_usim_aid); + memcpy(aprof->aid, adf_usim_aid, aprof->aid_len); /* ADF.USIM with its EF siblings */ - uadf = add_adf_with_ef(mf, adf_usim_aid, sizeof(adf_usim_aid), - "ADF.USIM", usim_ef_in_adf_usim, - ARRAY_SIZE(usim_ef_in_adf_usim)); + uadf = alloc_adf_with_ef(aprof, adf_usim_aid, sizeof(adf_usim_aid), + "ADF.USIM", usim_ef_in_adf_usim, ARRAY_SIZE(usim_ef_in_adf_usim)); + aprof->adf = uadf; /* DFs under ADF.USIM */ add_df_with_ef(uadf, 0x5F3A, "DF.PHONEBOOK", NULL, 0); @@ -360,6 +433,8 @@ struct osim_card_profile *osim_cprof_usim(void *ctx) ARRAY_SIZE(usim_ef_in_df_mexe)); add_df_with_ef(uadf, 0x5F40, "DF.WLAN", usim_ef_in_df_wlan, ARRAY_SIZE(usim_ef_in_df_wlan)); + add_df_with_ef(uadf, 0x5FC0, "DF.5GS", usim_ef_in_df_5gs, + ARRAY_SIZE(usim_ef_in_df_5gs)); /* Home-NodeB (femtocell) */ add_df_with_ef(uadf, 0x5F50, "DF.HNB", usim_ef_in_df_hnb, ARRAY_SIZE(usim_ef_in_df_hnb)); @@ -368,14 +443,10 @@ struct osim_card_profile *osim_cprof_usim(void *ctx) ARRAY_SIZE(usim_ef_in_solsa)); /* OMA BCAST Smart Card Profile */ add_df_with_ef(uadf, 0x5F80, "DF.BCAST", NULL, 0); + add_df_with_ef(uadf, 0x5F90, "DF.ProSe", usim_ef_in_df_prose, + ARRAY_SIZE(usim_ef_in_df_prose)); + add_df_with_ef(uadf, 0x5FA0, "DF.ACDC", usim_ef_in_df_acdc, + ARRAY_SIZE(usim_ef_in_df_acdc)); - /* DF.GSM and DF.TELECOM hierarchy as sub-directory of MF */ - rc = osim_int_cprof_add_gsm(mf); - rc |= osim_int_cprof_add_telecom(mf); - if (rc != 0) { - talloc_free(cprof); - return NULL; - } - - return cprof; + return aprof; } diff --git a/src/sim/class_tables.c b/src/sim/class_tables.c index 6f541ee2..29c1e40e 100644 --- a/src/sim/class_tables.c +++ b/src/sim/class_tables.c @@ -13,17 +13,13 @@ * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include <stdint.h> #include <osmocom/core/utils.h> #include <osmocom/sim/class_tables.h> -static const uint8_t iso7816_ins_tbl[] = { +static const uint8_t iso7816_ins_tbl[256] = { [0xB0] = 2, /* READ BIN */ [0xD0] = 3, /* WRITE BIN */ [0xD6] = 3, /* UPDATE BIN */ @@ -117,7 +113,7 @@ static const uint8_t uicc_ins_tbl_046[256] = { [0xD6] = 3, /* UPDATE BINARY */ [0xB2] = 2, /* READ RECORD */ [0xDC] = 3, /* UPDATE RECORD */ - [0xA2] = 4, /* SEEK */ + [0xA2] = 4, /* SEARCH RECORD */ [0x20] = 3, /* VERIFY PIN */ [0x24] = 3, /* CHANGE PIN */ [0x26] = 3, /* DISABLE PIN */ @@ -177,6 +173,24 @@ static int uicc046_cla_ins_helper(const struct osim_cla_ins_case *cic, return 0; } +static int gp_cla_ins_helper(const struct osim_cla_ins_case *cic, + const uint8_t *hdr) +{ + uint8_t ins = hdr[1]; + uint8_t p1 = hdr[2]; + + switch (ins) { + case 0xE2: /* STORE DATA */ + switch (p1 & 0x01) { + case 1: + return 4; + default: + return 3; + } + } + return 0; +} + /* ETSI TS 102 221, Table 10.5, CLA = 0x8x, 0xCx or 0xEx */ static const uint8_t uicc_ins_tbl_8ce[256] = { [0xF2] = 2, /* STATUS */ @@ -184,6 +198,7 @@ static const uint8_t uicc_ins_tbl_8ce[256] = { [0xCB] = 4, /* RETRIEVE DATA */ [0xDB] = 3, /* SET DATA */ [0xAA] = 3, /* TERMINAL CAPABILITY */ + [0x78] = 4, /* GET IDENTITY */ }; /* ETSI TS 102 221, Table 10.5, CLA = 0x80 */ @@ -192,6 +207,22 @@ static const uint8_t uicc_ins_tbl_80[256] = { [0xC2] = 4, /* ENVELOPE */ [0x12] = 2, /* FETCH */ [0x14] = 3, /* TERMINAL RESPONSE */ + [0x76] = 4, /* SUSPEND UICC */ + [0x7A] = 4, /* EXCHANGE CAPABILITIES */ +}; + +/* Card Specification v2.3.1*/ +static const uint8_t gp_ins_tbl_8ce[256] = { + [0xE4] = 4, /* DELETE */ + [0xE2] = 0x80, /* STORE DATA */ + [0xCA] = 4, /* GET DATA */ + [0xCB] = 4, /* GET DATA */ + [0xF2] = 4, /* GET STATUS */ + [0xE6] = 4, /* INSTALL */ + [0xE8] = 4, /* LOAD */ + [0xD8] = 4, /* PUT KEY */ + [0xF0] = 3, /* SET STATUS */ + [0xC0] = 2, /* GET RESPONSE */ }; static const struct osim_cla_ins_case uicc_ins_case[] = { @@ -226,6 +257,21 @@ static const struct osim_cla_ins_case uicc_ins_case[] = { .cla = 0xE0, .cla_mask = 0xF0, .ins_tbl = uicc_ins_tbl_8ce, + }, { + .cla = 0x80, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, + }, { + .cla = 0xC0, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, + }, { + .cla = 0xE0, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, }, }; @@ -273,7 +319,23 @@ static const struct osim_cla_ins_case uicc_sim_ins_case[] = { .cla = 0xE0, .cla_mask = 0xF0, .ins_tbl = uicc_ins_tbl_8ce, + }, { + .cla = 0x80, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, + }, { + .cla = 0xC0, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, + }, { + .cla = 0xE0, + .cla_mask = 0xF0, + .helper = gp_cla_ins_helper, + .ins_tbl = gp_ins_tbl_8ce, }, + }; const struct osim_cla_ins_card_profile osim_uicc_sim_cic_profile = { @@ -288,6 +350,13 @@ const uint8_t usim_ins_case[256] = { [0x88] = 4, /* AUTHENTICATE */ }; +/* https://learn.microsoft.com/en-us/windows-hardware/drivers/smartcard/discovery-process */ +static const uint8_t microsoft_discovery_ins_tbl[256] = { + [0xA4] = 4, /* SELECT FILE */ + [0xCA] = 2, /* GET DATA */ + [0xC0] = 2, /* GET RESPONSE */ +}; + int osim_determine_apdu_case(const struct osim_cla_ins_card_profile *prof, const uint8_t *hdr) { @@ -305,12 +374,23 @@ int osim_determine_apdu_case(const struct osim_cla_ins_card_profile *prof, case 0x80: return cic->helper(cic, hdr); case 0x00: - /* continue with fruther cic, rather than abort + /* continue with further cic, rather than abort * now */ continue; default: return rc; } } + /* special handling for Microsoft who insists to use INS=0xCA in CLA=0x00 which is not + * really part of GSM SIM, ETSI UICC or 3GPP USIM specifications, but only ISO7816. Rather than adding + * it to each and every card profile, let's add the instructions listed at + * https://learn.microsoft.com/en-us/windows-hardware/drivers/smartcard/discovery-process explicitly + * here. They will only be used in case no more specific match was found in the actual profile above. */ + if (cla == 0x00) { + rc = microsoft_discovery_ins_tbl[ins]; + if (rc) + return rc; + } + return 0; } diff --git a/src/sim/core.c b/src/sim/core.c index b93633c1..fa17e12d 100644 --- a/src/sim/core.c +++ b/src/sim/core.c @@ -1,7 +1,7 @@ /*! \file core.c * Core routines for SIM/UICC/USIM access. */ /* - * (C) 2012 by Harald Welte <laforge@gnumonks.org> + * (C) 2012-2020 by Harald Welte <laforge@gnumonks.org> * * All Rights Reserved * @@ -17,20 +17,19 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdlib.h> #include <stdint.h> #include <string.h> +#include <errno.h> #include <osmocom/core/talloc.h> #include <osmocom/sim/sim.h> +#include "sim_int.h" + struct osim_decoded_data *osim_file_decode(struct osim_file *file, int len, uint8_t *data) { @@ -154,21 +153,19 @@ add_df_with_ef(struct osim_file_desc *parent, } struct osim_file_desc * -add_adf_with_ef(struct osim_file_desc *parent, +alloc_adf_with_ef(void *ctx, const uint8_t *adf_name, uint8_t adf_name_len, const char *name, const struct osim_file_desc *in, int num) { struct osim_file_desc *df; - df = alloc_df(parent, 0xffff, name); + df = alloc_df(ctx, 0xffff, name); if (!df) return NULL; df->type = TYPE_ADF; df->df_name = adf_name; df->df_name_len = adf_name_len; - df->parent = parent; - llist_add_tail(&df->list, &parent->child_list); add_filedesc(df, in, num); return df; @@ -187,6 +184,22 @@ osim_file_desc_find_name(struct osim_file_desc *parent, const char *name) } struct osim_file_desc * +osim_file_desc_find_aid(struct osim_file_desc *parent, const uint8_t *aid, uint8_t aid_len) +{ + struct osim_file_desc *ofd; + llist_for_each_entry(ofd, &parent->child_list, list) { + if (ofd->type != TYPE_ADF) + continue; + if (aid_len > ofd->df_name_len) + continue; + if (!memcmp(ofd->df_name, aid, aid_len)) { + return ofd; + } + } + return NULL; +} + +struct osim_file_desc * osim_file_desc_find_fid(struct osim_file_desc *parent, uint16_t fid) { struct osim_file_desc *ofd; @@ -213,6 +226,88 @@ osim_file_desc_find_sfid(struct osim_file_desc *parent, uint8_t sfid) } +/*********************************************************************** + * Application Profiles + Applications + ***********************************************************************/ + +static LLIST_HEAD(g_app_profiles); + +/*! Register an application profile. Typically called at early start-up. */ +void osim_app_profile_register(struct osim_card_app_profile *aprof) +{ + OSMO_ASSERT(!osim_app_profile_find_by_name(aprof->name)); + OSMO_ASSERT(!osim_app_profile_find_by_aid(aprof->aid, aprof->aid_len)); + llist_add_tail(&aprof->list, &g_app_profiles); +} + +/*! Find any registered application profile based on its name (e.g. "ADF.USIM") */ +const struct osim_card_app_profile * +osim_app_profile_find_by_name(const char *name) +{ + struct osim_card_app_profile *ap; + + llist_for_each_entry(ap, &g_app_profiles, list) { + if (!strcmp(name, ap->name)) + return ap; + } + return NULL; +} + +/*! Find any registered application profile based on its AID */ +const struct osim_card_app_profile * +osim_app_profile_find_by_aid(const uint8_t *aid, uint8_t aid_len) +{ + struct osim_card_app_profile *ap; + + llist_for_each_entry(ap, &g_app_profiles, list) { + if (ap->aid_len > aid_len) + continue; + if (!memcmp(ap->aid, aid, ap->aid_len)) + return ap; + } + return NULL; +} + +struct osim_card_app_hdl * +osim_card_hdl_find_app(struct osim_card_hdl *ch, const uint8_t *aid, uint8_t aid_len) +{ + struct osim_card_app_hdl *ah; + + if (aid_len > MAX_AID_LEN) + return NULL; + + llist_for_each_entry(ah, &ch->apps, list) { + if (!memcmp(ah->aid, aid, aid_len)) + return ah; + } + return NULL; +} + +/*! Add an application to a given card */ +int osim_card_hdl_add_app(struct osim_card_hdl *ch, const uint8_t *aid, uint8_t aid_len, + const char *label) +{ + struct osim_card_app_hdl *ah; + + if (aid_len > MAX_AID_LEN) + return -EINVAL; + + if (osim_card_hdl_find_app(ch, aid, aid_len)) + return -EEXIST; + + ah = talloc_zero(ch, struct osim_card_app_hdl); + if (!ah) + return -ENOMEM; + + memcpy(ah->aid, aid, aid_len); + ah->aid_len = aid_len; + ah->prof = osim_app_profile_find_by_aid(ah->aid, ah->aid_len); + if (label) + ah->label = talloc_strdup(ah, label); + llist_add_tail(&ah->list, &ch->apps); + return 0; +} + /*! Generate an APDU message and initialize APDU command header * \param[in] cla CLASS byte * \param[in] ins INSTRUCTION byte @@ -268,14 +363,19 @@ struct msgb *osim_new_apdumsg(uint8_t cla, uint8_t ins, uint8_t p1, } -char *osim_print_sw_buf(char *buf, size_t buf_len, const struct osim_card_hdl *ch, uint16_t sw_in) +char *osim_print_sw_buf(char *buf, size_t buf_len, const struct osim_chan_hdl *ch, uint16_t sw_in) { - const struct osim_card_sw *csw; + const struct osim_card_sw *csw = NULL; - if (!ch || !ch->prof) + if (!ch) goto ret_def; - csw = osim_find_sw(ch->prof, sw_in); + if (ch->cur_app && ch->cur_app->prof) + csw = osim_app_profile_find_sw(ch->cur_app->prof, sw_in); + + if (!csw && ch->card->prof) + csw = osim_cprof_find_sw(ch->card->prof, sw_in); + if (!csw) goto ret_def; @@ -298,13 +398,13 @@ ret_def: return buf; } -char *osim_print_sw(const struct osim_card_hdl *ch, uint16_t sw_in) +char *osim_print_sw(const struct osim_chan_hdl *ch, uint16_t sw_in) { static __thread char sw_print_buf[256]; return osim_print_sw_buf(sw_print_buf, sizeof(sw_print_buf), ch, sw_in); } -char *osim_print_sw_c(const void *ctx, const struct osim_card_hdl *ch, uint16_t sw_in) +char *osim_print_sw_c(const void *ctx, const struct osim_chan_hdl *ch, uint16_t sw_in) { char *buf = talloc_size(ctx, 256); if (!buf) @@ -312,8 +412,8 @@ char *osim_print_sw_c(const void *ctx, const struct osim_card_hdl *ch, uint16_t return osim_print_sw_buf(buf, 256, ch, sw_in); } -const struct osim_card_sw *osim_find_sw(const struct osim_card_profile *cp, - uint16_t sw_in) +/*! Find status word within given card profile */ +const struct osim_card_sw *osim_cprof_find_sw(const struct osim_card_profile *cp, uint16_t sw_in) { const struct osim_card_sw **sw_lists = cp->sws; const struct osim_card_sw *sw_list, *sw; @@ -327,10 +427,30 @@ const struct osim_card_sw *osim_find_sw(const struct osim_card_profile *cp, return NULL; } -enum osim_card_sw_class osim_sw_class(const struct osim_card_profile *cp, - uint16_t sw_in) +/*! Find application-specific status word within given card application profile */ +const struct osim_card_sw *osim_app_profile_find_sw(const struct osim_card_app_profile *ap, uint16_t sw_in) +{ + const struct osim_card_sw *sw_list = ap->sw, *sw; + + for (sw = sw_list; sw->code != 0 && sw->mask != 0; sw++) { + if ((sw_in & sw->mask) == sw->code) + return sw; + } + return NULL; +} + +enum osim_card_sw_class osim_sw_class(const struct osim_chan_hdl *ch, uint16_t sw_in) { - const struct osim_card_sw *csw = osim_find_sw(cp, sw_in); + const struct osim_card_sw *csw = NULL; + + OSMO_ASSERT(ch); + OSMO_ASSERT(ch->card); + + if (ch->cur_app && ch->cur_app->prof) + csw = osim_app_profile_find_sw(ch->cur_app->prof, sw_in); + + if (!csw && ch->card->prof) + csw = osim_cprof_find_sw(ch->card->prof, sw_in); if (!csw) return SW_CLS_NONE; @@ -349,3 +469,12 @@ int default_decode(struct osim_decoded_data *dd, return 0; } + +int osim_init(void *ctx) +{ + osim_app_profile_register(osim_aprof_usim(ctx)); + osim_app_profile_register(osim_aprof_isim(ctx)); + osim_app_profile_register(osim_aprof_hpsim(ctx)); + + return 0; +} diff --git a/src/sim/reader.c b/src/sim/reader.c index d1a9ae60..982b2eef 100644 --- a/src/sim/reader.c +++ b/src/sim/reader.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -35,6 +31,7 @@ #include <osmocom/core/msgb.h> #include <osmocom/sim/sim.h> +#include "config.h" #include "sim_int.h" @@ -43,7 +40,7 @@ static int get_sw(struct msgb *resp) { int ret; - if (!msgb_apdu_de(resp) || msgb_apdu_le(resp) < 2) + if (!resp->l2h || msgb_apdu_le(resp) < 2) return -EIO; ret = msgb_get_u16(resp); @@ -122,7 +119,6 @@ transceive_again: /* save SW */ sw = msgb_apdu_sw(tmsg); - printf("sw = 0x%04x\n", sw); msgb_apdu_sw(amsg) = sw; switch (msgb_apdu_case(amsg)) { @@ -242,9 +238,11 @@ struct osim_reader_hdl *osim_reader_open(enum osim_reader_driver driver, int idx struct osim_reader_hdl *rh; switch (driver) { +#ifdef HAVE_PCSC case OSIM_READER_DRV_PCSC: ops = &pcsc_reader_ops; break; +#endif default: return NULL; } @@ -275,3 +273,24 @@ struct osim_card_hdl *osim_card_open(struct osim_reader_hdl *rh, enum osim_proto return ch; } + +int osim_card_reset(struct osim_card_hdl *card, bool cold_reset) +{ + struct osim_reader_hdl *rh = card->reader; + + return rh->ops->card_reset(card, cold_reset); +} + +int osim_card_close(struct osim_card_hdl *card) +{ + struct osim_reader_hdl *rh = card->reader; + int rc; + + rc = rh->ops->card_close(card); + + card->reader = NULL; + talloc_free(card); + rh->card = NULL; + + return rc; +} diff --git a/src/sim/reader_pcsc.c b/src/sim/reader_pcsc.c index f22103f4..c37072ee 100644 --- a/src/sim/reader_pcsc.c +++ b/src/sim/reader_pcsc.c @@ -17,10 +17,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ @@ -41,12 +37,9 @@ if (rv != SCARD_S_SUCCESS) { \ fprintf(stderr, text ": %s (0x%lX)\n", pcsc_stringify_error(rv), rv); \ goto end; \ -} else { \ - printf(text ": OK\n\n"); \ } - struct pcsc_reader_state { SCARDCONTEXT hContext; SCARDHANDLE hCard; @@ -56,6 +49,27 @@ struct pcsc_reader_state { char *name; }; +static int pcsc_get_atr(struct osim_card_hdl *card) +{ + struct osim_reader_hdl *rh = card->reader; + struct pcsc_reader_state *st = rh->priv; + char pbReader[MAX_READERNAME]; + DWORD dwReaderLen = sizeof(pbReader); + DWORD dwAtrLen = sizeof(card->atr); + DWORD dwState, dwProt; + long rc; + + rc = SCardStatus(st->hCard, pbReader, &dwReaderLen, &dwState, &dwProt, + card->atr, &dwAtrLen); + PCSC_ERROR(rc, "SCardStatus"); + card->atr_len = dwAtrLen; + + return 0; + +end: + return -EIO; +} + static struct osim_reader_hdl *pcsc_reader_open(int num, const char *id, void *ctx) { struct osim_reader_hdl *rh; @@ -79,18 +93,22 @@ static struct osim_reader_hdl *pcsc_reader_open(int num, const char *id, void *c rc = SCardListReaders(st->hContext, NULL, (LPSTR)&mszReaders, &dwReaders); PCSC_ERROR(rc, "SCardListReaders"); + /* SCARD_S_SUCCESS means there is at least one reader in the group */ num_readers = 0; ptr = mszReaders; - while (*ptr != '\0') { + while (*ptr != '\0' && num_readers != num) { ptr += strlen(ptr)+1; num_readers++; } - if (num_readers == 0) + if (num != num_readers) { + SCardFreeMemory(st->hContext, mszReaders); goto end; + } - st->name = talloc_strdup(rh, mszReaders); + st->name = talloc_strdup(rh, ptr); st->dwActiveProtocol = -1; + SCardFreeMemory(st->hContext, mszReaders); return rh; end: @@ -117,6 +135,7 @@ static struct osim_card_hdl *pcsc_card_open(struct osim_reader_hdl *rh, card = talloc_zero(rh, struct osim_card_hdl); INIT_LLIST_HEAD(&card->channels); + INIT_LLIST_HEAD(&card->apps); card->reader = rh; rh->card = card; @@ -125,12 +144,42 @@ static struct osim_card_hdl *pcsc_card_open(struct osim_reader_hdl *rh, chan->card = card; llist_add(&chan->list, &card->channels); + pcsc_get_atr(card); + return card; end: return NULL; } +static int pcsc_card_reset(struct osim_card_hdl *card, bool cold_reset) +{ + struct pcsc_reader_state *st = card->reader->priv; + LONG rc; + + rc = SCardReconnect(st->hCard, SCARD_SHARE_SHARED, SCARD_PROTOCOL_T0, + cold_reset ? SCARD_UNPOWER_CARD : SCARD_RESET_CARD, + &st->dwActiveProtocol); + PCSC_ERROR(rc, "SCardReconnect"); + + return 0; +end: + return -EIO; +} + +static int pcsc_card_close(struct osim_card_hdl *card) +{ + struct pcsc_reader_state *st = card->reader->priv; + LONG rc; + + rc = SCardDisconnect(st->hCard, SCARD_UNPOWER_CARD); + PCSC_ERROR(rc, "SCardDisconnect"); + + return 0; +end: + return -EIO; +} + static int pcsc_transceive(struct osim_reader_hdl *rh, struct msgb *msg) { @@ -138,13 +187,10 @@ static int pcsc_transceive(struct osim_reader_hdl *rh, struct msgb *msg) DWORD rlen = msgb_tailroom(msg); LONG rc; - printf("TX: %s\n", osmo_hexdump(msg->data, msg->len)); - rc = SCardTransmit(st->hCard, st->pioSendPci, msg->data, msgb_length(msg), &st->pioRecvPci, msg->tail, &rlen); PCSC_ERROR(rc, "SCardEndTransaction"); - printf("RX: %s\n", osmo_hexdump(msg->tail, rlen)); msgb_put(msg, rlen); msgb_apdu_le(msg) = rlen; @@ -157,6 +203,8 @@ const struct osim_reader_ops pcsc_reader_ops = { .name = "PC/SC", .reader_open = pcsc_reader_open, .card_open = pcsc_card_open, + .card_reset = pcsc_card_reset, + .card_close = pcsc_card_close, .transceive = pcsc_transceive, }; diff --git a/src/sim/sim_int.h b/src/sim/sim_int.h index 885011ed..a96a9cd2 100644 --- a/src/sim/sim_int.h +++ b/src/sim/sim_int.h @@ -12,8 +12,6 @@ struct osim_decoded_element * element_alloc_sub(struct osim_decoded_element *ee, const char *name, enum osim_element_type type, enum osim_element_repr repr); -extern const struct osim_card_sw ts102221_uicc_sw[0]; - int default_decode(struct osim_decoded_data *dd, const struct osim_file_desc *desc, int len, uint8_t *data); @@ -26,11 +24,15 @@ add_df_with_ef(struct osim_file_desc *parent, const struct osim_file_desc *in, int num); struct osim_file_desc * -add_adf_with_ef(struct osim_file_desc *parent, - const uint8_t *adf_name, uint8_t adf_name_len, - const char *name, const struct osim_file_desc *in, - int num); +alloc_adf_with_ef(void *ctx, const uint8_t *adf_name, uint8_t adf_name_len, + const char *name, const struct osim_file_desc *in, int num); extern const struct osim_reader_ops pcsc_reader_ops; +void osim_app_profile_register(struct osim_card_app_profile *aprof); + +struct osim_card_app_profile *osim_aprof_usim(void *ctx); +struct osim_card_app_profile *osim_aprof_isim(void *ctx); +struct osim_card_app_profile *osim_aprof_hpsim(void *ctx); + #endif diff --git a/src/socket.c b/src/socket.c deleted file mode 100644 index 9b1d30ec..00000000 --- a/src/socket.c +++ /dev/null @@ -1,1320 +0,0 @@ -/* - * (C) 2011-2017 by Harald Welte <laforge@gnumonks.org> - * - * All Rights Reserved - * - * SPDX-License-Identifier: GPL-2.0+ - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 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 General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * - */ - -#include "../config.h" - -/*! \addtogroup socket - * @{ - * Osmocom socket convenience functions. - * - * \file socket.c */ - -#ifdef HAVE_SYS_SOCKET_H - -#include <osmocom/core/logging.h> -#include <osmocom/core/select.h> -#include <osmocom/core/socket.h> -#include <osmocom/core/talloc.h> -#include <osmocom/core/utils.h> - -#include <sys/ioctl.h> -#include <sys/socket.h> -#include <sys/types.h> -#include <sys/un.h> - -#include <netinet/in.h> -#include <arpa/inet.h> - -#include <stdio.h> -#include <unistd.h> -#include <stdint.h> -#include <string.h> -#include <errno.h> -#include <netdb.h> -#include <ifaddrs.h> - -#ifdef HAVE_LIBSCTP -#include <netinet/sctp.h> -#endif - -static struct addrinfo *addrinfo_helper(uint16_t family, uint16_t type, uint8_t proto, - const char *host, uint16_t port, bool passive) -{ - struct addrinfo hints, *result, *rp; - char portbuf[6]; - int rc; - - snprintf(portbuf, sizeof(portbuf), "%u", port); - memset(&hints, 0, sizeof(struct addrinfo)); - hints.ai_family = family; - if (type == SOCK_RAW) { - /* Workaround for glibc, that returns EAI_SERVICE (-8) if - * SOCK_RAW and IPPROTO_GRE is used. - * http://sourceware.org/bugzilla/show_bug.cgi?id=15015 - */ - hints.ai_socktype = SOCK_DGRAM; - hints.ai_protocol = IPPROTO_UDP; - } else { - hints.ai_socktype = type; - hints.ai_protocol = proto; - } - - if (passive) - hints.ai_flags |= AI_PASSIVE; - - rc = getaddrinfo(host, portbuf, &hints, &result); - if (rc != 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "getaddrinfo returned NULL: %s:%u: %s\n", - host, port, strerror(errno)); - return NULL; - } - - for (rp = result; rp != NULL; rp = rp->ai_next) { - /* Workaround for glibc again */ - if (type == SOCK_RAW) { - rp->ai_socktype = SOCK_RAW; - rp->ai_protocol = proto; - } - } - - return result; -} - -#ifdef HAVE_LIBSCTP -/*! Retrieve an array of addrinfo with specified hints, one for each host in the hosts array. - * \param[out] addrinfo array of addrinfo pointers, will be filled by the function on success. - * Its size must be at least the one of hosts. - * \param[in] family Socket family like AF_INET, AF_INET6. - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM. - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP. - * \param[in] hosts array of char pointers (strings) containing the addresses to query. - * \param[in] host_cnt length of the hosts array (in items). - * \param[in] port port number in host byte order. - * \param[in] passive whether to include the AI_PASSIVE flag in getaddrinfo() hints. - * \returns 0 is returned on success together with a filled addrinfo array; negative on error - */ -static int addrinfo_helper_multi(struct addrinfo **addrinfo, uint16_t family, uint16_t type, uint8_t proto, - const char **hosts, size_t host_cnt, uint16_t port, bool passive) -{ - int i, j; - - for (i = 0; i < host_cnt; i++) { - addrinfo[i] = addrinfo_helper(family, type, proto, hosts[i], port, passive); - if (!addrinfo[i]) { - for (j = 0; j < i; j++) - freeaddrinfo(addrinfo[j]); - return -EINVAL; - } - } - return 0; -} -#endif /* HAVE_LIBSCTP*/ - -static int socket_helper(const struct addrinfo *rp, unsigned int flags) -{ - int sfd, on = 1; - - sfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); - if (sfd == -1) { - LOGP(DLGLOBAL, LOGL_ERROR, - "unable to create socket: %s\n", strerror(errno)); - return sfd; - } - if (flags & OSMO_SOCK_F_NONBLOCK) { - if (ioctl(sfd, FIONBIO, (unsigned char *)&on) < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot set this socket unblocking: %s\n", - strerror(errno)); - close(sfd); - sfd = -EINVAL; - } - } - return sfd; -} - -#ifdef HAVE_LIBSCTP -/* Fill buf with a string representation of the address set, in the form: - * buf_len == 0: "()" - * buf_len == 1: "hostA" - * buf_len >= 2: (hostA|hostB|...|...) - */ -static int multiaddr_snprintf(char* buf, size_t buf_len, const char **hosts, size_t host_cnt) -{ - int len = 0, offset = 0, rem = buf_len; - int ret, i; - char *after; - - if (buf_len < 3) - return -EINVAL; - - if (host_cnt != 1) { - ret = snprintf(buf, rem, "("); - if (ret < 0) - return ret; - OSMO_SNPRINTF_RET(ret, rem, offset, len); - } - for (i = 0; i < host_cnt; i++) { - if (host_cnt == 1) - after = ""; - else - after = (i == (host_cnt - 1)) ? ")" : "|"; - ret = snprintf(buf + offset, rem, "%s%s", hosts[i] ? : "0.0.0.0", after); - OSMO_SNPRINTF_RET(ret, rem, offset, len); - } - - return len; -} -#endif /* HAVE_LIBSCTP */ - -static int osmo_sock_init_tail(int fd, uint16_t type, unsigned int flags) -{ - int rc; - - /* Make sure to call 'listen' on a bound, connection-oriented sock */ - if ((flags & (OSMO_SOCK_F_BIND|OSMO_SOCK_F_CONNECT)) == OSMO_SOCK_F_BIND) { - switch (type) { - case SOCK_STREAM: - case SOCK_SEQPACKET: - rc = listen(fd, 10); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to listen on socket: %s\n", - strerror(errno)); - return rc; - } - break; - } - } - - if (flags & OSMO_SOCK_F_NO_MCAST_LOOP) { - rc = osmo_sock_mcast_loop_set(fd, false); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to disable multicast loop: %s\n", - strerror(errno)); - return rc; - } - } - - if (flags & OSMO_SOCK_F_NO_MCAST_ALL) { - rc = osmo_sock_mcast_all_set(fd, false); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to disable receive of all multicast: %s\n", - strerror(errno)); - /* do not abort here, as this is just an - * optional additional optimization that only - * exists on Linux only */ - } - } - return 0; -} - -/*! Initialize a socket (including bind and/or connect) - * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] local_host local host name or IP address in string form - * \param[in] local_port local port number in host byte order - * \param[in] remote_host remote host name or IP address in string form - * \param[in] remote_port remote port number in host byte order - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket file descriptor on success; negative on error - * - * This function creates a new socket of the designated \a family, \a - * type and \a proto and optionally binds it to the \a local_host and \a - * local_port as well as optionally connects it to the \a remote_host - * and \q remote_port, depending on the value * of \a flags parameter. - * - * As opposed to \ref osmo_sock_init(), this function allows to combine - * the \ref OSMO_SOCK_F_BIND and \ref OSMO_SOCK_F_CONNECT flags. This - * is useful if you want to connect to a remote host/port, but still - * want to bind that socket to either a specific local alias IP and/or a - * specific local source port. - * - * You must specify either \ref OSMO_SOCK_F_BIND, or \ref - * OSMO_SOCK_F_CONNECT, or both. - * - * If \ref OSMO_SOCK_F_NONBLOCK is specified, the socket will be set to - * non-blocking mode. - */ -int osmo_sock_init2(uint16_t family, uint16_t type, uint8_t proto, - const char *local_host, uint16_t local_port, - const char *remote_host, uint16_t remote_port, unsigned int flags) -{ - struct addrinfo *result, *rp; - int sfd = -1, rc, on = 1; - - if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "invalid: you have to specify either " - "BIND or CONNECT flags\n"); - return -EINVAL; - } - - /* figure out local side of socket */ - if (flags & OSMO_SOCK_F_BIND) { - result = addrinfo_helper(family, type, proto, local_host, local_port, true); - if (!result) - return -EINVAL; - - for (rp = result; rp != NULL; rp = rp->ai_next) { - sfd = socket_helper(rp, flags); - if (sfd < 0) - continue; - - if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { - rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, - &on, sizeof(on)); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot setsockopt socket:" - " %s:%u: %s\n", - local_host, local_port, - strerror(errno)); - close(sfd); - continue; - } - } - - if (bind(sfd, rp->ai_addr, rp->ai_addrlen) == -1) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to bind socket: %s:%u: %s\n", - local_host, local_port, strerror(errno)); - close(sfd); - continue; - } - break; - } - freeaddrinfo(result); - if (rp == NULL) { - LOGP(DLGLOBAL, LOGL_ERROR, "no suitable local addr found for: %s:%u\n", - local_host, local_port); - return -ENODEV; - } - } - - /* Reached this point, if OSMO_SOCK_F_BIND then sfd is valid (>=0) or it - was already closed and func returned. If OSMO_SOCK_F_BIND is not - set, then sfd = -1 */ - - /* figure out remote side of socket */ - if (flags & OSMO_SOCK_F_CONNECT) { - result = addrinfo_helper(family, type, proto, remote_host, remote_port, false); - if (!result) { - if (sfd >= 0) - close(sfd); - return -EINVAL; - } - - for (rp = result; rp != NULL; rp = rp->ai_next) { - if (sfd < 0) { - sfd = socket_helper(rp, flags); - if (sfd < 0) - continue; - } - - rc = connect(sfd, rp->ai_addr, rp->ai_addrlen); - if (rc != 0 && errno != EINPROGRESS) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to connect socket: %s:%u: %s\n", - remote_host, remote_port, strerror(errno)); - /* We want to maintain the bind socket if bind was enabled */ - if (!(flags & OSMO_SOCK_F_BIND)) { - close(sfd); - sfd = -1; - } - continue; - } - break; - } - freeaddrinfo(result); - if (rp == NULL) { - LOGP(DLGLOBAL, LOGL_ERROR, "no suitable remote addr found for: %s:%u\n", - remote_host, remote_port); - if (sfd >= 0) - close(sfd); - return -ENODEV; - } - } - - rc = osmo_sock_init_tail(sfd, type, flags); - if (rc < 0) { - close(sfd); - sfd = -1; - } - - return sfd; -} - -#ifdef HAVE_LIBSCTP - - -/* Build array of addresses taking first addrinfo result of the requested family - * for each host in hosts. addrs4 or addrs6 are filled based on family type. */ -static int addrinfo_to_sockaddr(uint16_t family, const struct addrinfo **result, - const char **hosts, int host_cont, - struct sockaddr_in *addrs4, struct sockaddr_in6 *addrs6) { - size_t host_idx; - const struct addrinfo *rp; - OSMO_ASSERT(family == AF_INET || family == AF_INET6); - - for (host_idx = 0; host_idx < host_cont; host_idx++) { - for (rp = result[host_idx]; rp != NULL; rp = rp->ai_next) { - if (rp->ai_family != family) - continue; - if (family == AF_INET) - memcpy(&addrs4[host_idx], rp->ai_addr, sizeof(addrs4[host_idx])); - else - memcpy(&addrs6[host_idx], rp->ai_addr, sizeof(addrs6[host_idx])); - break; - } - if (!rp) { /* No addr could be bound for this host! */ - LOGP(DLGLOBAL, LOGL_ERROR, "No suitable remote address found for host: %s\n", - hosts[host_idx]); - return -ENODEV; - } - } - return 0; -} - -/*! Initialize a socket (including bind and/or connect) with multiple local or remote addresses. - * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] local_hosts array of char pointers (strings), each containing local host name or IP address in string form - * \param[in] local_hosts_cnt length of local_hosts (in items) - * \param[in] local_port local port number in host byte order - * \param[in] remote_host array of char pointers (strings), each containing remote host name or IP address in string form - * \param[in] remote_hosts_cnt length of remote_hosts (in items) - * \param[in] remote_port remote port number in host byte order - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket file descriptor on success; negative on error - * - * This function is similar to \ref osmo_sock_init2(), but can be passed an - * array of local or remote addresses for protocols supporting multiple - * addresses per socket, like SCTP (currently only one supported). This function - * should not be used by protocols not supporting this kind of features, but - * rather \ref osmo_sock_init2() should be used instead. - * See \ref osmo_sock_init2() for more information on flags and general behavior. - */ -int osmo_sock_init2_multiaddr(uint16_t family, uint16_t type, uint8_t proto, - const char **local_hosts, size_t local_hosts_cnt, uint16_t local_port, - const char **remote_hosts, size_t remote_hosts_cnt, uint16_t remote_port, - unsigned int flags) - -{ - struct addrinfo *result[OSMO_SOCK_MAX_ADDRS]; - int sfd = -1, rc, on = 1; - int i; - struct sockaddr_in addrs4[OSMO_SOCK_MAX_ADDRS]; - struct sockaddr_in6 addrs6[OSMO_SOCK_MAX_ADDRS]; - struct sockaddr *addrs; - char strbuf[512]; - - /* TODO: So far this function is only aimed for SCTP, but could be - reused in the future for other protocols with multi-addr support */ - if (proto != IPPROTO_SCTP) - return -ENOTSUP; - - /* TODO: Let's not support AF_UNSPEC for now. sctp_bindx() actually - supports binding both types of addresses on a AF_INET6 soscket, but - that would mean we could get both AF_INET and AF_INET6 addresses for - each host, and makes complexity of this function increase a lot since - we'd need to find out which subsets to use, use v4v6 mapped socket, - etc. */ - if (family == AF_UNSPEC) - return -ENOTSUP; - - if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "invalid: you have to specify either " - "BIND or CONNECT flags\n"); - return -EINVAL; - } - - if (((flags & OSMO_SOCK_F_BIND) && !local_hosts_cnt) || - ((flags & OSMO_SOCK_F_CONNECT) && !remote_hosts_cnt) || - local_hosts_cnt > OSMO_SOCK_MAX_ADDRS || - remote_hosts_cnt > OSMO_SOCK_MAX_ADDRS) - return -EINVAL; - - /* figure out local side of socket */ - if (flags & OSMO_SOCK_F_BIND) { - rc = addrinfo_helper_multi(result, family, type, proto, local_hosts, - local_hosts_cnt, local_port, true); - if (rc < 0) - return -EINVAL; - - /* Since addrinfo_helper sets ai_family, socktype and - ai_protocol in hints, we know all results will use same - values, so simply pick the first one and pass it to create - the socket: - */ - sfd = socket_helper(result[0], flags); - if (sfd < 0) { - for (i = 0; i < local_hosts_cnt; i++) - freeaddrinfo(result[i]); - return sfd; - } - - /* Since so far we only allow IPPROTO_SCTP in this function, - no need to check below for "proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR" */ - rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, - &on, sizeof(on)); - if (rc < 0) { - multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot setsockopt socket:" - " %s:%u: %s\n", - strbuf, local_port, - strerror(errno)); - for (i = 0; i < local_hosts_cnt; i++) - freeaddrinfo(result[i]); - close(sfd); - return rc; - } - - /* Build array of addresses taking first of same family for each host. - TODO: Ideally we should use backtracking storing last used - indexes and trying next combination if connect() fails .*/ - rc = addrinfo_to_sockaddr(family, (const struct addrinfo **)result, - local_hosts, local_hosts_cnt, addrs4, addrs6); - if (rc < 0) { - for (i = 0; i < local_hosts_cnt; i++) - freeaddrinfo(result[i]); - close(sfd); - return -ENODEV; - } - - if (family == AF_INET) - addrs = (struct sockaddr *)addrs4; - else - addrs = (struct sockaddr *)addrs6; - if (sctp_bindx(sfd, addrs, local_hosts_cnt, SCTP_BINDX_ADD_ADDR) == -1) { - multiaddr_snprintf(strbuf, sizeof(strbuf), local_hosts, local_hosts_cnt); - LOGP(DLGLOBAL, LOGL_NOTICE, "unable to bind socket: %s:%u: %s\n", - strbuf, local_port, strerror(errno)); - for (i = 0; i < local_hosts_cnt; i++) - freeaddrinfo(result[i]); - close(sfd); - return -ENODEV; - } - for (i = 0; i < local_hosts_cnt; i++) - freeaddrinfo(result[i]); - } - - /* Reached this point, if OSMO_SOCK_F_BIND then sfd is valid (>=0) or it - was already closed and func returned. If OSMO_SOCK_F_BIND is not - set, then sfd = -1 */ - - /* figure out remote side of socket */ - if (flags & OSMO_SOCK_F_CONNECT) { - rc = addrinfo_helper_multi(result, family, type, proto, remote_hosts, - remote_hosts_cnt, remote_port, false); - if (rc < 0) { - if (sfd >= 0) - close(sfd); - return -EINVAL; - } - - if (sfd < 0) { - /* Since addrinfo_helper sets ai_family, socktype and - ai_protocol in hints, we know all results will use same - values, so simply pick the first one and pass it to create - the socket: - */ - sfd = socket_helper(result[0], flags); - if (sfd < 0) { - for (i = 0; i < remote_hosts_cnt; i++) - freeaddrinfo(result[i]); - return sfd; - } - } - - /* Build array of addresses taking first of same family for each host. - TODO: Ideally we should use backtracking storing last used - indexes and trying next combination if connect() fails .*/ - rc = addrinfo_to_sockaddr(family, (const struct addrinfo **)result, - remote_hosts, remote_hosts_cnt, addrs4, addrs6); - if (rc < 0) { - for (i = 0; i < remote_hosts_cnt; i++) - freeaddrinfo(result[i]); - close(sfd); - return -ENODEV; - } - - if (family == AF_INET) - addrs = (struct sockaddr *)addrs4; - else - addrs = (struct sockaddr *)addrs6; - rc = sctp_connectx(sfd, addrs, remote_hosts_cnt, NULL); - if (rc != 0 && errno != EINPROGRESS) { - multiaddr_snprintf(strbuf, sizeof(strbuf), remote_hosts, remote_hosts_cnt); - LOGP(DLGLOBAL, LOGL_ERROR, "unable to connect socket: %s:%u: %s\n", - strbuf, remote_port, strerror(errno)); - for (i = 0; i < remote_hosts_cnt; i++) - freeaddrinfo(result[i]); - close(sfd); - return -ENODEV; - } - for (i = 0; i < remote_hosts_cnt; i++) - freeaddrinfo(result[i]); - } - - rc = osmo_sock_init_tail(sfd, type, flags); - if (rc < 0) { - close(sfd); - sfd = -1; - } - - return sfd; -} -#endif /* HAVE_LIBSCTP */ - -/*! Initialize a socket (including bind/connect) - * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] host remote host name or IP address in string form - * \param[in] port remote port number in host byte order - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket file descriptor on success; negative on error - * - * This function creates a new socket of the designated \a family, \a - * type and \a proto and optionally binds or connects it, depending on - * the value of \a flags parameter. - */ -int osmo_sock_init(uint16_t family, uint16_t type, uint8_t proto, - const char *host, uint16_t port, unsigned int flags) -{ - struct addrinfo *result, *rp; - int sfd, rc, on = 1; - - if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == - (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) { - LOGP(DLGLOBAL, LOGL_ERROR, "invalid: both bind and connect flags set:" - " %s:%u\n", host, port); - return -EINVAL; - } - - result = addrinfo_helper(family, type, proto, host, port, flags & OSMO_SOCK_F_BIND); - if (!result) { - LOGP(DLGLOBAL, LOGL_ERROR, "getaddrinfo returned NULL: %s:%u: %s\n", - host, port, strerror(errno)); - return -EINVAL; - } - - for (rp = result; rp != NULL; rp = rp->ai_next) { - sfd = socket_helper(rp, flags); - if (sfd == -1) - continue; - - if (flags & OSMO_SOCK_F_CONNECT) { - rc = connect(sfd, rp->ai_addr, rp->ai_addrlen); - if (rc != 0 && errno != EINPROGRESS) { - close(sfd); - continue; - } - } else { - if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { - rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, - &on, sizeof(on)); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot setsockopt socket:" - " %s:%u: %s\n", - host, port, strerror(errno)); - close(sfd); - continue; - } - } - if (bind(sfd, rp->ai_addr, rp->ai_addrlen) == -1) { - LOGP(DLGLOBAL, LOGL_ERROR, "unable to bind socket:" - "%s:%u: %s\n", - host, port, strerror(errno)); - close(sfd); - continue; - } - } - break; - } - freeaddrinfo(result); - - if (rp == NULL) { - LOGP(DLGLOBAL, LOGL_ERROR, "no suitable addr found for: %s:%u\n", - host, port); - return -ENODEV; - } - - if (proto != IPPROTO_UDP || flags & OSMO_SOCK_F_UDP_REUSEADDR) { - rc = setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)); - if (rc < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot setsockopt socket: %s:%u: %s\n", host, - port, strerror(errno)); - close(sfd); - sfd = -1; - } - } - - rc = osmo_sock_init_tail(sfd, type, flags); - if (rc < 0) { - close(sfd); - sfd = -1; - } - - return sfd; -} - -/*! fill \ref osmo_fd for a give sfd - * \param[out] ofd file descriptor (will be filled in) - * \param[in] sfd socket file descriptor - * \returns socket fd on success; negative on error - * - * This function fills the \a ofd structure. - */ -static inline int osmo_fd_init_ofd(struct osmo_fd *ofd, int sfd) -{ - int rc; - - if (sfd < 0) - return sfd; - - ofd->fd = sfd; - ofd->when = OSMO_FD_READ; - - rc = osmo_fd_register(ofd); - if (rc < 0) { - close(sfd); - return rc; - } - - return sfd; -} - -/*! Initialize a socket and fill \ref osmo_fd - * \param[out] ofd file descriptor (will be filled in) - * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] host remote host name or IP address in string form - * \param[in] port remote port number in host byte order - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket fd on success; negative on error - * - * This function creates (and optionall binds/connects) a socket using - * \ref osmo_sock_init, but also fills the \a ofd structure. - */ -int osmo_sock_init_ofd(struct osmo_fd *ofd, int family, int type, int proto, - const char *host, uint16_t port, unsigned int flags) -{ - return osmo_fd_init_ofd(ofd, osmo_sock_init(family, type, proto, host, port, flags)); -} - -/*! Initialize a socket and fill \ref osmo_fd - * \param[out] ofd file descriptor (will be filled in) - * \param[in] family Address Family like AF_INET, AF_INET6, AF_UNSPEC - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] local_host local host name or IP address in string form - * \param[in] local_port local port number in host byte order - * \param[in] remote_host remote host name or IP address in string form - * \param[in] remote_port remote port number in host byte order - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket fd on success; negative on error - * - * This function creates (and optionall binds/connects) a socket using - * \ref osmo_sock_init2, but also fills the \a ofd structure. - */ -int osmo_sock_init2_ofd(struct osmo_fd *ofd, int family, int type, int proto, - const char *local_host, uint16_t local_port, - const char *remote_host, uint16_t remote_port, unsigned int flags) -{ - return osmo_fd_init_ofd(ofd, osmo_sock_init2(family, type, proto, local_host, - local_port, remote_host, remote_port, flags)); -} - -/*! Initialize a socket and fill \ref sockaddr - * \param[out] ss socket address (will be filled in) - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket fd on success; negative on error - * - * This function creates (and optionall binds/connects) a socket using - * \ref osmo_sock_init, but also fills the \a ss structure. - */ -int osmo_sock_init_sa(struct sockaddr *ss, uint16_t type, - uint8_t proto, unsigned int flags) -{ - char host[NI_MAXHOST]; - uint16_t port; - struct sockaddr_in *sin; - struct sockaddr_in6 *sin6; - int s, sa_len; - - /* determine port and host from ss */ - switch (ss->sa_family) { - case AF_INET: - sin = (struct sockaddr_in *) ss; - sa_len = sizeof(struct sockaddr_in); - port = ntohs(sin->sin_port); - break; - case AF_INET6: - sin6 = (struct sockaddr_in6 *) ss; - sa_len = sizeof(struct sockaddr_in6); - port = ntohs(sin6->sin6_port); - break; - default: - return -EINVAL; - } - - s = getnameinfo(ss, sa_len, host, NI_MAXHOST, - NULL, 0, NI_NUMERICHOST); - if (s != 0) { - LOGP(DLGLOBAL, LOGL_ERROR, "getnameinfo failed:" - " %s\n", strerror(errno)); - return s; - } - - return osmo_sock_init(ss->sa_family, type, proto, host, port, flags); -} - -static int sockaddr_equal(const struct sockaddr *a, - const struct sockaddr *b, unsigned int len) -{ - struct sockaddr_in *sin_a, *sin_b; - struct sockaddr_in6 *sin6_a, *sin6_b; - - if (a->sa_family != b->sa_family) - return 0; - - switch (a->sa_family) { - case AF_INET: - sin_a = (struct sockaddr_in *)a; - sin_b = (struct sockaddr_in *)b; - if (!memcmp(&sin_a->sin_addr, &sin_b->sin_addr, - sizeof(struct in_addr))) - return 1; - break; - case AF_INET6: - sin6_a = (struct sockaddr_in6 *)a; - sin6_b = (struct sockaddr_in6 *)b; - if (!memcmp(&sin6_a->sin6_addr, &sin6_b->sin6_addr, - sizeof(struct in6_addr))) - return 1; - break; - } - return 0; -} - -/*! Determine if the given address is a local address - * \param[in] addr Socket Address - * \param[in] addrlen Length of socket address in bytes - * \returns 1 if address is local, 0 otherwise. - */ -int osmo_sockaddr_is_local(struct sockaddr *addr, unsigned int addrlen) -{ - struct ifaddrs *ifaddr, *ifa; - - if (getifaddrs(&ifaddr) == -1) { - LOGP(DLGLOBAL, LOGL_ERROR, "getifaddrs:" - " %s\n", strerror(errno)); - return -EIO; - } - - for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { - if (!ifa->ifa_addr) - continue; - if (sockaddr_equal(ifa->ifa_addr, addr, addrlen)) { - freeifaddrs(ifaddr); - return 1; - } - } - - freeifaddrs(ifaddr); - return 0; -} - -/*! Convert sockaddr_in to IP address as char string and port as uint16_t. - * \param[out] addr String buffer to write IP address to, or NULL. - * \param[out] addr_len Size of \a addr. - * \param[out] port Pointer to uint16_t to write the port number to, or NULL. - * \param[in] sin Sockaddr to convert. - * \returns the required string buffer size, like osmo_strlcpy(), or 0 if \a addr is NULL. - */ -size_t osmo_sockaddr_in_to_str_and_uint(char *addr, unsigned int addr_len, uint16_t *port, - const struct sockaddr_in *sin) -{ - if (port) - *port = ntohs(sin->sin_port); - - if (addr) - return osmo_strlcpy(addr, inet_ntoa(sin->sin_addr), addr_len); - - return 0; -} - -/*! Convert sockaddr to IP address as char string and port as uint16_t. - * \param[out] addr String buffer to write IP address to, or NULL. - * \param[out] addr_len Size of \a addr. - * \param[out] port Pointer to uint16_t to write the port number to, or NULL. - * \param[in] sa Sockaddr to convert. - * \returns the required string buffer size, like osmo_strlcpy(), or 0 if \a addr is NULL. - */ -unsigned int osmo_sockaddr_to_str_and_uint(char *addr, unsigned int addr_len, uint16_t *port, - const struct sockaddr *sa) -{ - const struct sockaddr_in *sin = (const struct sockaddr_in *)sa; - - return osmo_sockaddr_in_to_str_and_uint(addr, addr_len, port, sin); -} - -/*! Initialize a unix domain socket (including bind/connect) - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] socket_path path to identify the socket - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket fd on success; negative on error - * - * This function creates a new unix domain socket, \a - * type and \a proto and optionally binds or connects it, depending on - * the value of \a flags parameter. - */ -#if defined(__clang__) && defined(SUN_LEN) -__attribute__((no_sanitize("undefined"))) -#endif -int osmo_sock_unix_init(uint16_t type, uint8_t proto, - const char *socket_path, unsigned int flags) -{ - struct sockaddr_un local; - int sfd, rc, on = 1; - unsigned int namelen; - - if ((flags & (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) == - (OSMO_SOCK_F_BIND | OSMO_SOCK_F_CONNECT)) - return -EINVAL; - - local.sun_family = AF_UNIX; - /* When an AF_UNIX socket is bound, sun_path should be NUL-terminated. See unix(7) man page. */ - if (osmo_strlcpy(local.sun_path, socket_path, sizeof(local.sun_path)) >= sizeof(local.sun_path)) { - LOGP(DLGLOBAL, LOGL_ERROR, "Socket path exceeds maximum length of %zd bytes: %s\n", - sizeof(local.sun_path), socket_path); - return -ENOSPC; - } - -#if defined(BSD44SOCKETS) || defined(__UNIXWARE__) - local.sun_len = strlen(local.sun_path); -#endif -#if defined(BSD44SOCKETS) || defined(SUN_LEN) - namelen = SUN_LEN(&local); -#else - namelen = strlen(local.sun_path) + - offsetof(struct sockaddr_un, sun_path); -#endif - - sfd = socket(AF_UNIX, type, proto); - if (sfd < 0) - return -1; - - if (flags & OSMO_SOCK_F_CONNECT) { - rc = connect(sfd, (struct sockaddr *)&local, namelen); - if (rc < 0) - goto err; - } else { - unlink(local.sun_path); - rc = bind(sfd, (struct sockaddr *)&local, namelen); - if (rc < 0) - goto err; - } - - if (flags & OSMO_SOCK_F_NONBLOCK) { - if (ioctl(sfd, FIONBIO, (unsigned char *)&on) < 0) { - LOGP(DLGLOBAL, LOGL_ERROR, - "cannot set this socket unblocking: %s\n", - strerror(errno)); - close(sfd); - return -EINVAL; - } - } - - rc = osmo_sock_init_tail(sfd, type, flags); - if (rc < 0) { - close(sfd); - sfd = -1; - } - - return sfd; -err: - close(sfd); - return -1; -} - -/*! Initialize a unix domain socket and fill \ref osmo_fd - * \param[out] ofd file descriptor (will be filled in) - * \param[in] type Socket type like SOCK_DGRAM, SOCK_STREAM - * \param[in] proto Protocol like IPPROTO_TCP, IPPROTO_UDP - * \param[in] socket_path path to identify the socket - * \param[in] flags flags like \ref OSMO_SOCK_F_CONNECT - * \returns socket fd on success; negative on error - * - * This function creates (and optionally binds/connects) a socket - * using osmo_sock_unix_init, but also fills the ofd structure. - */ -int osmo_sock_unix_init_ofd(struct osmo_fd *ofd, uint16_t type, uint8_t proto, - const char *socket_path, unsigned int flags) -{ - return osmo_fd_init_ofd(ofd, osmo_sock_unix_init(type, proto, socket_path, flags)); -} - -/*! Get the IP and/or port number on socket in separate string buffers. - * \param[in] fd file descriptor of socket - * \param[out] ip IP address (will be filled in when not NULL) - * \param[in] ip_len length of the ip buffer - * \param[out] port number (will be filled in when not NULL) - * \param[in] port_len length of the port buffer - * \param[in] local (true) or remote (false) name will get looked at - * \returns 0 on success; negative otherwise - */ -int osmo_sock_get_ip_and_port(int fd, char *ip, size_t ip_len, char *port, size_t port_len, bool local) -{ - struct sockaddr sa; - socklen_t len = sizeof(sa); - char ipbuf[INET6_ADDRSTRLEN], portbuf[6]; - int rc; - - rc = local ? getsockname(fd, &sa, &len) : getpeername(fd, &sa, &len); - if (rc < 0) - return rc; - - rc = getnameinfo(&sa, len, ipbuf, sizeof(ipbuf), - portbuf, sizeof(portbuf), - NI_NUMERICHOST | NI_NUMERICSERV); - if (rc < 0) - return rc; - - if (ip) - strncpy(ip, ipbuf, ip_len); - if (port) - strncpy(port, portbuf, port_len); - return 0; -} - -/*! Get local IP address on socket - * \param[in] fd file descriptor of socket - * \param[out] ip IP address (will be filled in) - * \param[in] len length of the output buffer - * \returns 0 on success; negative otherwise - */ -int osmo_sock_get_local_ip(int fd, char *ip, size_t len) -{ - return osmo_sock_get_ip_and_port(fd, ip, len, NULL, 0, true); -} - -/*! Get local port on socket - * \param[in] fd file descriptor of socket - * \param[out] port number (will be filled in) - * \param[in] len length of the output buffer - * \returns 0 on success; negative otherwise - */ -int osmo_sock_get_local_ip_port(int fd, char *port, size_t len) -{ - return osmo_sock_get_ip_and_port(fd, NULL, 0, port, len, true); -} - -/*! Get remote IP address on socket - * \param[in] fd file descriptor of socket - * \param[out] ip IP address (will be filled in) - * \param[in] len length of the output buffer - * \returns 0 on success; negative otherwise - */ -int osmo_sock_get_remote_ip(int fd, char *ip, size_t len) -{ - return osmo_sock_get_ip_and_port(fd, ip, len, NULL, 0, false); -} - -/*! Get remote port on socket - * \param[in] fd file descriptor of socket - * \param[out] port number (will be filled in) - * \param[in] len length of the output buffer - * \returns 0 on success; negative otherwise - */ -int osmo_sock_get_remote_ip_port(int fd, char *port, size_t len) -{ - return osmo_sock_get_ip_and_port(fd, NULL, 0, port, len, false); -} - -/*! Get address/port information on socket in dyn-alloc string like "(r=1.2.3.4:5<->l=6.7.8.9:10)". - * Usually, it is better to use osmo_sock_get_name2() for a static string buffer or osmo_sock_get_name_buf() for a - * caller provided string buffer, to avoid the dynamic talloc allocation. - * \param[in] ctx talloc context from which to allocate string buffer - * \param[in] fd file descriptor of socket - * \returns string identifying the connection of this socket, talloc'd from ctx. - */ -char *osmo_sock_get_name(const void *ctx, int fd) -{ - char str[OSMO_SOCK_NAME_MAXLEN]; - int rc; - rc = osmo_sock_get_name_buf(str, sizeof(str), fd); - if (rc <= 0) - return NULL; - return talloc_asprintf(ctx, "(%s)", str); -} - -/*! Get address/port information on socket in provided string buffer, like "r=1.2.3.4:5<->l=6.7.8.9:10". - * This does not include braces like osmo_sock_get_name(). - * \param[out] str Destination string buffer. - * \param[in] str_len sizeof(str). - * \param[in] fd File descriptor of socket. - * \return String length as returned by snprintf(), or negative on error. - */ -int osmo_sock_get_name_buf(char *str, size_t str_len, int fd) -{ - char hostbuf_l[INET6_ADDRSTRLEN], hostbuf_r[INET6_ADDRSTRLEN]; - char portbuf_l[6], portbuf_r[6]; - int rc; - - /* get local */ - if ((rc = osmo_sock_get_ip_and_port(fd, hostbuf_l, sizeof(hostbuf_l), portbuf_l, sizeof(portbuf_l), true))) { - osmo_strlcpy(str, "<error-in-getsockname>", str_len); - return rc; - } - - /* get remote */ - if (osmo_sock_get_ip_and_port(fd, hostbuf_r, sizeof(hostbuf_r), portbuf_r, sizeof(portbuf_r), false) != 0) - return snprintf(str, str_len, "r=NULL<->l=%s:%s", hostbuf_l, portbuf_l); - - return snprintf(str, str_len, "r=%s:%s<->l=%s:%s", hostbuf_r, portbuf_r, hostbuf_l, portbuf_l); -} - -/*! Get address/port information on socket in static string, like "r=1.2.3.4:5<->l=6.7.8.9:10". - * This does not include braces like osmo_sock_get_name(). - * \param[in] fd File descriptor of socket. - * \return Static string buffer containing the result. - */ -const char *osmo_sock_get_name2(int fd) -{ - static __thread char str[OSMO_SOCK_NAME_MAXLEN]; - osmo_sock_get_name_buf(str, sizeof(str), fd); - return str; -} - -/*! Get address/port information on socket in static string, like "r=1.2.3.4:5<->l=6.7.8.9:10". - * This does not include braces like osmo_sock_get_name(). - * \param[in] fd File descriptor of socket. - * \return Static string buffer containing the result. - */ -char *osmo_sock_get_name2_c(const void *ctx, int fd) -{ - char *str = talloc_size(ctx, OSMO_SOCK_NAME_MAXLEN); - if (!str) - return NULL; - osmo_sock_get_name_buf(str, OSMO_SOCK_NAME_MAXLEN, fd); - return str; -} - -static int sock_get_domain(int fd) -{ - int domain; -#ifdef SO_DOMAIN - socklen_t dom_len = sizeof(domain); - int rc; - - rc = getsockopt(fd, SOL_SOCKET, SO_DOMAIN, &domain, &dom_len); - if (rc < 0) - return rc; -#else - /* This of course sucks, but what shall we do on OSs like - * FreeBSD that don't seem to expose a method by which one can - * learn the address family of a socket? */ - domain = AF_INET; -#endif - return domain; -} - - -/*! Activate or de-activate local loop-back of transmitted multicast packets - * \param[in] fd file descriptor of related socket - * \param[in] enable Enable (true) or disable (false) loop-back - * \returns 0 on success; negative otherwise */ -int osmo_sock_mcast_loop_set(int fd, bool enable) -{ - int domain, loop = 0; - - if (enable) - loop = 1; - - domain = sock_get_domain(fd); - if (domain < 0) - return domain; - - switch (domain) { - case AF_INET: - return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &loop, sizeof(loop)); - case AF_INET6: - return setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &loop, sizeof(loop)); - default: - return -EINVAL; - } -} - -/*! Set the TTL of outbound multicast packets - * \param[in] fd file descriptor of related socket - * \param[in] ttl TTL of to-be-sent multicast packets - * \returns 0 on success; negative otherwise */ -int osmo_sock_mcast_ttl_set(int fd, uint8_t ttl) -{ - int domain, ttli = ttl; - - domain = sock_get_domain(fd); - if (domain < 0) - return domain; - - switch (domain) { - case AF_INET: - return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttli, sizeof(ttli)); - case AF_INET6: - return setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &ttli, sizeof(ttli)); - default: - return -EINVAL; - } -} - -/*! Enable/disable receiving all multicast packets, even for non-subscribed groups - * \param[in] fd file descriptor of related socket - * \param[in] enable Enable or Disable receiving of all packets - * \returns 0 on success; negative otherwise */ -int osmo_sock_mcast_all_set(int fd, bool enable) -{ - int domain, all = 0; - - if (enable) - all = 1; - - domain = sock_get_domain(fd); - if (domain < 0) - return domain; - - switch (domain) { - case AF_INET: -#ifdef IP_MULTICAST_ALL - return setsockopt(fd, IPPROTO_IP, IP_MULTICAST_ALL, &all, sizeof(all)); -#endif - case AF_INET6: - /* there seems no equivalent ?!? */ - default: - return -EINVAL; - } -} - -/* FreeBSD calls the socket option differently */ -#if !defined(IPV6_ADD_MEMBERSHIP) && defined(IPV6_JOIN_GROUP) -#define IPV6_ADD_MEMBERSHIP IPV6_JOIN_GROUP -#endif - -/*! Subscribe to the given IP multicast group - * \param[in] fd file descriptor of related scoket - * \param[in] grp_addr ASCII representation of the multicast group address - * \returns 0 on success; negative otherwise */ -int osmo_sock_mcast_subscribe(int fd, const char *grp_addr) -{ - int rc, domain; - struct ip_mreq mreq; - struct ipv6_mreq mreq6; - struct in6_addr i6a; - - domain = sock_get_domain(fd); - if (domain < 0) - return domain; - - switch (domain) { - case AF_INET: - memset(&mreq, 0, sizeof(mreq)); - mreq.imr_multiaddr.s_addr = inet_addr(grp_addr); - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - return setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); -#ifdef IPV6_ADD_MEMBERSHIP - case AF_INET6: - memset(&mreq6, 0, sizeof(mreq6)); - rc = inet_pton(AF_INET6, grp_addr, (void *)&i6a); - if (rc < 0) - return -EINVAL; - mreq6.ipv6mr_multiaddr = i6a; - return setsockopt(fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq6, sizeof(mreq6)); -#endif - default: - return -EINVAL; - } -} - -/*! Determine the matching local IP-address for a given remote IP-Address. - * \param[out] local_ip caller provided memory for resulting local IP-address - * \param[in] remote_ip remote IP-address - * \param[in] fd file descriptor of related scoket - * \returns 0 on success; negative otherwise - * - * The function accepts IPv4 and IPv6 address strings. The caller must provide - * at least INET6_ADDRSTRLEN bytes for local_ip if an IPv6 is expected as - * as result. For IPv4 addresses the required amount is INET_ADDRSTRLEN. */ -int osmo_sock_local_ip(char *local_ip, const char *remote_ip) -{ - int sfd; - int rc; - struct addrinfo addrinfo_hint; - struct addrinfo *addrinfo = NULL; - struct sockaddr_in local_addr; - socklen_t local_addr_len; - uint16_t family; - - /* Find out the address family (AF_INET or AF_INET6?) */ - memset(&addrinfo_hint, '\0', sizeof(addrinfo_hint)); - addrinfo_hint.ai_family = PF_UNSPEC; - addrinfo_hint.ai_flags = AI_NUMERICHOST; - rc = getaddrinfo(remote_ip, NULL, &addrinfo_hint, &addrinfo); - if (rc) - return -EINVAL; - family = addrinfo->ai_family; - freeaddrinfo(addrinfo); - - /* Connect a dummy socket to trick the kernel into determining the - * ip-address of the interface that would be used if we would send - * out an actual packet */ - sfd = osmo_sock_init2(family, SOCK_DGRAM, IPPROTO_UDP, NULL, 0, remote_ip, 0, OSMO_SOCK_F_CONNECT); - if (sfd < 0) - return -EINVAL; - - /* Request the IP address of the interface that the kernel has - * actually choosen. */ - memset(&local_addr, 0, sizeof(local_addr)); - local_addr_len = sizeof(local_addr); - rc = getsockname(sfd, (struct sockaddr *)&local_addr, &local_addr_len); - close(sfd); - if (rc < 0) - return -EINVAL; - if (local_addr.sin_family == AF_INET) - inet_ntop(AF_INET, &local_addr.sin_addr, local_ip, INET_ADDRSTRLEN); - else if (local_addr.sin_family == AF_INET6) - inet_ntop(AF_INET6, &local_addr.sin_addr, local_ip, INET6_ADDRSTRLEN); - else - return -EINVAL; - - return 0; -} - -#endif /* HAVE_SYS_SOCKET_H */ - -/*! @} */ diff --git a/src/stat_item.c b/src/stat_item.c deleted file mode 100644 index 67165754..00000000 --- a/src/stat_item.c +++ /dev/null @@ -1,359 +0,0 @@ -/*! \file stat_item.c - * utility routines for keeping statistical values */ -/* - * (C) 2009-2010 by Harald Welte <laforge@gnumonks.org> - * (C) 2015 by sysmocom - s.f.m.c. GmbH - * - * All Rights Reserved - * - * SPDX-License-Identifier: GPL-2.0+ - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 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 General Public License for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * - */ - -/*! \addtogroup osmo_stat_item - * @{ - * - * This osmo_stat_item module adds instrumentation capabilities to - * gather measurement and statistical values in a similar fashion to - * what we have as \ref osmo_counter_group. - * - * As opposed to counters, osmo_stat_item do not increment but consist - * of a configurable-sized FIFO, which can store not only the current - * (most recent) value, but also historic values. - * - * The only supported value type is an int32_t. - * - * Getting values from the osmo_stat_item does not modify its state to - * allow for multiple independent back-ends retrieving values (e.g. VTY - * and statd). - * - * Each value stored in the FIFO of an osmo_stat_item has an associated - * value_id. The value_id is derived from an application-wide globally - * incrementing counter, so (until the counter wraps) more recent - * values will have higher values. - * - * When a new value is set, the oldest value in the FIFO gets silently - * overwritten. Lost values are skipped when getting values from the - * item. - * - */ - -#include <stdint.h> -#include <string.h> - -#include <osmocom/core/utils.h> -#include <osmocom/core/linuxlist.h> -#include <osmocom/core/talloc.h> -#include <osmocom/core/timer.h> -#include <osmocom/core/stat_item.h> - -/*! global list of stat_item groups */ -static LLIST_HEAD(osmo_stat_item_groups); -/*! counter for assigning globally unique value identifiers */ -static int32_t global_value_id = 0; - -/*! talloc context from which we allocate */ -static void *tall_stat_item_ctx; - -/*! Allocate a new group of counters according to description. - * Allocate a group of stat items described in \a desc from talloc context \a ctx, - * giving the new group the index \a idx. - * \param[in] ctx \ref talloc context - * \param[in] desc Statistics item group description - * \param[in] idx Index of new stat item group - */ -struct osmo_stat_item_group *osmo_stat_item_group_alloc(void *ctx, - const struct osmo_stat_item_group_desc *desc, - unsigned int idx) -{ - unsigned int group_size; - unsigned long items_size = 0; - unsigned int item_idx; - void *items; - - struct osmo_stat_item_group *group; - - group_size = sizeof(struct osmo_stat_item_group) + - desc->num_items * sizeof(struct osmo_stat_item *); - - if (!ctx) - ctx = tall_stat_item_ctx; - - group = talloc_zero_size(ctx, group_size); - if (!group) - return NULL; - - group->desc = desc; - group->idx = idx; - - /* Get combined size of all items */ - for (item_idx = 0; item_idx < desc->num_items; item_idx++) { - unsigned int size; - size = sizeof(struct osmo_stat_item) + - sizeof(struct osmo_stat_item_value) * - desc->item_desc[item_idx].num_values; - /* Align to pointer size */ - size = (size + sizeof(void *) - 1) & ~(sizeof(void *) - 1); - - /* Store offsets into the item array */ - group->items[item_idx] = (void *)items_size; - - items_size += size; - } - - items = talloc_zero_size(group, items_size); - if (!items) { - talloc_free(group); - return NULL; - } - - /* Update item pointers */ - for (item_idx = 0; item_idx < desc->num_items; item_idx++) { - struct osmo_stat_item *item = (struct osmo_stat_item *) - ((uint8_t *)items + (unsigned long)group->items[item_idx]); - unsigned int i; - - group->items[item_idx] = item; - item->last_offs = desc->item_desc[item_idx].num_values - 1; - item->last_value_index = -1; - item->desc = &desc->item_desc[item_idx]; - - for (i = 0; i <= item->last_offs; i++) { - item->values[i].value = desc->item_desc[item_idx].default_value; - item->values[i].id = OSMO_STAT_ITEM_NOVALUE_ID; - } - } - - llist_add(&group->list, &osmo_stat_item_groups); - - return group; -} - -/*! Free the memory for the specified group of stat items */ -void osmo_stat_item_group_free(struct osmo_stat_item_group *grp) -{ - llist_del(&grp->list); - talloc_free(grp); -} - -/*! Increase the stat_item to the given value. - * This function adds a new value for the given stat_item at the end of - * the FIFO. - * \param[in] item The stat_item whose \a value we want to set - * \param[in] value The numeric value we want to store at end of FIFO - */ -void osmo_stat_item_inc(struct osmo_stat_item *item, int32_t value) -{ - int32_t oldvalue = item->values[item->last_offs].value; - osmo_stat_item_set(item, oldvalue + value); -} - -/*! Descrease the stat_item to the given value. - * This function adds a new value for the given stat_item at the end of - * the FIFO. - * \param[in] item The stat_item whose \a value we want to set - * \param[in] value The numeric value we want to store at end of FIFO - */ -void osmo_stat_item_dec(struct osmo_stat_item *item, int32_t value) -{ - int32_t oldvalue = item->values[item->last_offs].value; - osmo_stat_item_set(item, oldvalue - value); -} - -/*! Set the a given stat_item to the given value. - * This function adds a new value for the given stat_item at the end of - * the FIFO. - * \param[in] item The stat_item whose \a value we want to set - * \param[in] value The numeric value we want to store at end of FIFO - */ -void osmo_stat_item_set(struct osmo_stat_item *item, int32_t value) -{ - item->last_offs += 1; - if (item->last_offs >= item->desc->num_values) - item->last_offs = 0; - - global_value_id += 1; - if (global_value_id == OSMO_STAT_ITEM_NOVALUE_ID) - global_value_id += 1; - - item->values[item->last_offs].value = value; - item->values[item->last_offs].id = global_value_id; -} - -/*! Retrieve the next value from the osmo_stat_item object. - * If a new value has been set, it is returned. The idx is used to decide - * which value to return. - * On success, *idx is updated to refer to the next unread value. If - * values have been missed due to FIFO overflow, *idx is incremented by - * (1 + num_lost). - * This way, the osmo_stat_item object can be kept stateless from the reader's - * perspective and therefore be used by several backends simultaneously. - * - * \param val the osmo_stat_item object - * \param idx identifies the next value to be read - * \param value a pointer to store the value - * \returns the increment of the index (0: no value has been read, - * 1: one value has been taken, - * (1+n): n values have been skipped, one has been taken) - */ -int osmo_stat_item_get_next(const struct osmo_stat_item *item, int32_t *next_idx, - int32_t *value) -{ - const struct osmo_stat_item_value *next_value; - const struct osmo_stat_item_value *item_value = NULL; - int idx_delta; - int next_offs; - - next_offs = item->last_offs; - next_value = &item->values[next_offs]; - - while (next_value->id - *next_idx >= 0 && - next_value->id != OSMO_STAT_ITEM_NOVALUE_ID) - { - item_value = next_value; - - next_offs -= 1; - if (next_offs < 0) - next_offs = item->desc->num_values - 1; - if (next_offs == item->last_offs) - break; - next_value = &item->values[next_offs]; - } - - if (!item_value) - /* All items have been read */ - return 0; - - *value = item_value->value; - - idx_delta = item_value->id + 1 - *next_idx; - - *next_idx = item_value->id + 1; - - return idx_delta; -} - -/*! Skip/discard all values of this item and update \a idx accordingly */ -int osmo_stat_item_discard(const struct osmo_stat_item *item, int32_t *idx) -{ - int discarded = item->values[item->last_offs].id + 1 - *idx; - *idx = item->values[item->last_offs].id + 1; - - return discarded; -} - -/*! Skip all values of all items and update \a idx accordingly */ -int osmo_stat_item_discard_all(int32_t *idx) -{ - int discarded = global_value_id + 1 - *idx; - *idx = global_value_id + 1; - - return discarded; -} - -/*! Initialize the stat item module. Call this once from your program. - * \param[in] tall_ctx Talloc context from which this module allocates */ -int osmo_stat_item_init(void *tall_ctx) -{ - tall_stat_item_ctx = tall_ctx; - - return 0; -} - -/*! Search for item group based on group name and index - * \param[in] name Name of stats_item_group we want to find - * \param[in] idx Index of the group we want to find - * \returns pointer to group, if found; NULL otherwise */ -struct osmo_stat_item_group *osmo_stat_item_get_group_by_name_idx( - const char *name, const unsigned int idx) -{ - struct osmo_stat_item_group *statg; - - llist_for_each_entry(statg, &osmo_stat_item_groups, list) { - if (!statg->desc) - continue; - - if (!strcmp(statg->desc->group_name_prefix, name) && - statg->idx == idx) - return statg; - } - return NULL; -} - -/*! Search for item based on group + item name - * \param[in] statg group in which to search for the item - * \param[in] name name of item to search within \a statg - * \returns pointer to item, if found; NULL otherwise */ -const struct osmo_stat_item *osmo_stat_item_get_by_name( - const struct osmo_stat_item_group *statg, const char *name) -{ - int i; - const struct osmo_stat_item_desc *item_desc; - - if (!statg->desc) - return NULL; - - for (i = 0; i < statg->desc->num_items; i++) { - item_desc = &statg->desc->item_desc[i]; - - if (!strcmp(item_desc->name, name)) { - return statg->items[i]; - } - } - return NULL; -} - -/*! Iterate over all items in group, call user-supplied function on each - * \param[in] statg stat_item group over whose items to iterate - * \param[in] handle_item Call-back function, aborts if rc < 0 - * \param[in] data Private data handed through to \a handle_item - */ -int osmo_stat_item_for_each_item(struct osmo_stat_item_group *statg, - osmo_stat_item_handler_t handle_item, void *data) -{ - int rc = 0; - int i; - - for (i = 0; i < statg->desc->num_items; i++) { - struct osmo_stat_item *item = statg->items[i]; - rc = handle_item(statg, item, data); - if (rc < 0) - return rc; - } - - return rc; -} - -/*! Iterate over all stat_item groups in system, call user-supplied function on each - * \param[in] handle_group Call-back function, aborts if rc < 0 - * \param[in] data Private data handed through to \a handle_group - */ -int osmo_stat_item_for_each_group(osmo_stat_item_group_handler_t handle_group, void *data) -{ - struct osmo_stat_item_group *statg; - int rc = 0; - - llist_for_each_entry(statg, &osmo_stat_item_groups, list) { - rc = handle_group(statg, data); - if (rc < 0) - return rc; - } - - return rc; -} - -/*! @} */ diff --git a/src/usb/Makefile.am b/src/usb/Makefile.am new file mode 100644 index 00000000..c7d7a2a2 --- /dev/null +++ b/src/usb/Makefile.am @@ -0,0 +1,24 @@ +# This is _NOT_ the library release version, it's an API version. +# Please read chapter "Library interface versions" of the libtool documentation +# before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html +LIBVERSION=0:1:0 + +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -fPIC -Wall $(LIBUSB_CFLAGS) $(TALLOC_CFLAGS) +AM_LDFLAGS = $(COVERAGE_LDFLAGS) + +if ENABLE_LIBUSB + +lib_LTLIBRARIES = libosmousb.la + +libosmousb_la_SOURCES = osmo_libusb.c +libosmousb_la_LDFLAGS = \ + -version-info $(LIBVERSION) \ + -no-undefined \ + $(NULL) +libosmousb_la_LIBADD = \ + $(top_builddir)/src/core/libosmocore.la \ + $(TALLOC_LIBS) \ + $(LIBUSB_LIBS) + +endif diff --git a/src/usb/osmo_libusb.c b/src/usb/osmo_libusb.c new file mode 100644 index 00000000..a249d100 --- /dev/null +++ b/src/usb/osmo_libusb.c @@ -0,0 +1,776 @@ +/* libosmocore integration with libusb-1.0 + * + * (C) 2019-2019 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved. + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * 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 General Public License for more details. + */ +#include <errno.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdint.h> +#include <string.h> +#include <poll.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/select.h> +#include <osmocom/core/talloc.h> + +#include <libusb.h> + +#include <osmocom/usb/libusb.h> + +/*********************************************************************** + * logging integration + ***********************************************************************/ + +#define DLUSB DLINP + +#ifdef LIBUSB_LOG_CB_CONTEXT /* introduced in 1.0.23 */ +static const int usb2logl[] = { + [LIBUSB_LOG_LEVEL_NONE] = LOGL_FATAL, + [LIBUSB_LOG_LEVEL_ERROR] = LOGL_ERROR, + [LIBUSB_LOG_LEVEL_WARNING] = LOGL_NOTICE, + [LIBUSB_LOG_LEVEL_INFO] = LOGL_INFO, + [LIBUSB_LOG_LEVEL_DEBUG] = LOGL_DEBUG, +}; + +/* called by libusb if it wants to log something */ +static void libosmo_usb_log_cb(libusb_context *luctx, enum libusb_log_level level_usb, const char *str) +{ + int level = LOGL_NOTICE; + + if (level_usb < ARRAY_SIZE(usb2logl)) + level = usb2logl[level_usb]; + + LOGP(DLUSB, level, "%s", str); +} +#endif /* LIBUSB_LOG_CB_CONTEXT */ + +/*********************************************************************** + * select loop integration + ***********************************************************************/ + +static int osmo_usb_fd_cb(struct osmo_fd *ofd, unsigned int what) +{ + libusb_context *luctx = ofd->data; + + /* we assume that we're running Linux v2.6.27 with timerfd support here + * and hence don't have to perform manual timeout handling. See + * "Notes on time-based events" at + * http://libusb.sourceforge.net/api-1.0/group__libusb__poll.html */ + struct timeval zero_tv = { 0, 0 }; + libusb_handle_events_timeout(luctx, &zero_tv); + + return 0; +} + +/* called by libusb if it wants to add a file-descriptor */ +static void osmo_usb_added_cb(int fd, short events, void *user_data) +{ + struct osmo_fd *ofd = talloc_zero(OTC_GLOBAL, struct osmo_fd); + libusb_context *luctx = user_data; + unsigned int when = 0; + int rc; + + if (events & POLLIN) + when |= OSMO_FD_READ; + if (events & POLLOUT) + when |= OSMO_FD_WRITE; + + osmo_fd_setup(ofd, fd, when, osmo_usb_fd_cb, luctx, 0); + rc = osmo_fd_register(ofd); + if (rc) + LOGP(DLUSB, LOGL_ERROR, "osmo_fd_register() failed with rc=%d\n", rc); +} + +/* called by libusb if it wants to remove a file-descriptor */ +static void osmo_usb_removed_cb(int fd, void *user_data) +{ + struct osmo_fd *ofd = osmo_fd_get_by_fd(fd); + if (!ofd) + return; + osmo_fd_unregister(ofd); + talloc_free(ofd); +} + +/*********************************************************************** + * utility functions + ***********************************************************************/ + +/*! obtain the string representation of the USB device path of given device. + * \param[out] buf Output string buffer + * \param[in] bufsize Size of output string buffer in bytes + * \param[in] dev USB device whose bus path we want to obtain + * \returns pointer to 'buf' in case of success; NULL in case of error */ +char *osmo_libusb_dev_get_path_buf(char *buf, size_t bufsize, libusb_device *dev) +{ +#if (defined(LIBUSB_API_VERSION) && LIBUSB_API_VERSION >= 0x01000102) || \ + (defined(LIBUSBX_API_VERSION) && LIBUSBX_API_VERSION >= 0x01000102) + struct osmo_strbuf sb = { .buf = buf, .len = bufsize }; + uint8_t path[8]; + int r,j; + r = libusb_get_port_numbers(dev, path, sizeof(path)); + if (r > 0) { + OSMO_STRBUF_PRINTF(sb, "%d-%d", libusb_get_bus_number(dev), path[0]); + for (j = 1; j < r; j++){ + OSMO_STRBUF_PRINTF(sb, ".%d", path[j]); + } + } + return buf; +#else +# warning "libusb too old - building without USB path support!" + return NULL; +#endif +} + +/*! obtain the string representation of the USB device path of given device. + * \param[in] talloc context from which to dynamically allocate output string buffer + * \param[in] dev USB device whose bus path we want to obtain + * \returns pointer to 'buf' in case of success; NULL in case of error */ +char *osmo_libusb_dev_get_path_c(void *ctx, libusb_device *dev) +{ + char *buf = talloc_zero_size(ctx, USB_MAX_PATH_LEN); + if (!buf) + return NULL; + return osmo_libusb_dev_get_path_buf(buf, USB_MAX_PATH_LEN, dev); +} + +static int match_dev_id(const struct libusb_device_descriptor *desc, const struct dev_id *id) +{ + if ((desc->idVendor == id->vendor_id) && (desc->idProduct == id->product_id)) + return 1; + return 0; +} + +static int match_dev_ids(const struct libusb_device_descriptor *desc, const struct dev_id *ids) +{ + const struct dev_id *id; + + for (id = ids; id->vendor_id || id->product_id; id++) { + if (match_dev_id(desc, id)) + return 1; + } + return 0; +} + +/*! Find USB devices matching the specified list of USB VendorID/ProductIDs + * \param[in] ctx talloc context from which to allocate output data + * \param[in] luctx libusb context on which to operate + * \param[in] dev_ids zero-terminated array of VendorId/ProductId tuples + * \returns array of up to 256 libusb_device pointers; NULL in case of error */ +libusb_device **osmo_libusb_find_matching_usb_devs(void *ctx, struct libusb_context *luctx, + const struct dev_id *dev_ids) +{ + libusb_device **list; + libusb_device **out = talloc_zero_array(ctx, libusb_device *, 256); + libusb_device **cur = out; + unsigned int i; + int rc; + + if (!out) + return NULL; + + rc = libusb_get_device_list(luctx, &list); + if (rc <= 0) { + perror("No USB devices found"); + talloc_free(out); + return NULL; + } + + for (i = 0; list[i] != NULL; i++) { + struct libusb_device_descriptor dev_desc; + libusb_device *dev = list[i]; + + rc = libusb_get_device_descriptor(dev, &dev_desc); + if (rc < 0) { + perror("Couldn't get device descriptor\n"); + libusb_unref_device(dev); + continue; + } + + if (match_dev_ids(&dev_desc, dev_ids)) { + *cur = dev; + cur++; + /* overflow check */ + if (cur >= out + 256) + break; + } else + libusb_unref_device(dev); + } + if (cur == out) { + libusb_free_device_list(list, 1); + talloc_free(out); + return NULL; + } + + libusb_free_device_list(list, 0); + return out; +} + +/*! Find a USB device of matching VendorID/ProductID at given path. + * \param[in] luctx libusb context on which to operate + * \param[in] dev_ids zer-oterminated array of VendorId/ProductId tuples + * \param[in] path string representation of USB path + * \returns libusb_device if there was exactly one match; NULL otherwise */ +libusb_device *osmo_libusb_find_matching_dev_path(struct libusb_context *luctx, + const struct dev_id *dev_ids, + const char *path) +{ + libusb_device **list; + libusb_device *match = NULL; + unsigned int i; + int rc; + + rc = libusb_get_device_list(luctx, &list); + if (rc <= 0) + return NULL; + + for (i = 0; list[i] != NULL; i++) { + struct libusb_device_descriptor dev_desc; + libusb_device *dev = list[i]; + char pathbuf[128]; + + rc = libusb_get_device_descriptor(dev, &dev_desc); + if (rc < 0) { + LOGP(DLUSB, LOGL_ERROR, "couldn't get device descriptor\n"); + continue; + } + + /* check if device doesn't match */ + if (!match_dev_ids(&dev_desc, dev_ids)) + continue; + + /* check if path doesn't match */ + if (path) { + osmo_libusb_dev_get_path_buf(pathbuf, sizeof(pathbuf), dev); + if (strcmp(pathbuf, path)) + continue; + } + + if (match) { + /* we already have a match, but now found a second -> FAIL */ + libusb_free_device_list(list, 1); + LOGP(DLUSB, LOGL_ERROR, "Found more than one matching USB device\n"); + return NULL; + } else + match = dev; + } + + if (!match) { + /* no match: free the list with automatic unref of all devices */ + libusb_free_device_list(list, 1); + return NULL; + } + + /* unref all devices *except* the match we found */ + for (i = 0; list[i] != NULL; i++) { + libusb_device *dev = list[i]; + if (dev != match) + libusb_unref_device(dev); + } + /* free the list *without* automatic unref of all devices */ + libusb_free_device_list(list, 0); + return match; +} + +/*! Find a USB device of matching VendorID/ProductID and given iSerial string. + * \param[in] luctx libusb context on which to operate + * \param[in] dev_ids zer-oterminated array of VendorId/ProductId tuples + * \param[in] serial string representation of serial number + * \returns libusb_device if there was exactly one match; NULL otherwise */ +libusb_device *osmo_libusb_find_matching_dev_serial(struct libusb_context *luctx, + const struct dev_id *dev_ids, + const char *serial) +{ + libusb_device **list; + libusb_device *match = NULL; + unsigned int i; + int rc; + + rc = libusb_get_device_list(luctx, &list); + if (rc <= 0) + return NULL; + + for (i = 0; list[i] != NULL; i++) { + struct libusb_device_descriptor dev_desc; + libusb_device *dev = list[i]; + + rc = libusb_get_device_descriptor(dev, &dev_desc); + if (rc < 0) { + LOGP(DLUSB, LOGL_ERROR, "couldn't get device descriptor\n"); + continue; + } + + /* check if device doesn't match */ + if (!match_dev_ids(&dev_desc, dev_ids)) + continue; + + /* check if serial number string doesn't match */ + if (serial) { + char strbuf[256]; + libusb_device_handle *devh; + rc = libusb_open(dev, &devh); + if (rc < 0) { + LOGP(DLUSB, LOGL_ERROR, "Cannot open USB Device: %s\n", + libusb_strerror(rc)); + /* there's no point in continuing here, as we don't know if there + * are multiple matches if we cannot read the iSerial string of all + * devices with matching vid/pid */ + libusb_free_device_list(list, 1); + return NULL; + } + rc = libusb_get_string_descriptor_ascii(devh, dev_desc.iSerialNumber, + (uint8_t *) strbuf, sizeof(strbuf)); + if (rc < 0) { + LOGP(DLUSB, LOGL_ERROR, "Cannot read USB Descriptor: %s\n", + libusb_strerror(rc)); + libusb_close(devh); + continue; + } + libusb_close(devh); + if (strcmp(strbuf, serial)) + continue; + } + + if (match) { + /* we already have a match, but now found a second -> FAIL */ + libusb_free_device_list(list, 1); + LOGP(DLUSB, LOGL_ERROR, "Found more than one matching USB device\n"); + return NULL; + } else + match = dev; + } + + if (!match) { + /* no match: free the list with automatic unref of all devices */ + libusb_free_device_list(list, 1); + return NULL; + } + + /* unref all devices *except* the match we found */ + for (i = 0; list[i] != NULL; i++) { + libusb_device *dev = list[i]; + if (dev != match) + libusb_unref_device(dev); + } + /* free the list *without* automatic unref of all devices */ + libusb_free_device_list(list, 0); + return match; +} + + +/*! find a matching interface among all interfaces of the given USB device. + * \param[in] dev USB device in which we shall search + * \param[in] class USB Interface Class to look for + * \param[in] sub_class USB Interface Subclass to look for + * \param[in] protocol USB Interface Protocol to look for + * \param[out] out User-allocated array for storing matches + * \param[in] out_len Length of out array + * \returns number of matching interfaces; negative in case of error */ +int osmo_libusb_dev_find_matching_interfaces(libusb_device *dev, int class, int sub_class, + int protocol, struct usb_interface_match *out, + unsigned int out_len) +{ + struct libusb_device_descriptor dev_desc; + int rc, i, out_idx = 0; + uint8_t addr; + char pathbuf[USB_MAX_PATH_LEN]; + char *path; + + rc = libusb_get_device_descriptor(dev, &dev_desc); + if (rc < 0) { + perror("Couldn't get device descriptor\n"); + return -EIO; + } + + addr = libusb_get_device_address(dev); + path = osmo_libusb_dev_get_path_buf(pathbuf, sizeof(pathbuf), dev); + + /* iterate over all configurations */ + for (i = 0; i < dev_desc.bNumConfigurations; i++) { + struct libusb_config_descriptor *conf_desc; + int j; + + rc = libusb_get_config_descriptor(dev, i, &conf_desc); + if (rc < 0) { + fprintf(stderr, "Couldn't get config descriptor %u\n", i); + continue; + } + /* iterate over all interfaces */ + for (j = 0; j < conf_desc->bNumInterfaces; j++) { + const struct libusb_interface *intf = &conf_desc->interface[j]; + int k; + /* iterate over all alternate settings */ + for (k = 0; k < intf->num_altsetting; k++) { + const struct libusb_interface_descriptor *if_desc; + if_desc = &intf->altsetting[k]; + if (class >= 0 && if_desc->bInterfaceClass != class) + continue; + if (sub_class >= 0 && if_desc->bInterfaceSubClass != sub_class) + continue; + if (protocol >= 0 && if_desc->bInterfaceProtocol != protocol) + continue; + /* MATCH! */ + out[out_idx].usb_dev = dev; + out[out_idx].vendor = dev_desc.idVendor; + out[out_idx].product = dev_desc.idProduct; + out[out_idx].addr = addr; + OSMO_STRLCPY_ARRAY(out[out_idx].path, path); + out[out_idx].path[sizeof(out[out_idx].path)-1] = '\0'; + out[out_idx].configuration = conf_desc->bConfigurationValue; + out[out_idx].interface = if_desc->bInterfaceNumber; + out[out_idx].altsetting = if_desc->bAlternateSetting; + out[out_idx].class = if_desc->bInterfaceClass; + out[out_idx].sub_class = if_desc->bInterfaceSubClass; + out[out_idx].protocol = if_desc->bInterfaceProtocol; + out[out_idx].string_idx = if_desc->iInterface; + out_idx++; + if (out_idx >= out_len) + return out_idx; + } + } + } + return out_idx; +} + +/*! find matching interfaces among a list devices of specified VendorId/ProductID tuples. + * \param[in] luctx libusb context on which to operate + * \param[in] dev_ids zero-terminated array of VendorId/ProductId tuples + * \param[in] class USB Interface Class to look for + * \param[in] sub_class USB Interface Subclass to look for + * \param[in] protocol USB Interface Protocol to look for + * \param[out] out User-allocated array for storing matches + * \param[in] out_len Length of out array + * \returns number of matching interfaces; negative in case of error */ +int osmo_libusb_find_matching_interfaces(libusb_context *luctx, const struct dev_id *dev_ids, + int class, int sub_class, int protocol, + struct usb_interface_match *out, unsigned int out_len) +{ + struct usb_interface_match *out_cur = out; + unsigned int out_len_remain = out_len; + libusb_device **list; + libusb_device **dev; + + list = osmo_libusb_find_matching_usb_devs(NULL, luctx, dev_ids); + if (!list) + return 0; + + for (dev = list; *dev; dev++) { + int rc; + +#if 0 + struct libusb_device_descriptor dev_desc; + uint8_t ports[8]; + uint8_t addr; + rc = libusb_get_device_descriptor(*dev, &dev_desc); + if (rc < 0) { + perror("Cannot get device descriptor"); + continue; + } + + addr = libusb_get_device_address(*dev); + + rc = libusb_get_port_numbers(*dev, ports, sizeof(ports)); + if (rc < 0) { + perror("Cannot get device path"); + continue; + } + + printf("Found USB Device %04x:%04x at address %d\n", + dev_desc.idVendor, dev_desc.idProduct, addr); +#endif + + rc = osmo_libusb_dev_find_matching_interfaces(*dev, class, sub_class, + protocol, out_cur, out_len_remain); + if (rc < 0) + continue; + out_cur += rc; + out_len_remain -= rc; + + } + + /* unref / free list */ + for (dev = list; *dev; dev++) + libusb_unref_device(*dev); + talloc_free(list); + + return out_len - out_len_remain; +} + +/*! open matching USB device and claim interface + * \param[in] ctx talloc context to use for related allocations + * \param[in] luctx libusb context on which to operate + * \param[in] ifm interface match describing interface to claim + * \returns libusb device chandle on success; NULL on error */ +libusb_device_handle *osmo_libusb_open_claim_interface(void *ctx, libusb_context *luctx, + const struct usb_interface_match *ifm) +{ + int rc, config; + struct dev_id dev_ids[] = { { ifm->vendor, ifm->product }, { 0, 0 } }; + libusb_device **list; + libusb_device **dev; + libusb_device_handle *usb_devh = NULL; + + list = osmo_libusb_find_matching_usb_devs(ctx, luctx, dev_ids); + if (!list) { + perror("No USB device with matching VID/PID"); + return NULL; + } + + for (dev = list; *dev; dev++) { + int addr; + char pathbuf[USB_MAX_PATH_LEN]; + char *path; + + addr = libusb_get_device_address(*dev); + path = osmo_libusb_dev_get_path_buf(pathbuf, sizeof(pathbuf), *dev); + if ((ifm->addr && addr == ifm->addr) || + (strlen(ifm->path) && !strcmp(path, ifm->path)) || + (!ifm->addr && !strlen(ifm->path) && !list[1] /* only one device */)) { + rc = libusb_open(*dev, &usb_devh); + if (rc < 0) { + fprintf(stderr, "Cannot open device: %s\n", libusb_error_name(rc)); + usb_devh = NULL; + break; + } + rc = libusb_get_configuration(usb_devh, &config); + if (rc < 0) { + fprintf(stderr, "Cannot get current configuration: %s\n", libusb_error_name(rc)); + libusb_close(usb_devh); + usb_devh = NULL; + break; + } + if (config != ifm->configuration) { + rc = libusb_set_configuration(usb_devh, ifm->configuration); + if (rc < 0) { + fprintf(stderr, "Cannot set configuration: %s\n", libusb_error_name(rc)); + libusb_close(usb_devh); + usb_devh = NULL; + break; + } + } + rc = libusb_claim_interface(usb_devh, ifm->interface); + if (rc < 0) { + fprintf(stderr, "Cannot claim interface: %s\n", libusb_error_name(rc)); + libusb_close(usb_devh); + usb_devh = NULL; + break; + } + rc = libusb_set_interface_alt_setting(usb_devh, ifm->interface, ifm->altsetting); + if (rc < 0) { + fprintf(stderr, "Cannot set interface altsetting: %s\n", libusb_error_name(rc)); + libusb_release_interface(usb_devh, ifm->interface); + libusb_close(usb_devh); + usb_devh = NULL; + break; + } + } + } + + /* unref / free list */ + for (dev = list; *dev; dev++) + libusb_unref_device(*dev); + talloc_free(list); + + return usb_devh; +} + +void osmo_libusb_match_init(struct osmo_usb_matchspec *cfg, int if_class, int if_subclass, int if_proto) +{ + cfg->dev.vendor_id = -1; + cfg->dev.product_id = -1; + cfg->dev.path = NULL; + + cfg->config_id = -1; + + cfg->intf.class = if_class; + cfg->intf.subclass = if_subclass; + cfg->intf.proto = if_proto; + + cfg->intf.num = cfg->intf.altsetting = -1; +} + + +/*! high-level all-in-one function for USB device, config + interface matching + opening. + * This function offers the highest level of API among all libosmousb helper functions. It + * is intended as a one-stop shop for everything related to grabbing an interface. + * + * 1) looks for a device matching either the VID/PID from 'cfg' or 'default_dev_ids', + * if more than one is found, the user is expected to fill in cfg->dev.path to disambiguate. + * 2) find any interfaces on the device that match the specification in 'cfg'. The match + * could be done based on any of (class, subclass, proto, interface number). If there + * are multiple matches, the caller must disambiguate by specifying the interface number. + * 3) open the USB device; set the configuration (if needed); claim the interface and set + * the altsetting + * + * \param[in] cfg user-supplied match configuration (from command line or config file) + * \param[in] default_dev_ids Default list of supported VendorId/ProductIds + * \returns libusb_device_handle on success, NULL on error + */ +libusb_device_handle *osmo_libusb_find_open_claim(const struct osmo_usb_matchspec *cfg, + const struct dev_id *default_dev_ids) +{ + struct usb_interface_match if_matches[16]; + struct usb_interface_match *ifm = NULL; + libusb_device_handle *usb_devh = NULL; + struct dev_id user_dev_ids[2] = { + { cfg->dev.vendor_id, cfg->dev.product_id }, + { 0, 0 } + }; + const struct dev_id *dev_ids = default_dev_ids; + libusb_device *dev; + int rc, i; + + /* Stage 1: Find a device matching either the user-specified VID/PID or + * the list of IDs in default_dev_ids plus optionally the user-specified path */ + if (cfg->dev.vendor_id != -1 || cfg->dev.product_id != -1) + dev_ids = user_dev_ids; + dev = osmo_libusb_find_matching_dev_path(NULL, dev_ids, cfg->dev.path); + if (!dev) + goto close_exit; + + /* Stage 2: Find any interfaces matching the class/subclass/proto as specified */ + rc = osmo_libusb_dev_find_matching_interfaces(dev, cfg->intf.class, cfg->intf.subclass, + cfg->intf.proto, if_matches, sizeof(if_matches)); + if (rc < 1) { + LOGP(DLUSB, LOGL_NOTICE, "can't find matching USB interface at device\n"); + goto close_exit; + } else if (rc == 1) { + ifm = if_matches; + } else if (rc > 1) { + if (cfg->intf.num == -1) { + LOGP(DLUSB, LOGL_ERROR, "Found %d matching USB interfaces, you " + "have to specify the interface number\n", rc); + goto close_exit; + } + for (i = 0; i < rc; i++) { + if (if_matches[i].interface == cfg->intf.num) { + ifm = &if_matches[i]; + break; + } + /* FIXME: match altsetting */ + } + } + if (!ifm) { + LOGP(DLUSB, LOGL_NOTICE, "Couldn't find matching interface\n"); + goto close_exit; + } + + /* Stage 3: Open device; set config (if required); claim interface; set altsetting */ + usb_devh = osmo_libusb_open_claim_interface(NULL, NULL, ifm); + if (!usb_devh) { + LOGP(DLUSB, LOGL_ERROR, "can't open USB device (permissions issue?)\n"); + goto close_exit; + } + return usb_devh; +close_exit: + /* release if_matches */ + if (usb_devh) + libusb_close(usb_devh); + + return NULL; +} + +/*! obtain the endpoint addresses for a given USB interface. + * \param[in] devh USB device handle on which to operate + * \param[in] if_num USB Interface number on which to operate + * \param[out] out user-provided storage for OUT endpoint number + * \param[out] in user-provided storage for IN endpoint number + * \param[out] irq user-provided storage for IRQ endpoint number + * \returns 0 in case of success; negative in case of error */ +int osmo_libusb_get_ep_addrs(libusb_device_handle *devh, unsigned int if_num, + uint8_t *out, uint8_t *in, uint8_t *irq) +{ + libusb_device *dev = libusb_get_device(devh); + struct libusb_config_descriptor *cdesc; + const struct libusb_interface_descriptor *idesc; + const struct libusb_interface *iface; + int rc, l; + + rc = libusb_get_active_config_descriptor(dev, &cdesc); + if (rc < 0) + return rc; + + iface = &cdesc->interface[if_num]; + /* FIXME: we assume there's no altsetting */ + idesc = &iface->altsetting[0]; + + for (l = 0; l < idesc->bNumEndpoints; l++) { + const struct libusb_endpoint_descriptor *edesc = &idesc->endpoint[l]; + switch (edesc->bmAttributes & 3) { + case LIBUSB_TRANSFER_TYPE_BULK: + if (edesc->bEndpointAddress & 0x80) { + if (in) + *in = edesc->bEndpointAddress; + } else { + if (out) + *out = edesc->bEndpointAddress; + } + break; + case LIBUSB_TRANSFER_TYPE_INTERRUPT: + if (irq) + *irq = edesc->bEndpointAddress; + break; + default: + break; + } + } + return 0; +} +/*********************************************************************** + * initialization + ***********************************************************************/ + +int osmo_libusb_init(libusb_context **pluctx) +{ + libusb_context *luctx = NULL; + const struct libusb_pollfd **pfds; + + int rc; + + rc = libusb_init(pluctx); + if (rc != 0) { + LOGP(DLUSB, LOGL_ERROR, "Error initializing libusb: %s\n", libusb_strerror(rc)); + return rc; + } + + if (pluctx) + luctx = *pluctx; + +#ifdef LIBUSB_LOG_CB_CONTEXT /* introduced in 1.0.23 */ + libusb_set_log_cb(luctx, &libosmo_usb_log_cb, LIBUSB_LOG_CB_CONTEXT); +#endif + + libusb_set_pollfd_notifiers(luctx, osmo_usb_added_cb, osmo_usb_removed_cb, luctx); + + /* get the initial file descriptors which were created even before during libusb_init() */ + pfds = libusb_get_pollfds(luctx); + if (pfds) { + const struct libusb_pollfd **pfds2 = pfds; + const struct libusb_pollfd *pfd; + /* synthesize 'add' call-backs. not sure why libusb doesn't do that by itself? */ + for (pfd = *pfds2; pfd; pfd = *++pfds2) + osmo_usb_added_cb(pfd->fd, pfd->events, luctx); + libusb_free_pollfds(pfds); + } + + return 0; +} + +void osmo_libusb_exit(libusb_context *luctx) +{ + /* we just assume libusb is cleaning up all the osmo_Fd's we've allocated */ + libusb_exit(luctx); +} diff --git a/src/vty/Makefile.am b/src/vty/Makefile.am index abed92ac..c314a141 100644 --- a/src/vty/Makefile.am +++ b/src/vty/Makefile.am @@ -1,10 +1,10 @@ # This is _NOT_ the library release version, it's an API version. # Please read chapter "Library interface versions" of the libtool documentation # before making any modifications: https://www.gnu.org/software/libtool/manual/html_node/Versioning.html -LIBVERSION=8:0:4 +LIBVERSION=13:0:0 -AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -AM_CFLAGS = -Wall $(TALLOC_CFLAGS) +AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include -I$(top_builddir) +AM_CFLAGS = -Wall $(TALLOC_CFLAGS) $(PTHREAD_CFLAGS) if ENABLE_VTY lib_LTLIBRARIES = libosmovty.la @@ -12,7 +12,7 @@ lib_LTLIBRARIES = libosmovty.la libosmovty_la_SOURCES = buffer.c command.c vty.c vector.c utils.c \ telnet_interface.c logging_vty.c stats_vty.c \ fsm_vty.c talloc_ctx_vty.c \ - tdef_vty.c + cpu_sched_vty.c tdef_vty.c libosmovty_la_LDFLAGS = -version-info $(LIBVERSION) -no-undefined -libosmovty_la_LIBADD = $(top_builddir)/src/libosmocore.la $(TALLOC_LIBS) +libosmovty_la_LIBADD = $(top_builddir)/src/core/libosmocore.la $(TALLOC_LIBS) $(PTHREAD_LIBS) endif diff --git a/src/vty/command.c b/src/vty/command.c index 6a9d18af..1719690b 100644 --- a/src/vty/command.c +++ b/src/vty/command.c @@ -37,14 +37,19 @@ Boston, MA 02110-1301, USA. */ #include <unistd.h> #include <ctype.h> #include <time.h> +#include <limits.h> +#include <signal.h> #include <sys/time.h> #include <sys/stat.h> +#include <sys/types.h> #include <osmocom/vty/vector.h> #include <osmocom/vty/vty.h> #include <osmocom/vty/command.h> +#include <osmocom/core/logging.h> #include <osmocom/core/talloc.h> +#include <osmocom/core/timer.h> #include <osmocom/core/utils.h> #ifndef MAXPATHLEN @@ -62,6 +67,9 @@ Boston, MA 02110-1301, USA. */ void *tall_vty_cmd_ctx; +/* Set by on_dso_load_starttime() for "show uptime". */ +static struct timespec starttime; + /* Command vector which includes some level of command lists. Normally each daemon maintains each own cmdvec. */ vector cmdvec; @@ -69,6 +77,21 @@ vector cmdvec; /* Host information structure. */ struct host host; +struct vty_parent_node { + struct llist_head entry; + + /*! private data, specified by creator */ + void *priv; + void *index; + + /*! Node status of this vty */ + int node; + + /*! When reading from a config file, these are the indenting characters expected for children of + * this VTY node. */ + char *indent; +}; + /* Standard command node structures. */ struct cmd_node auth_node = { AUTH_NODE, @@ -200,18 +223,6 @@ static int cmp_desc(const void *p, const void *q) return strcmp(a->cmd, b->cmd); } -static int is_config_child(struct vty *vty) -{ - if (vty->node <= CONFIG_NODE) - return 0; - else if (vty->node > CONFIG_NODE && vty->node < _LAST_OSMOVTY_NODE) - return 1; - else if (host.app_info->is_config_node) - return host.app_info->is_config_node(vty, vty->node); - else - return vty->node > CONFIG_NODE; -} - /*! Sort each node's command element according to command string. */ void sort_node(void) { @@ -631,81 +642,209 @@ static char *xml_escape(const char *inp) return out; } +typedef int (*print_func_t)(void *data, const char *fmt, ...); + +static const struct value_string cmd_attr_desc[] = { + { CMD_ATTR_DEPRECATED, "This command is deprecated" }, + { CMD_ATTR_HIDDEN, "This command is hidden (check expert mode)" }, + { CMD_ATTR_IMMEDIATE, "This command applies immediately" }, + { CMD_ATTR_NODE_EXIT, "This command applies on VTY node exit" }, + /* CMD_ATTR_LIB_COMMAND is intentionally skipped */ + { 0, NULL } +}; + +/* Public attributes (to be printed in the VTY / XML reference) */ +#define CMD_ATTR_PUBLIC_MASK \ + (CMD_ATTR_HIDDEN | CMD_ATTR_IMMEDIATE | CMD_ATTR_NODE_EXIT) + +/* Get a flag character for a global VTY command attribute */ +static char cmd_attr_get_flag(unsigned int attr) +{ + switch (attr) { + case CMD_ATTR_HIDDEN: + return '^'; + case CMD_ATTR_IMMEDIATE: + return '!'; + case CMD_ATTR_NODE_EXIT: + return '@'; + default: + return '.'; + } +} + +/* Description of attributes shared between the lib commands */ +static const char * const cmd_lib_attr_desc[32] = { + /* [OSMO_LIBNAME_LIB_ATTR_ATTRNAME] = \ + * "Brief but meaningful description", */ + [OSMO_SCCP_LIB_ATTR_RSTRT_ASP] = \ + "This command applies on ASP restart", + [OSMO_ABIS_LIB_ATTR_IPA_NEW_LNK] = \ + "This command applies on IPA link establishment", + [OSMO_ABIS_LIB_ATTR_LINE_UPD] = \ + "This command applies on E1 line update", +}; + +/* Flag letters of attributes shared between the lib commands. + * NOTE: uppercase letters only, the rest is reserved for applications. */ +static const char cmd_lib_attr_letters[32] = { + /* [OSMO_LIBNAME_LIB_ATTR_ATTRNAME] = 'X', */ + [OSMO_SCCP_LIB_ATTR_RSTRT_ASP] = 'A', + [OSMO_ABIS_LIB_ATTR_IPA_NEW_LNK] = 'I', + [OSMO_ABIS_LIB_ATTR_LINE_UPD] = 'L', +}; + /* - * Write one cmd_element as XML to the given VTY. + * Write one cmd_element as XML via a print_func_t. */ -static int vty_dump_element(struct cmd_element *cmd, struct vty *vty) +static int vty_dump_element(const struct cmd_element *cmd, print_func_t print_func, + void *data, const char *newline) { char *xml_string = xml_escape(cmd->string); + unsigned int i; - vty_out(vty, " <command id='%s'>%s", xml_string, VTY_NEWLINE); - vty_out(vty, " <params>%s", VTY_NEWLINE); + print_func(data, " <command id='%s'>%s", xml_string, newline); - int j; - for (j = 0; j < vector_count(cmd->strvec); ++j) { - vector descvec = vector_slot(cmd->strvec, j); - int i; - for (i = 0; i < vector_active(descvec); ++i) { + /* Print global attributes and their description */ + if (cmd->attr & CMD_ATTR_PUBLIC_MASK) { /* ... only public ones */ + print_func(data, " <attributes scope='global'>%s", newline); + + for (i = 0; i < ARRAY_SIZE(cmd_attr_desc) - 1; i++) { + char *xml_att_desc; + char flag; + + if (~cmd->attr & cmd_attr_desc[i].value) + continue; + + xml_att_desc = xml_escape(cmd_attr_desc[i].str); + print_func(data, " <attribute doc='%s'", + xml_att_desc, newline); + talloc_free(xml_att_desc); + + flag = cmd_attr_get_flag(cmd_attr_desc[i].value); + if (flag != '.') + print_func(data, " flag='%c'", flag); + print_func(data, " />%s", newline); + } + + print_func(data, " </attributes>%s", newline); + } + + /* Print application specific attributes and their description */ + if (cmd->usrattr != 0x00) { /* ... if at least one flag is set */ + const char * const *desc; + const char *letters; + + if (cmd->attr & CMD_ATTR_LIB_COMMAND) { + print_func(data, " <attributes scope='library'>%s", newline); + letters = &cmd_lib_attr_letters[0]; + desc = &cmd_lib_attr_desc[0]; + } else { + print_func(data, " <attributes scope='application'>%s", newline); + letters = &host.app_info->usr_attr_letters[0]; + desc = &host.app_info->usr_attr_desc[0]; + } + + for (i = 0; i < ARRAY_SIZE(host.app_info->usr_attr_desc); i++) { + char *xml_att_desc; + char flag; + + /* Skip attribute if *not* set */ + if (~cmd->usrattr & ((unsigned)1 << i)) + continue; + + xml_att_desc = xml_escape(desc[i]); + print_func(data, " <attribute doc='%s'", xml_att_desc); + talloc_free(xml_att_desc); + + if ((flag = letters[i]) != '\0') + print_func(data, " flag='%c'", flag); + print_func(data, " />%s", newline); + } + + print_func(data, " </attributes>%s", newline); + } + + print_func(data, " <params>%s", newline); + + for (i = 0; i < vector_count(cmd->strvec); ++i) { + vector descvec = vector_slot(cmd->strvec, i); + int j; + for (j = 0; j < vector_active(descvec); ++j) { char *xml_param, *xml_doc; - struct desc *desc = vector_slot(descvec, i); + struct desc *desc = vector_slot(descvec, j); if (desc == NULL) continue; xml_param = xml_escape(desc->cmd); xml_doc = xml_escape(desc->str); - vty_out(vty, " <param name='%s' doc='%s' />%s", - xml_param, xml_doc, VTY_NEWLINE); + print_func(data, " <param name='%s' doc='%s' />%s", + xml_param, xml_doc, newline); talloc_free(xml_param); talloc_free(xml_doc); } } - vty_out(vty, " </params>%s", VTY_NEWLINE); - vty_out(vty, " </command>%s", VTY_NEWLINE); + print_func(data, " </params>%s", newline); + print_func(data, " </command>%s", newline); talloc_free(xml_string); return 0; } -static bool vty_command_is_common(struct cmd_element *cmd); +static bool vty_command_is_common(const struct cmd_element *cmd); /* - * Dump all nodes and commands associated with a given node as XML to the VTY. + * Dump all nodes and commands associated with a given node as XML via a print_func_t. + * + * (gflag_mask, match = false) - print only those commands with non-matching flags. + * (gflag_mask, match = true) - print only those commands with matching flags. + * + * Some examples: + * + * Print all commands except deprecated and hidden (default mode): + * (CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN, false) + * Print only deprecated and hidden commands: + * (CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN, true) + * Print all commands except deprecated (expert mode): + * (CMD_ATTR_DEPRECATED, false) + * Print only hidden commands: + * (CMD_ATTR_HIDDEN, true) */ -static int vty_dump_nodes(struct vty *vty) +static int vty_dump_nodes(print_func_t print_func, void *data, const char *newline, + unsigned char gflag_mask, bool match) { int i, j; int same_name_count; - vty_out(vty, "<vtydoc xmlns='urn:osmocom:xml:libosmocore:vty:doc:1.0'>%s", VTY_NEWLINE); + print_func(data, "<vtydoc xmlns='urn:osmocom:xml:libosmocore:vty:doc:1.0'>%s", newline); /* Only once, list all common node commands. Use the CONFIG node to find common node commands. */ - vty_out(vty, " <node id='_common_cmds_'>%s", VTY_NEWLINE); - vty_out(vty, " <name>Common Commands</name>%s", VTY_NEWLINE); - vty_out(vty, " <description>These commands are available on all VTY nodes. They are listed" - " here only once, to unclutter the VTY reference.</description>%s", VTY_NEWLINE); + print_func(data, " <node id='_common_cmds_'>%s", newline); + print_func(data, " <name>Common Commands</name>%s", newline); + print_func(data, " <description>These commands are available on all VTY nodes. They are listed" + " here only once, to unclutter the VTY reference.</description>%s", newline); for (i = 0; i < vector_active(cmdvec); ++i) { - struct cmd_node *cnode; - cnode = vector_slot(cmdvec, i); + const struct cmd_node *cnode = vector_slot(cmdvec, i); if (!cnode) continue; if (cnode->node != CONFIG_NODE) continue; for (j = 0; j < vector_active(cnode->cmd_vector); ++j) { - struct cmd_element *elem; - elem = vector_slot(cnode->cmd_vector, j); + const struct cmd_element *elem = vector_slot(cnode->cmd_vector, j); if (!vty_command_is_common(elem)) continue; - if (!(elem->attr & (CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN))) - vty_dump_element(elem, vty); + if (!match && (elem->attr & gflag_mask) != 0x00) + continue; + if (match && (elem->attr & gflag_mask) == 0x00) + continue; + vty_dump_element(elem, print_func, data, newline); } } - vty_out(vty, " </node>%s", VTY_NEWLINE); + print_func(data, " </node>%s", newline); for (i = 0; i < vector_active(cmdvec); ++i) { - struct cmd_node *cnode; - cnode = vector_slot(cmdvec, i); + const struct cmd_node *cnode = vector_slot(cmdvec, i); if (!cnode) continue; if (vector_active(cnode->cmd_vector) < 1) @@ -716,37 +855,124 @@ static int vty_dump_nodes(struct vty *vty) * 'name', the second becomes 'name_2', then 'name_3', ... */ same_name_count = 1; for (j = 0; j < i; ++j) { - struct cmd_node *cnode2; - cnode2 = vector_slot(cmdvec, j); + const struct cmd_node *cnode2 = vector_slot(cmdvec, j); if (!cnode2) continue; if (strcmp(cnode->name, cnode2->name) == 0) same_name_count ++; } - vty_out(vty, " <node id='%s", cnode->name); + print_func(data, " <node id='%s", cnode->name); if (same_name_count > 1 || !*cnode->name) - vty_out(vty, "_%d", same_name_count); - vty_out(vty, "'>%s", VTY_NEWLINE); - vty_out(vty, " <name>%s</name>%s", cnode->name, VTY_NEWLINE); + print_func(data, "_%d", same_name_count); + print_func(data, "'>%s", newline); + print_func(data, " <name>%s</name>%s", cnode->name, newline); for (j = 0; j < vector_active(cnode->cmd_vector); ++j) { - struct cmd_element *elem; - elem = vector_slot(cnode->cmd_vector, j); + const struct cmd_element *elem = vector_slot(cnode->cmd_vector, j); if (vty_command_is_common(elem)) continue; - if (!(elem->attr & (CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN))) - vty_dump_element(elem, vty); + if (!match && (elem->attr & gflag_mask) != 0x00) + continue; + if (match && (elem->attr & gflag_mask) == 0x00) + continue; + vty_dump_element(elem, print_func, data, newline); } - vty_out(vty, " </node>%s", VTY_NEWLINE); + print_func(data, " </node>%s", newline); } - vty_out(vty, "</vtydoc>%s", VTY_NEWLINE); + print_func(data, "</vtydoc>%s", newline); return 0; } +static int print_func_vty(void *data, const char *format, ...) +{ + struct vty *vty = data; + va_list args; + int rc; + va_start(args, format); + rc = vty_out_va(vty, format, args); + va_end(args); + return rc; +} + +static int vty_dump_xml_ref_to_vty(struct vty *vty) +{ + unsigned char gflag_mask = CMD_ATTR_DEPRECATED; + if (!vty->expert_mode) + gflag_mask |= CMD_ATTR_HIDDEN; + return vty_dump_nodes(print_func_vty, vty, VTY_NEWLINE, gflag_mask, false); +} + +static int print_func_stream(void *data, const char *format, ...) +{ + va_list args; + int rc; + va_start(args, format); + rc = vfprintf((FILE*)data, format, args); + va_end(args); + return rc; +} + +const struct value_string vty_ref_gen_mode_names[] = { + { VTY_REF_GEN_MODE_DEFAULT, "default" }, + { VTY_REF_GEN_MODE_EXPERT, "expert" }, + { VTY_REF_GEN_MODE_HIDDEN, "hidden" }, + { 0, NULL } +}; + +const struct value_string vty_ref_gen_mode_desc[] = { + { VTY_REF_GEN_MODE_DEFAULT, "all commands except deprecated and hidden" }, + { VTY_REF_GEN_MODE_EXPERT, "all commands including hidden, excluding deprecated" }, + { VTY_REF_GEN_MODE_HIDDEN, "hidden commands only" }, + { 0, NULL } +}; + +/*! Print the XML reference of all VTY nodes to the given stream. + * \param[out] stream Output stream to print the XML reference to. + * \param[in] mode The XML reference generation mode. + * \returns always 0 for now, no errors possible. + */ +int vty_dump_xml_ref_mode(FILE *stream, enum vty_ref_gen_mode mode) +{ + unsigned char gflag_mask; + bool match = false; + + switch (mode) { + case VTY_REF_GEN_MODE_EXPERT: + /* All commands except deprecated */ + gflag_mask = CMD_ATTR_DEPRECATED; + break; + case VTY_REF_GEN_MODE_HIDDEN: + /* Only hidden commands */ + gflag_mask = CMD_ATTR_HIDDEN; + match = true; + break; + case VTY_REF_GEN_MODE_DEFAULT: + default: + /* All commands except deprecated and hidden */ + gflag_mask = CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN; + break; + } + + return vty_dump_nodes(print_func_stream, stream, "\n", gflag_mask, match); +} + +/*! Print the XML reference of all VTY nodes to the given stream. + * \param[out] stream Output stream to print the XML reference to. + * \returns always 0 for now, no errors possible. + * + * NOTE: this function is deprecated because it does not allow to + * specify the XML reference generation mode (default mode + * is hard-coded). Use vty_dump_xml_ref_mode() instead. + */ +int vty_dump_xml_ref(FILE *stream) +{ + return vty_dump_xml_ref_mode(stream, VTY_REF_GEN_MODE_DEFAULT); +} + /* Check if a command with given string exists at given node */ static int check_element_exists(struct cmd_node *cnode, const char *cmdstring) { @@ -784,6 +1010,16 @@ void install_element(int ntype, struct cmd_element *cmd) cmd->cmdsize = cmd_cmdsize(cmd->strvec); } +/*! Install a library command into a node + * \param[in] ntype Node Type + * \param[in] cmd element to be installed + */ +void install_lib_element(int ntype, struct cmd_element *cmd) +{ + cmd->attr |= CMD_ATTR_LIB_COMMAND; + install_element(ntype, cmd); +} + /* Install a command into VIEW and ENABLE node */ void install_element_ve(struct cmd_element *cmd) { @@ -791,6 +1027,13 @@ void install_element_ve(struct cmd_element *cmd) install_element(ENABLE_NODE, cmd); } +/* Install a library command into VIEW and ENABLE node */ +void install_lib_element_ve(struct cmd_element *cmd) +{ + cmd->attr |= CMD_ATTR_LIB_COMMAND; + install_element_ve(cmd); +} + #ifdef VTY_CRYPT_PW static unsigned char itoa64[] = "./0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz"; @@ -864,6 +1107,7 @@ static int config_write_host(struct vty *vty) static vector cmd_node_vector(vector v, enum node_type ntype) { struct cmd_node *cnode = vector_slot(v, ntype); + OSMO_ASSERT(cnode != NULL); return cnode->cmd_vector; } @@ -1121,7 +1365,6 @@ static enum match_type cmd_ipv6_prefix_match(const char *str) int colons = 0, nums = 0, double_colon = 0; int mask; const char *sp = NULL; - char *endptr = NULL; if (str == NULL) return PARTLY_MATCH; @@ -1219,11 +1462,7 @@ static enum match_type cmd_ipv6_prefix_match(const char *str) if (state < STATE_MASK) return PARTLY_MATCH; - mask = strtol(str, &endptr, 10); - if (*endptr != '\0') - return NO_MATCH; - - if (mask < 0 || mask > 128) + if (osmo_str_to_int(&mask, str, 10, 0, 128)) return NO_MATCH; /* I don't know why mask < 13 makes command match partly. @@ -1239,21 +1478,69 @@ static enum match_type cmd_ipv6_prefix_match(const char *str) #endif /* HAVE_IPV6 */ -#define DECIMAL_STRLEN_MAX 10 -static int cmd_range_match(const char *range, const char *str) +#if ULONG_MAX == 18446744073709551615UL +#define DECIMAL_STRLEN_MAX_UNSIGNED 20 +#elif ULONG_MAX == 4294967295UL +#define DECIMAL_STRLEN_MAX_UNSIGNED 10 +#else +#error "ULONG_MAX not defined!" +#endif + +#if LONG_MAX == 9223372036854775807L +#define DECIMAL_STRLEN_MAX_SIGNED 19 +#elif LONG_MAX == 2147483647L +#define DECIMAL_STRLEN_MAX_SIGNED 10 +#else +#error "LONG_MAX not defined!" +#endif + +/* This function is aimed at quickly guessing & filtering the numeric base a + * string can contain, by no means validates the entire value. + * Returns 16 if string follows pattern "({+,-}0x[digits])" + * Returns 10 if string follows pattern "({+,-}[digits])" + * Returns a negative value if something other is detected (error) +*/ +static int check_base(const char *str) +{ + const char *ptr = str; + /* Skip any space */ + while (isspace(*ptr)) ptr++; + + /* Skip optional sign: */ + if (*ptr == '+' || *ptr == '-') + ptr++; + if (*ptr == '0') { + ptr++; + if (*ptr == '\0' || isdigit(*ptr)) + return 10; + if (!(*ptr == 'x' || *ptr == 'X')) + return -1; + ptr++; + if (isxdigit(*ptr)) + return 16; + return -1; + } + return 10; +} + +int vty_cmd_range_match(const char *range, const char *str) { char *p; - char buf[DECIMAL_STRLEN_MAX + 1]; + char buf[DECIMAL_STRLEN_MAX_UNSIGNED + 1]; char *endptr = NULL; + int min_base, max_base, val_base; if (str == NULL) return 1; + if ((val_base = check_base(str)) < 0) + return 0; + if (range[1] == '-') { signed long min = 0, max = 0, val; - val = strtol(str, &endptr, 10); + val = strtol(str, &endptr, val_base); if (*endptr != '\0') return 0; @@ -1261,11 +1548,13 @@ static int cmd_range_match(const char *range, const char *str) p = strchr(range, '-'); if (p == NULL) return 0; - if (p - range > DECIMAL_STRLEN_MAX) + if (p - range > DECIMAL_STRLEN_MAX_SIGNED) return 0; strncpy(buf, range, p - range); buf[p - range] = '\0'; - min = -strtol(buf, &endptr, 10); + if ((min_base = check_base(buf)) < 0) + return 0; + min = -strtol(buf, &endptr, min_base); if (*endptr != '\0') return 0; @@ -1273,11 +1562,13 @@ static int cmd_range_match(const char *range, const char *str) p = strchr(range, '>'); if (p == NULL) return 0; - if (p - range > DECIMAL_STRLEN_MAX) + if (p - range > DECIMAL_STRLEN_MAX_SIGNED) return 0; strncpy(buf, range, p - range); buf[p - range] = '\0'; - max = strtol(buf, &endptr, 10); + if ((max_base = check_base(buf)) < 0) + return 0; + max = strtol(buf, &endptr, max_base); if (*endptr != '\0') return 0; @@ -1286,7 +1577,10 @@ static int cmd_range_match(const char *range, const char *str) } else { unsigned long min, max, val; - val = strtoul(str, &endptr, 10); + if (str[0] == '-') + return 0; + + val = strtoul(str, &endptr, val_base); if (*endptr != '\0') return 0; @@ -1294,11 +1588,13 @@ static int cmd_range_match(const char *range, const char *str) p = strchr(range, '-'); if (p == NULL) return 0; - if (p - range > DECIMAL_STRLEN_MAX) + if (p - range > DECIMAL_STRLEN_MAX_UNSIGNED) return 0; strncpy(buf, range, p - range); buf[p - range] = '\0'; - min = strtoul(buf, &endptr, 10); + if ((min_base = check_base(buf)) < 0) + return 0; + min = strtoul(buf, &endptr, min_base); if (*endptr != '\0') return 0; @@ -1306,11 +1602,13 @@ static int cmd_range_match(const char *range, const char *str) p = strchr(range, '>'); if (p == NULL) return 0; - if (p - range > DECIMAL_STRLEN_MAX) + if (p - range > DECIMAL_STRLEN_MAX_UNSIGNED) return 0; strncpy(buf, range, p - range); buf[p - range] = '\0'; - max = strtoul(buf, &endptr, 10); + if ((max_base = check_base(buf)) < 0) + return 0; + max = strtoul(buf, &endptr, max_base); if (*endptr != '\0') return 0; @@ -1318,6 +1616,14 @@ static int cmd_range_match(const char *range, const char *str) return 0; } + /* Don't allow ranges specified by min and max with different bases */ + if (min_base != max_base) + return 0; + /* arg value passed must match the base of the range */ + if (min_base != val_base) + return 0; + + /* Everything's fine */ return 1; } @@ -1361,7 +1667,7 @@ cmd_match(const char *str, const char *command, return VARARG_MATCH; else if (CMD_RANGE(str)) { - if (cmd_range_match(str, command)) + if (vty_cmd_range_match(str, command)) return RANGE_MATCH; } #ifdef HAVE_IPV6 @@ -1558,7 +1864,7 @@ is_cmd_ambiguous(char *command, vector v, int index, enum match_type type) } break; case RANGE_MATCH: - if (cmd_range_match + if (vty_cmd_range_match (str, command)) { if (matched && strcmp(matched, @@ -1651,7 +1957,7 @@ static const char *cmd_entry_function_desc(const char *src, const char *dst) return dst; if (CMD_RANGE(dst)) { - if (cmd_range_match(dst, src)) + if (vty_cmd_range_match(dst, src)) return dst; else return NULL; @@ -1840,7 +2146,9 @@ cmd_describe_command_real(vector vline, struct vty *vty, int *status) if (!cmd_element) continue; - if (cmd_element->attr & (CMD_ATTR_DEPRECATED|CMD_ATTR_HIDDEN)) + if (cmd_element->attr & CMD_ATTR_DEPRECATED) + continue; + if (!vty->expert_mode && (cmd_element->attr & CMD_ATTR_HIDDEN)) continue; strvec = cmd_element->strvec; @@ -2145,6 +2453,7 @@ static bool vty_pop_parent(struct vty *vty) llist_del(&parent->entry); vty->node = parent->node; vty->priv = parent->priv; + vty->index = parent->index; if (vty->indent) talloc_free(vty->indent); vty->indent = parent->indent; @@ -2175,38 +2484,23 @@ static void vty_clear_parents(struct vty *vty) int vty_go_parent(struct vty *vty) { switch (vty->node) { - case AUTH_NODE: - case VIEW_NODE: - case ENABLE_NODE: - case CONFIG_NODE: - vty_clear_parents(vty); - break; - - case AUTH_ENABLE_NODE: - vty->node = VIEW_NODE; - vty_clear_parents(vty); - break; + case AUTH_NODE: + case VIEW_NODE: + case ENABLE_NODE: + case CONFIG_NODE: + vty_clear_parents(vty); + break; - case CFG_LOG_NODE: - case VTY_NODE: - vty->node = CONFIG_NODE; - vty_clear_parents(vty); - break; + case AUTH_ENABLE_NODE: + vty->node = VIEW_NODE; + vty_clear_parents(vty); + break; - default: - if (host.app_info->go_parent_cb) { - host.app_info->go_parent_cb(vty); - vty_pop_parent(vty); - } - else if (is_config_child(vty)) { - vty->node = CONFIG_NODE; - vty_clear_parents(vty); - } - else { - vty->node = VIEW_NODE; - vty_clear_parents(vty); - } - break; + default: + if (host.app_info->go_parent_cb) + host.app_info->go_parent_cb(vty); + vty_pop_parent(vty); + break; } return vty->node; @@ -2365,9 +2659,31 @@ cmd_execute_command_real(vector vline, struct vty *vty, if (matched_element->daemon) rc = CMD_SUCCESS_DAEMON; - else /* Execute matched command. */ + else { + /* Execute matched command. */ + struct vty_parent_node this_node = { + .node = vty->node, + .priv = vty->priv, + .index = vty->index, + .indent = vty->indent, + }; + struct vty_parent_node *parent = vty_parent(vty); rc = (*matched_element->func) (matched_element, vty, argc, argv); + /* If we have stepped down into a child node, push a parent frame. + * The causality is such: we don't expect every single node entry implementation to push + * a parent node entry onto vty->parent_nodes. Instead we expect vty_go_parent() to *pop* + * a parent node. Hence if the node changed without the parent node changing, we must + * have stepped into a child node. */ + if (vty->node != this_node.node && parent == vty_parent(vty) + && vty->node > CONFIG_NODE) { + /* Push the parent node. */ + parent = talloc_zero(vty, struct vty_parent_node); + *parent = this_node; + llist_add(&parent->entry, &vty->parent_nodes); + } + } + rc_free_deopt_ctx: /* Now after we called the command func, we can free temporary strings */ talloc_free(cmd_deopt_ctx); @@ -2624,6 +2940,7 @@ int config_from_file(struct vty *vty, FILE * fp) this_node = (struct vty_parent_node){ .node = vty->node, .priv = vty->priv, + .index = vty->index, .indent = vty->indent, }; @@ -2680,7 +2997,7 @@ return_invalid_indent: /* Configration from terminal */ DEFUN(config_terminal, config_terminal_cmd, - "configure terminal", + "configure [terminal]", "Configuration from vty interface\n" "Configuration terminal\n") { if (vty_config_lock(vty)) @@ -2694,7 +3011,10 @@ DEFUN(config_terminal, } /* Enable command */ -DEFUN(enable, config_enable_cmd, "enable", "Turn on privileged mode command\n") +DEFUN(enable, config_enable_cmd, + "enable [expert-mode]", + "Turn on privileged mode command\n" + "Enable the expert mode (show hidden commands)\n") { /* If enable password is NULL, change to ENABLE_NODE */ if ((host.enable == NULL && host.enable_encrypt == NULL) || @@ -2703,6 +3023,8 @@ DEFUN(enable, config_enable_cmd, "enable", "Turn on privileged mode command\n") else vty->node = AUTH_ENABLE_NODE; + vty->expert_mode = argc > 0; + return CMD_SUCCESS; } @@ -2712,6 +3034,9 @@ DEFUN(disable, { if (vty->node == ENABLE_NODE) vty->node = VIEW_NODE; + + vty->expert_mode = false; + return CMD_SUCCESS; } @@ -2763,6 +3088,18 @@ gDEFUN(config_exit, return CMD_SUCCESS; } +DEFUN(shutdown, + shutdown_cmd, "shutdown", "Request a shutdown of the program\n") +{ + LOGP(DLGLOBAL, LOGL_INFO, "Shutdown requested from telnet\n"); + vty_out(vty, "%s is shutting down. Bye!%s", host.app_info->name, VTY_NEWLINE); + + /* Same exit path as if it was killed by the service manager */ + kill(getpid(), SIGTERM); + + return CMD_SUCCESS; +} + /* Show version. */ DEFUN(show_version, show_version_cmd, "show version", SHOW_STR "Displays program version\n") @@ -2778,7 +3115,24 @@ DEFUN(show_version, DEFUN(show_online_help, show_online_help_cmd, "show online-help", SHOW_STR "Online help\n") { - vty_dump_nodes(vty); + vty_dump_xml_ref_to_vty(vty); + return CMD_SUCCESS; +} + +DEFUN(show_pid, + show_pid_cmd, "show pid", SHOW_STR "Displays the process ID\n") +{ + vty_out(vty, "%d%s", getpid(), VTY_NEWLINE); + return CMD_SUCCESS; +} + +DEFUN(show_uptime, + show_uptime_cmd, "show uptime", SHOW_STR "Displays how long the program has been running\n") +{ + vty_out(vty, "%s has been running for ", host.app_info->name); + vty_out_uptime(vty, &starttime); + vty_out_newline(vty); + return CMD_SUCCESS; } @@ -2786,34 +3140,235 @@ DEFUN(show_online_help, gDEFUN(config_help, config_help_cmd, "help", "Description of the interactive help system\n") { - vty_out(vty, - "This VTY provides advanced help features. When you need help,%s\ -anytime at the command line please press '?'.%s\ -%s\ -If nothing matches, the help list will be empty and you must backup%s\ - until entering a '?' shows the available options.%s\ -Two styles of help are provided:%s\ -1. Full help is available when you are ready to enter a%s\ -command argument (e.g. 'show ?') and describes each possible%s\ -argument.%s\ -2. Partial help is provided when an abbreviated argument is entered%s\ - and you want to know what arguments match the input%s\ - (e.g. 'show me?'.)%s%s", VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, - VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE); + vty_out(vty, "This VTY provides advanced help features. When you need help,%s" + "anytime at the command line please press '?'.%s%s" + "If nothing matches, the help list will be empty and you must backup%s" + " until entering a '?' shows the available options.%s" + "Two styles of help are provided:%s" + "1. Full help is available when you are ready to enter a%s" + "command argument (e.g. 'show ?') and describes each possible%s" + "argument.%s" + "2. Partial help is provided when an abbreviated argument is entered%s" + " and you want to know what arguments match the input%s" + " (e.g. 'show me?'.)%s%s", + VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, + VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, + VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, VTY_NEWLINE, + VTY_NEWLINE); return CMD_SUCCESS; } +enum { + ATTR_TYPE_GLOBAL = (1 << 0), + ATTR_TYPE_LIB = (1 << 1), + ATTR_TYPE_APP = (1 << 2), +}; + +static void print_attr_list(struct vty *vty, unsigned int attr_mask) +{ + const char *desc; + unsigned int i; + bool found; + char flag; + + if (attr_mask & ATTR_TYPE_GLOBAL) { + vty_out(vty, " Global attributes:%s", VTY_NEWLINE); + + for (i = 0; i < ARRAY_SIZE(cmd_attr_desc) - 1; i++) { + flag = cmd_attr_get_flag(cmd_attr_desc[i].value); + desc = cmd_attr_desc[i].str; + + /* Skip attributes without flags */ + if (flag != '.') + vty_out(vty, " %c %s%s", flag, desc, VTY_NEWLINE); + } + } + + if (attr_mask & ATTR_TYPE_LIB) { + vty_out(vty, " Library specific attributes:%s", VTY_NEWLINE); + + for (i = 0, found = false; i < _OSMO_CORE_LIB_ATTR_COUNT; i++) { + if ((desc = cmd_lib_attr_desc[i]) == NULL) + continue; + found = true; + + flag = cmd_lib_attr_letters[i]; + if (flag == '\0') + flag = '.'; + + vty_out(vty, " %c %s%s", flag, desc, VTY_NEWLINE); + } + + if (!found) + vty_out(vty, " (no attributes)%s", VTY_NEWLINE); + } + + if (attr_mask & ATTR_TYPE_APP) { + vty_out(vty, " Application specific attributes:%s", VTY_NEWLINE); + + for (i = 0, found = false; i < VTY_CMD_USR_ATTR_NUM; i++) { + if ((desc = host.app_info->usr_attr_desc[i]) == NULL) + continue; + found = true; + + flag = host.app_info->usr_attr_letters[i]; + if (flag == '\0') + flag = '.'; + + vty_out(vty, " %c %s%s", flag, desc, VTY_NEWLINE); + } + + if (!found) + vty_out(vty, " (no attributes)%s", VTY_NEWLINE); + } +} + +gDEFUN(show_vty_attr_all, show_vty_attr_all_cmd, + "show vty-attributes", + SHOW_STR "List of VTY attributes\n") +{ + print_attr_list(vty, 0xff); + return CMD_SUCCESS; +} + +gDEFUN(show_vty_attr, show_vty_attr_cmd, + "show vty-attributes (application|library|global)", + SHOW_STR "List of VTY attributes\n" + "Application specific attributes only\n" + "Library specific attributes only\n" + "Global attributes only\n") +{ + unsigned int attr_mask = 0; + + if (argv[0][0] == 'g') /* global */ + attr_mask |= ATTR_TYPE_GLOBAL; + else if (argv[0][0] == 'l') /* library */ + attr_mask |= ATTR_TYPE_LIB; + else if (argv[0][0] == 'a') /* application */ + attr_mask |= ATTR_TYPE_APP; + + print_attr_list(vty, attr_mask); + return CMD_SUCCESS; +} + +/* Compose flag bit-mask for all commands within the given node */ +static unsigned int node_flag_mask(const struct cmd_node *cnode, bool expert_mode) +{ + unsigned int flag_mask = 0x00; + unsigned int f, i; + + for (f = 0; f < VTY_CMD_USR_ATTR_NUM; f++) { + for (i = 0; i < vector_active(cnode->cmd_vector); i++) { + const struct cmd_element *cmd; + char flag_letter; + + if ((cmd = vector_slot(cnode->cmd_vector, i)) == NULL) + continue; + if (cmd->attr & CMD_ATTR_DEPRECATED) + continue; + if (!expert_mode && (cmd->attr & CMD_ATTR_HIDDEN)) + continue; + if (~cmd->usrattr & ((unsigned)1 << f)) + continue; + + if (cmd->attr & CMD_ATTR_LIB_COMMAND) + flag_letter = cmd_lib_attr_letters[f]; + else + flag_letter = host.app_info->usr_attr_letters[f]; + + if (flag_letter == '\0') + continue; + + flag_mask |= (1 << f); + break; + } + } + + return flag_mask; +} + +/* Compose global flag char-mask for the given command (e.g. "!" or "@") */ +static const char *cmd_gflag_mask(const struct cmd_element *cmd) +{ + static char char_mask[8 + 1]; + char *ptr = &char_mask[0]; + + /* Mutually exclusive global attributes */ + if (cmd->attr & CMD_ATTR_HIDDEN) + *(ptr++) = cmd_attr_get_flag(CMD_ATTR_HIDDEN); + else if (cmd->attr & CMD_ATTR_IMMEDIATE) + *(ptr++) = cmd_attr_get_flag(CMD_ATTR_IMMEDIATE); + else if (cmd->attr & CMD_ATTR_NODE_EXIT) + *(ptr++) = cmd_attr_get_flag(CMD_ATTR_NODE_EXIT); + else + *(ptr++) = '.'; + + *ptr = '\0'; + + return char_mask; +} + +/* Compose app / lib flag char-mask for the given command (e.g. ".F.OB..") */ +static const char *cmd_flag_mask(const struct cmd_element *cmd, + unsigned int flag_mask) +{ + static char char_mask[VTY_CMD_USR_ATTR_NUM + 1]; + char *ptr = &char_mask[0]; + char flag_letter; + unsigned int f; + + for (f = 0; f < VTY_CMD_USR_ATTR_NUM; f++) { + if (~flag_mask & ((unsigned)1 << f)) + continue; + if (~cmd->usrattr & ((unsigned)1 << f)) { + *(ptr++) = '.'; + continue; + } + + if (cmd->attr & CMD_ATTR_LIB_COMMAND) + flag_letter = cmd_lib_attr_letters[f]; + else + flag_letter = host.app_info->usr_attr_letters[f]; + + *(ptr++) = flag_letter ? flag_letter : '.'; + } + + *ptr = '\0'; + + return char_mask; +} + /* Help display function for all node. */ -gDEFUN(config_list, config_list_cmd, "list", "Print command list\n") +gDEFUN(config_list, config_list_cmd, + "list [with-flags]", + "Print command list\n" + "Also print the VTY attribute flags\n") { unsigned int i; struct cmd_node *cnode = vector_slot(cmdvec, vty->node); + unsigned int flag_mask = 0x00; struct cmd_element *cmd; - for (i = 0; i < vector_active(cnode->cmd_vector); i++) - if ((cmd = vector_slot(cnode->cmd_vector, i)) != NULL - && !(cmd->attr & (CMD_ATTR_DEPRECATED | CMD_ATTR_HIDDEN))) + if (argc > 0) + flag_mask = node_flag_mask(cnode, vty->expert_mode); + + for (i = 0; i < vector_active(cnode->cmd_vector); i++) { + if ((cmd = vector_slot(cnode->cmd_vector, i)) == NULL) + continue; + if (cmd->attr & CMD_ATTR_DEPRECATED) + continue; + if (!vty->expert_mode && (cmd->attr & CMD_ATTR_HIDDEN)) + continue; + if (!argc) vty_out(vty, " %s%s", cmd->string, VTY_NEWLINE); + else { + vty_out(vty, " %s %s %s%s", + cmd_gflag_mask(cmd), + cmd_flag_mask(cmd, flag_mask), + cmd->string, VTY_NEWLINE); + } + } + return CMD_SUCCESS; } @@ -3302,16 +3857,7 @@ DEFUN(config_terminal_length, config_terminal_length_cmd, "Set number of lines on a screen\n" "Number of lines on screen (0 for no pausing)\n") { - int lines; - char *endptr = NULL; - - lines = strtol(argv[0], &endptr, 10); - if (lines < 0 || lines > 512 || *endptr != '\0') { - vty_out(vty, "length is malformed%s", VTY_NEWLINE); - return CMD_WARNING; - } - vty->lines = lines; - + vty->lines = atoi(argv[0]); return CMD_SUCCESS; } @@ -3330,16 +3876,7 @@ DEFUN(service_terminal_length, service_terminal_length_cmd, "System wide terminal length configuration\n" "Number of lines of VTY (0 means no line control)\n") { - int lines; - char *endptr = NULL; - - lines = strtol(argv[0], &endptr, 10); - if (lines < 0 || lines > 512 || *endptr != '\0') { - vty_out(vty, "length is malformed%s", VTY_NEWLINE); - return CMD_WARNING; - } - host.lines = lines; - + host.lines = atoi(argv[0]); return CMD_SUCCESS; } @@ -3760,7 +4297,12 @@ DEFUN(no_banner_motd, /* Set config filename. Called from vty.c */ void host_config_set(const char *filename) { - host.config = talloc_strdup(tall_vty_cmd_ctx, filename); + osmo_talloc_replace_string(tall_vty_cmd_ctx, &host.config, filename); +} + +const char *host_config_file(void) +{ + return host.config; } /*! Deprecated, now happens implicitly when calling install_node(). @@ -3780,29 +4322,34 @@ void vty_install_default(int node) /*! Install common commands like 'exit' and 'list'. */ static void install_basic_node_commands(int node) { - install_element(node, &config_help_cmd); - install_element(node, &config_list_cmd); + install_lib_element(node, &config_help_cmd); + install_lib_element(node, &config_list_cmd); + + install_lib_element(node, &show_vty_attr_all_cmd); + install_lib_element(node, &show_vty_attr_cmd); - install_element(node, &config_write_terminal_cmd); - install_element(node, &config_write_file_cmd); - install_element(node, &config_write_memory_cmd); - install_element(node, &config_write_cmd); - install_element(node, &show_running_config_cmd); + install_lib_element(node, &config_write_terminal_cmd); + install_lib_element(node, &config_write_file_cmd); + install_lib_element(node, &config_write_memory_cmd); + install_lib_element(node, &config_write_cmd); + install_lib_element(node, &show_running_config_cmd); - install_element(node, &config_exit_cmd); + install_lib_element(node, &config_exit_cmd); if (node >= CONFIG_NODE) { /* It's not a top node. */ - install_element(node, &config_end_cmd); + install_lib_element(node, &config_end_cmd); } } /*! Return true if a node is installed by install_basic_node_commands(), so * that we can avoid repeating them for each and every node during 'show * running-config' */ -static bool vty_command_is_common(struct cmd_element *cmd) +static bool vty_command_is_common(const struct cmd_element *cmd) { if (cmd == &config_help_cmd + || cmd == &show_vty_attr_all_cmd + || cmd == &show_vty_attr_cmd || cmd == &config_list_cmd || cmd == &config_write_terminal_cmd || cmd == &config_write_file_cmd @@ -3880,55 +4427,92 @@ void cmd_init(int terminal) install_node(&config_node, config_write_host); /* Each node's basic commands. */ - install_element(VIEW_NODE, &show_version_cmd); - install_element(VIEW_NODE, &show_online_help_cmd); + install_lib_element(VIEW_NODE, &show_pid_cmd); + install_lib_element(VIEW_NODE, &show_uptime_cmd); + install_lib_element(VIEW_NODE, &show_version_cmd); + install_lib_element(VIEW_NODE, &show_online_help_cmd); if (terminal) { - install_element(VIEW_NODE, &config_list_cmd); - install_element(VIEW_NODE, &config_exit_cmd); - install_element(VIEW_NODE, &config_help_cmd); - install_element(VIEW_NODE, &config_enable_cmd); - install_element(VIEW_NODE, &config_terminal_length_cmd); - install_element(VIEW_NODE, &config_terminal_no_length_cmd); - install_element(VIEW_NODE, &echo_cmd); + install_lib_element(VIEW_NODE, &config_list_cmd); + install_lib_element(VIEW_NODE, &config_exit_cmd); + install_lib_element(VIEW_NODE, &config_help_cmd); + install_lib_element(VIEW_NODE, &show_vty_attr_all_cmd); + install_lib_element(VIEW_NODE, &show_vty_attr_cmd); + install_lib_element(VIEW_NODE, &config_enable_cmd); + install_lib_element(VIEW_NODE, &config_terminal_length_cmd); + install_lib_element(VIEW_NODE, &config_terminal_no_length_cmd); + install_lib_element(VIEW_NODE, &echo_cmd); } if (terminal) { - install_element(ENABLE_NODE, &config_disable_cmd); - install_element(ENABLE_NODE, &config_terminal_cmd); - install_element (ENABLE_NODE, ©_runningconfig_startupconfig_cmd); + install_lib_element(ENABLE_NODE, &config_disable_cmd); + install_lib_element(ENABLE_NODE, &config_terminal_cmd); + install_lib_element(ENABLE_NODE, ©_runningconfig_startupconfig_cmd); + install_lib_element(ENABLE_NODE, &shutdown_cmd); } - install_element (ENABLE_NODE, &show_startup_config_cmd); - install_element(ENABLE_NODE, &show_version_cmd); - install_element(ENABLE_NODE, &show_online_help_cmd); + install_lib_element(ENABLE_NODE, &show_startup_config_cmd); + install_lib_element(ENABLE_NODE, &show_version_cmd); + install_lib_element(ENABLE_NODE, &show_online_help_cmd); if (terminal) { - install_element(ENABLE_NODE, &config_terminal_length_cmd); - install_element(ENABLE_NODE, &config_terminal_no_length_cmd); - install_element(ENABLE_NODE, &echo_cmd); + install_lib_element(ENABLE_NODE, &config_terminal_length_cmd); + install_lib_element(ENABLE_NODE, &config_terminal_no_length_cmd); + install_lib_element(ENABLE_NODE, &echo_cmd); } - install_element(CONFIG_NODE, &hostname_cmd); - install_element(CONFIG_NODE, &no_hostname_cmd); + install_lib_element(CONFIG_NODE, &hostname_cmd); + install_lib_element(CONFIG_NODE, &no_hostname_cmd); if (terminal) { - install_element(CONFIG_NODE, &password_cmd); - install_element(CONFIG_NODE, &password_text_cmd); - install_element(CONFIG_NODE, &enable_password_cmd); - install_element(CONFIG_NODE, &enable_password_text_cmd); - install_element(CONFIG_NODE, &no_enable_password_cmd); + install_lib_element(CONFIG_NODE, &password_cmd); + install_lib_element(CONFIG_NODE, &password_text_cmd); + install_lib_element(CONFIG_NODE, &enable_password_cmd); + install_lib_element(CONFIG_NODE, &enable_password_text_cmd); + install_lib_element(CONFIG_NODE, &no_enable_password_cmd); #ifdef VTY_CRYPT_PW - install_element(CONFIG_NODE, &service_password_encrypt_cmd); - install_element(CONFIG_NODE, &no_service_password_encrypt_cmd); + install_lib_element(CONFIG_NODE, &service_password_encrypt_cmd); + install_lib_element(CONFIG_NODE, &no_service_password_encrypt_cmd); #endif - install_element(CONFIG_NODE, &banner_motd_default_cmd); - install_element(CONFIG_NODE, &banner_motd_file_cmd); - install_element(CONFIG_NODE, &no_banner_motd_cmd); - install_element(CONFIG_NODE, &service_terminal_length_cmd); - install_element(CONFIG_NODE, &no_service_terminal_length_cmd); + install_lib_element(CONFIG_NODE, &banner_motd_default_cmd); + install_lib_element(CONFIG_NODE, &banner_motd_file_cmd); + install_lib_element(CONFIG_NODE, &no_banner_motd_cmd); + install_lib_element(CONFIG_NODE, &service_terminal_length_cmd); + install_lib_element(CONFIG_NODE, &no_service_terminal_length_cmd); } srand(time(NULL)); } +static __attribute__((constructor)) void on_dso_load_starttime(void) +{ + osmo_clock_gettime(CLOCK_MONOTONIC, &starttime); +} + +/* FIXME: execute this section in the unit test instead */ +static __attribute__((constructor)) void on_dso_load(void) +{ + unsigned int i, j; + + /* Check total number of the library specific attributes */ + OSMO_ASSERT(_OSMO_CORE_LIB_ATTR_COUNT < 32); + + /* Check for duplicates in the list of library specific flags */ + for (i = 0; i < _OSMO_CORE_LIB_ATTR_COUNT; i++) { + if (cmd_lib_attr_letters[i] == '\0') + continue; + + /* Some flag characters are reserved for global attributes */ + const char rafc[] = VTY_CMD_ATTR_FLAGS_RESERVED; + for (j = 0; j < ARRAY_SIZE(rafc); j++) + OSMO_ASSERT(cmd_lib_attr_letters[i] != rafc[j]); + + /* Only upper case flag letters are allowed for libraries */ + OSMO_ASSERT(cmd_lib_attr_letters[i] >= 'A'); + OSMO_ASSERT(cmd_lib_attr_letters[i] <= 'Z'); + + for (j = i + 1; j < _OSMO_CORE_LIB_ATTR_COUNT; j++) + OSMO_ASSERT(cmd_lib_attr_letters[i] != cmd_lib_attr_letters[j]); + } +} + /*! @} */ diff --git a/src/vty/cpu_sched_vty.c b/src/vty/cpu_sched_vty.c new file mode 100644 index 00000000..7198f747 --- /dev/null +++ b/src/vty/cpu_sched_vty.c @@ -0,0 +1,682 @@ +/*! \file cpu_sched_vty.c + * Implementation to CPU / Threading / Scheduler properties from VTY configuration. + */ +/* (C) 2020 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * Author: Pau Espin Pedrol <pespin@sysmocom.de> + * + * All Rights Reserved + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * SPDX-License-Identifier: GPLv2+ + */ + +#define _GNU_SOURCE + +#include "config.h" + +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <limits.h> +#include <unistd.h> +#include <sched.h> +#include <ctype.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <fcntl.h> +#include <dirent.h> +#include <pthread.h> +#include <inttypes.h> + +#include <osmocom/vty/vty.h> +#include <osmocom/vty/command.h> +#include <osmocom/vty/tdef_vty.h> +#include <osmocom/core/tdef.h> +#include <osmocom/core/fsm.h> +#include <osmocom/core/linuxlist.h> + +/*! \addtogroup Tdef_VTY + * + * CPU Scheduling related VTY API. + * + * @{ + * \file cpu_sched_vty.c + */ + +enum sched_vty_thread_id { + SCHED_VTY_THREAD_SELF, + SCHED_VTY_THREAD_ALL, + SCHED_VTY_THREAD_ID, + SCHED_VTY_THREAD_NAME, + SCHED_VTY_THREAD_UNKNOWN, +}; + +struct cpu_affinity_it { + struct llist_head entry; + enum sched_vty_thread_id tid_type; + char bufname[64]; + cpu_set_t *cpuset; + size_t cpuset_size; + bool delay; +}; + +struct sched_vty_opts { + void *tall_ctx; + int sched_rr_prio; + struct llist_head cpu_affinity_li; + pthread_mutex_t cpu_affinity_li_mutex; +}; + +static struct sched_vty_opts *sched_vty_opts; + +static struct cmd_node sched_node = { + L_CPU_SCHED_NODE, + "%s(config-cpu-sched)# ", + 1, +}; + +/* returns number of configured CPUs in the system, or negative otherwise */ +static int get_num_cpus(void) { + static unsigned int num_cpus = 0; + long ln; + + if (num_cpus) + return num_cpus; + + /* This is expensive (goes across /sys, so let's do it only once. It is + * guaranteed it won't change during process span anyway). */ + ln = sysconf(_SC_NPROCESSORS_CONF); + if (ln < 0) { + LOGP(DLGLOBAL, LOGL_ERROR, "sysconf(_SC_NPROCESSORS_CONF) failed: %s\n", + strerror(errno)); + return -1; + } + num_cpus = (unsigned int) ln; + return num_cpus; +} + +/* Parses string with CPU hex Affinity Mask, with right-most bit being CPU0, and + * fills a cpuset of size cpuset_size. + */ +static int parse_cpu_hex_mask(const char *str, cpu_set_t *cpuset, size_t cpuset_size) +{ + int len = strlen(str); + const char *ptr = str + len - 1; + int cpu = 0; + + /* skip optional '0x' prefix format */ + if (len >= 2 && str[0] == '0' && str[1] == 'x') + str += 2; + CPU_ZERO_S(cpuset_size, cpuset); + + while (ptr >= str) { + char c = *ptr; + uint8_t val; + + if (c >= '0' && c <= '9') { + val = c - '0'; + } else { + c = (char)tolower((int)c); + if (c >= 'a' && c <= 'f') + val = c + (10 - 'a'); + else + return -1; + } + if (val & 0x01) + CPU_SET_S(cpu, cpuset_size, cpuset); + if (val & 0x02) + CPU_SET_S(cpu + 1, cpuset_size, cpuset); + if (val & 0x04) + CPU_SET_S(cpu + 2, cpuset_size, cpuset); + if (val & 0x08) + CPU_SET_S(cpu + 3, cpuset_size, cpuset); + ptr--; + cpu += 4; + } + + return 0; +} + +/* Generates a hexstring in str from cpuset of size cpuset_size */ +static int generate_cpu_hex_mask(char *str, size_t str_buf_size, + cpu_set_t *cpuset, size_t cpuset_size) +{ + char *ptr = str; + int cpu; + bool first_nonzero_found = false; + + /* 2 char per byte, + '0x' prefix + '\0' */ + if (cpuset_size * 2 + 2 + 1 > str_buf_size) + return -1; + + *ptr++ = '0'; + *ptr++ = 'x'; + + for (cpu = cpuset_size*8 - 4; cpu >= 0; cpu -= 4) { + uint8_t val = 0; + + if (CPU_ISSET_S(cpu, cpuset_size, cpuset)) + val |= 0x01; + if (CPU_ISSET_S(cpu + 1, cpuset_size, cpuset)) + val |= 0x02; + if (CPU_ISSET_S(cpu + 2, cpuset_size, cpuset)) + val |= 0x04; + if (CPU_ISSET_S(cpu + 3, cpuset_size, cpuset)) + val |= 0x08; + + if (val < 10) + *ptr = '0' + val; + else + *ptr = ('a' - 10) + val; + if (val) + first_nonzero_found = true; + if (first_nonzero_found) + ptr++; + + } + if (!first_nonzero_found) + *ptr++ = '0'; + *ptr = '\0'; + return 0; +} + +/* Checks whther a thread identified by tid exists and belongs to the running process */ +static bool proc_tid_exists(pid_t tid) +{ + DIR *proc_dir; + struct dirent *entry; + char dirname[100]; + int tid_it; + bool found = false; + + snprintf(dirname, sizeof(dirname), "/proc/%ld/task", (long int)getpid()); + proc_dir = opendir(dirname); + if (!proc_dir) + return false; /*FIXME; print error */ + + while ((entry = readdir(proc_dir))) { + if (entry->d_name[0] == '.') + continue; + tid_it = atoi(entry->d_name); + if (tid_it == tid) { + found = true; + break; + } + } + + closedir(proc_dir); + return found; +} + +/* Checks whther a thread identified by name exists and belongs to the running + * process, and returns its disocevered TID in res_pid. + */ +static bool proc_name_exists(const char *name, pid_t *res_pid) +{ + DIR *proc_dir; + struct dirent *entry; + char path[100]; + char buf[17]; /* 15 + \n + \0 */ + int tid_it; + int fd; + pid_t mypid = getpid(); + bool found = false; + int rc; + + *res_pid = 0; + + snprintf(path, sizeof(path), "/proc/%ld/task", (long int)mypid); + proc_dir = opendir(path); + if (!proc_dir) + return false; + + while ((entry = readdir(proc_dir))) + { + if (entry->d_name[0] == '.') + continue; + + tid_it = atoi(entry->d_name); + snprintf(path, sizeof(path), "/proc/%ld/task/%ld/comm", (long int)mypid, (long int) tid_it); + if ((fd = open(path, O_RDONLY)) == -1) + continue; + rc = read(fd, buf, sizeof(buf) - 1); + if (rc >= 0) { + /* Last may char contain a '\n', get rid of it */ + if (rc > 0 && buf[rc - 1] == '\n') + buf[rc - 1] = '\0'; + else + buf[rc] = '\0'; + if (strcmp(name, buf) == 0) { + *res_pid = tid_it; + found = true; + } + } + close(fd); + + if (found) + break; + } + + closedir(proc_dir); + return found; +} + +/* Parse VTY THREADNAME variable, return its type and fill discovered res_pid if required */ +static enum sched_vty_thread_id procname2pid(pid_t *res_pid, const char *str, bool applynow) +{ + size_t i, len; + bool is_pid = true; + + if (strcmp(str, "all") == 0) { + *res_pid = 0; + return SCHED_VTY_THREAD_ALL; + } + + if (strcmp(str, "self") == 0) { + *res_pid = 0; + return SCHED_VTY_THREAD_SELF; + } + + len = strlen(str); + for (i = 0; i < len; i++) { + if (!isdigit(str[i])) { + is_pid = false; + break; + } + } + if (is_pid) { + int64_t val; + if (osmo_str_to_int64(&val, str, 0, 0, INT64_MAX)) + return SCHED_VTY_THREAD_UNKNOWN; + *res_pid = (pid_t)val; + if (*res_pid != val) + return SCHED_VTY_THREAD_UNKNOWN; + if (!applynow || proc_tid_exists(*res_pid)) + return SCHED_VTY_THREAD_ID; + else + return SCHED_VTY_THREAD_UNKNOWN; + } + + if (len > 15) { + /* Thread names only allow up to 15+1 null chars, see man pthread_setname_np */ + return SCHED_VTY_THREAD_UNKNOWN; + } + + if (applynow) { + if (proc_name_exists(str, res_pid)) + return SCHED_VTY_THREAD_NAME; + else + return SCHED_VTY_THREAD_UNKNOWN; + } else { + /* assume a thread will be named after it */ + *res_pid = 0; + return SCHED_VTY_THREAD_NAME; + } +} + +/* Wrapper for sched_setaffinity applying to single thread or all threads in process based on tid_type. */ +static int my_sched_setaffinity(enum sched_vty_thread_id tid_type, pid_t pid, + cpu_set_t *cpuset, size_t cpuset_size) +{ + DIR *proc_dir; + struct dirent *entry; + char dirname[100]; + char str_mask[1024]; + int tid_it; + int rc = 0; + + if (generate_cpu_hex_mask(str_mask, sizeof(str_mask), cpuset, cpuset_size) < 0) + str_mask[0] = '\0'; + + if (tid_type != SCHED_VTY_THREAD_ALL) { + LOGP(DLGLOBAL, LOGL_NOTICE, "Setting CPU affinity mask for tid %lu to: %s\n", + (unsigned long) pid, str_mask); + + rc = sched_setaffinity(pid, sizeof(cpu_set_t), cpuset); + return rc; + } + + snprintf(dirname, sizeof(dirname), "/proc/%ld/task", (long int)getpid()); + proc_dir = opendir(dirname); + if (!proc_dir) + return -EINVAL; + + while ((entry = readdir(proc_dir))) + { + if (entry->d_name[0] == '.') + continue; + tid_it = atoi(entry->d_name); + LOGP(DLGLOBAL, LOGL_NOTICE, "Setting CPU affinity mask for tid %lu to: %s\n", + (unsigned long) tid_it, str_mask); + + rc = sched_setaffinity(tid_it, sizeof(cpu_set_t), cpuset); + if (rc == -1) + break; + } + + closedir(proc_dir); + return rc; + +} + +DEFUN_ATTR(cfg_sched_cpu_affinity, cfg_sched_cpu_affinity_cmd, + "cpu-affinity (self|all|<0-4294967295>|THREADNAME) CPUHEXMASK [delay]", + "Set CPU affinity mask on a (group of) thread(s)\n" + "Set CPU affinity mask on thread running the VTY\n" + "Set CPU affinity mask on all process' threads\n" + "Set CPU affinity mask on a thread with specified PID\n" + "Set CPU affinity mask on a thread with specified thread name\n" + "CPU affinity mask\n" + "If set, delay applying the affinity mask now and let the app handle it at a later point\n", + CMD_ATTR_IMMEDIATE) +{ + const char* str_who = argv[0]; + const char *str_mask = argv[1]; + bool applynow = (argc != 3); + int rc; + pid_t pid; + enum sched_vty_thread_id tid_type; + struct cpu_affinity_it *it, *it_next; + cpu_set_t *cpuset; + size_t cpuset_size; + + tid_type = procname2pid(&pid, str_who, applynow); + if (tid_type == SCHED_VTY_THREAD_UNKNOWN) { + vty_out(vty, "%% Failed parsing target thread %s%s", + str_who, VTY_NEWLINE); + return CMD_WARNING; + } + + if (tid_type == SCHED_VTY_THREAD_ID && !applynow) { + vty_out(vty, "%% It makes no sense to delay applying cpu-affinity on tid %lu%s", + (unsigned long)pid, VTY_NEWLINE); + return CMD_WARNING; + } + if (tid_type == SCHED_VTY_THREAD_ALL && !applynow) { + vty_out(vty, "%% It makes no sense to delay applying cpu-affinity on all threads%s", + VTY_NEWLINE); + return CMD_WARNING; + } + + cpuset = CPU_ALLOC(get_num_cpus()); + cpuset_size = CPU_ALLOC_SIZE(get_num_cpus()); + if (parse_cpu_hex_mask(str_mask, cpuset, cpuset_size) < 0) { + vty_out(vty, "%% Failed parsing CPU Affinity Mask %s%s", + str_mask, VTY_NEWLINE); + CPU_FREE(cpuset); + return CMD_WARNING; + } + + if (applynow) { + rc = my_sched_setaffinity(tid_type, pid, cpuset, cpuset_size); + if (rc == -1) { + vty_out(vty, "%% Failed setting sched CPU Affinity Mask %s: %s%s", + str_mask, strerror(errno), VTY_NEWLINE); + CPU_FREE(cpuset); + return CMD_WARNING; + } + } + + /* Keep history of cmds applied to be able to rewrite config. If PID was passed + directly it makes no sense to store it since PIDs are temporary */ + if (tid_type == SCHED_VTY_THREAD_SELF || + tid_type == SCHED_VTY_THREAD_ALL || + tid_type == SCHED_VTY_THREAD_NAME) { + pthread_mutex_lock(&sched_vty_opts->cpu_affinity_li_mutex); + + /* Drop previous entries matching, since they will be overwritten */ + llist_for_each_entry_safe(it, it_next, &sched_vty_opts->cpu_affinity_li, entry) { + if (strcmp(it->bufname, str_who) == 0) { + llist_del(&it->entry); + CPU_FREE(it->cpuset); + talloc_free(it); + break; + } + } + it = talloc_zero(sched_vty_opts->tall_ctx, struct cpu_affinity_it); + OSMO_STRLCPY_ARRAY(it->bufname, str_who); + it->tid_type = tid_type; + it->cpuset = cpuset; + it->cpuset_size = cpuset_size; + it->delay = !applynow; + llist_add_tail(&it->entry, &sched_vty_opts->cpu_affinity_li); + + pthread_mutex_unlock(&sched_vty_opts->cpu_affinity_li_mutex); + } else { + /* We don't need cpuset for later, free it: */ + CPU_FREE(cpuset); + } + return CMD_SUCCESS; +} + +static int set_sched_rr(unsigned int prio) +{ + struct sched_param param; + int rc; + memset(¶m, 0, sizeof(param)); + param.sched_priority = prio; + LOGP(DLGLOBAL, LOGL_NOTICE, "Setting SCHED_RR priority %d\n", param.sched_priority); + rc = sched_setscheduler(getpid(), SCHED_RR, ¶m); + if (rc == -1) { + LOGP(DLGLOBAL, LOGL_ERROR, "Setting SCHED_RR priority %d failed: %s\n", + param.sched_priority, strerror(errno)); + return -1; + } + return 0; +} + +DEFUN_ATTR(cfg_sched_policy, cfg_sched_policy_cmd, + "policy rr <1-32>", + "Set the scheduling policy to use for the process\n" + "Use the SCHED_RR real-time scheduling algorithm\n" + "Set the SCHED_RR real-time priority\n", + CMD_ATTR_IMMEDIATE) +{ + sched_vty_opts->sched_rr_prio = atoi(argv[0]); + + if (set_sched_rr(sched_vty_opts->sched_rr_prio) < 0) { + vty_out(vty, "%% Failed setting SCHED_RR priority %d%s", + sched_vty_opts->sched_rr_prio, VTY_NEWLINE); + return CMD_WARNING; + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_sched, + cfg_sched_cmd, + "cpu-sched", "Configure CPU Scheduler related settings") +{ + vty->index = NULL; + vty->node = L_CPU_SCHED_NODE; + + return CMD_SUCCESS; +} + +DEFUN(show_sched_threads, show_sched_threads_cmd, + "show cpu-sched threads", + SHOW_STR + "Show Sched section information\n" + "Show information about running threads)\n") +{ + DIR *proc_dir; + struct dirent *entry; + char path[100]; + char name[17]; + char str_mask[1024]; + int tid_it; + int fd; + pid_t mypid = getpid(); + int rc; + cpu_set_t *cpuset; + size_t cpuset_size; + + vty_out(vty, "Thread list for PID %lu:%s", (unsigned long) mypid, VTY_NEWLINE); + + snprintf(path, sizeof(path), "/proc/%ld/task", (long int)mypid); + proc_dir = opendir(path); + if (!proc_dir) { + vty_out(vty, "%% Failed opening dir%s%s", path, VTY_NEWLINE); + return CMD_WARNING; + } + + while ((entry = readdir(proc_dir))) + { + if (entry->d_name[0] == '.') + continue; + + tid_it = atoi(entry->d_name); + snprintf(path, sizeof(path), "/proc/%ld/task/%ld/comm", (long int)mypid, (long int)tid_it); + if ((fd = open(path, O_RDONLY)) != -1) { + rc = read(fd, name, sizeof(name) - 1); + if (rc >= 0) { + /* Last may char contain a '\n', get rid of it */ + if (rc > 0 && name[rc - 1] == '\n') + name[rc - 1] = '\0'; + else + name[rc] = '\0'; + } + close(fd); + } else { + name[0] = '\0'; + } + + str_mask[0] = '\0'; + cpuset = CPU_ALLOC(get_num_cpus()); + cpuset_size = CPU_ALLOC_SIZE(get_num_cpus()); + CPU_ZERO_S(cpuset_size, cpuset); + if (sched_getaffinity(tid_it, cpuset_size, cpuset) == 0) { + if (generate_cpu_hex_mask(str_mask, sizeof(str_mask), cpuset, cpuset_size) < 0) + str_mask[0] = '\0'; + } + CPU_FREE(cpuset); + + vty_out(vty, " TID: %lu, NAME: '%s', cpu-affinity: %s%s", + (unsigned long) tid_it, name, str_mask, VTY_NEWLINE); + } + + closedir(proc_dir); + return CMD_SUCCESS; +} + +static int config_write_sched(struct vty *vty) +{ + struct cpu_affinity_it *it; + char str_mask[1024]; + + /* Only add the node if there's something to write under it */ + if (sched_vty_opts->sched_rr_prio || !llist_empty(&sched_vty_opts->cpu_affinity_li)) + vty_out(vty, "cpu-sched%s", VTY_NEWLINE); + + if (sched_vty_opts->sched_rr_prio) + vty_out(vty, " policy rr %d%s", sched_vty_opts->sched_rr_prio, VTY_NEWLINE); + + llist_for_each_entry(it, &sched_vty_opts->cpu_affinity_li, entry) { + if (generate_cpu_hex_mask(str_mask, sizeof(str_mask), it->cpuset, it->cpuset_size) < 0) + OSMO_STRLCPY_ARRAY(str_mask, "ERROR"); + vty_out(vty, " cpu-affinity %s %s%s%s", it->bufname, str_mask, + it->delay ? " delay" : "", VTY_NEWLINE); + } + + return CMD_SUCCESS; +} + +/*! Initialize sched VTY nodes + * \param[in] tall_ctx Talloc context to use internally by vty_sched subsystem. + * \return 0 on success, non-zero on error. + */ +int osmo_cpu_sched_vty_init(void *tall_ctx) +{ + OSMO_ASSERT(!sched_vty_opts); /* assert only called once */ + + sched_vty_opts = talloc_zero(tall_ctx, struct sched_vty_opts); + sched_vty_opts->tall_ctx = tall_ctx; + INIT_LLIST_HEAD(&sched_vty_opts->cpu_affinity_li); + pthread_mutex_init(&sched_vty_opts->cpu_affinity_li_mutex, NULL); + + install_lib_element(CONFIG_NODE, &cfg_sched_cmd); + install_node(&sched_node, config_write_sched); + + install_lib_element(L_CPU_SCHED_NODE, &cfg_sched_policy_cmd); + install_lib_element(L_CPU_SCHED_NODE, &cfg_sched_cpu_affinity_cmd); + + install_lib_element_ve(&show_sched_threads_cmd); + + /* Initialize amount of cpus now */ + if (get_num_cpus() < 0) + return -1; + + return 0; +} + +/*! Apply cpu-affinity on calling thread based on VTY configuration + * \return 0 on success, non-zero on error. + */ +int osmo_cpu_sched_vty_apply_localthread(void) +{ + struct cpu_affinity_it *it, *it_match = NULL; + char name[16]; /* 15 + \0 */ + char str_mask[1024]; + bool has_name = false; + int rc = 0; + + /* Assert subsystem was inited and structs are preset */ + if (!sched_vty_opts) { + LOGP(DLGLOBAL, LOGL_FATAL, "Setting cpu-affinity mask impossible: no opts!\n"); + return 0; + } + +#ifdef HAVE_PTHREAD_GETNAME_NP + if (pthread_getname_np(pthread_self(), name, sizeof(name)) == 0) + has_name = true; +#endif + + /* Get latest matching mask for the thread */ + pthread_mutex_lock(&sched_vty_opts->cpu_affinity_li_mutex); + llist_for_each_entry(it, &sched_vty_opts->cpu_affinity_li, entry) { + switch (it->tid_type) { + case SCHED_VTY_THREAD_SELF: + continue; /* self to the VTY thread, not us */ + case SCHED_VTY_THREAD_ALL: + it_match = it; + break; + case SCHED_VTY_THREAD_NAME: + if (!has_name) + continue; + if (strcmp(name, it->bufname) != 0) + continue; + it_match = it; + break; + default: + OSMO_ASSERT(0); + } + } + + if (it_match) { + rc = my_sched_setaffinity(SCHED_VTY_THREAD_SELF, 0, it_match->cpuset, it_match->cpuset_size); + if (rc == -1) { + if (generate_cpu_hex_mask(str_mask, sizeof(str_mask), + it_match->cpuset, it_match->cpuset_size) < 0) + str_mask[0] = '\0'; + LOGP(DLGLOBAL, LOGL_FATAL, "Setting cpu-affinity mask %s failed: %s\n", + str_mask, strerror(errno)); + } + } + pthread_mutex_unlock(&sched_vty_opts->cpu_affinity_li_mutex); + return rc; +} + +/*! @} */ diff --git a/src/vty/fsm_vty.c b/src/vty/fsm_vty.c index 9bde241c..da6038fa 100644 --- a/src/vty/fsm_vty.c +++ b/src/vty/fsm_vty.c @@ -14,16 +14,12 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdlib.h> #include <string.h> -#include "../../config.h" +#include "config.h" #include <osmocom/vty/command.h> #include <osmocom/vty/buffer.h> @@ -51,62 +47,82 @@ extern struct llist_head osmo_g_fsms; /*! Print information about a FSM [class] to the given VTY * \param vty The VTY to which to print + * \param[in] prefix prefix to print at start of each line (typically indenting) * \param[in] fsm The FSM class to print */ -void vty_out_fsm(struct vty *vty, struct osmo_fsm *fsm) +void vty_out_fsm2(struct vty *vty, const char *prefix, struct osmo_fsm *fsm) { unsigned int i; const struct value_string *evt_name; - vty_out(vty, "FSM Name: '%s', Log Subsys: '%s'%s", fsm->name, + vty_out(vty, "%sFSM Name: '%s', Log Subsys: '%s'%s", prefix, fsm->name, log_category_name(fsm->log_subsys), VTY_NEWLINE); /* list the events */ if (fsm->event_names) { for (evt_name = fsm->event_names; evt_name->str != NULL; evt_name++) { - vty_out(vty, " Event %02u (0x%08x): '%s'%s", evt_name->value, - (1 << evt_name->value), evt_name->str, VTY_NEWLINE); + vty_out(vty, "%s Event %02u (0x%08x): '%s'%s", prefix, evt_name->value, + (1U << evt_name->value), evt_name->str, VTY_NEWLINE); } } else - vty_out(vty, " No event names are defined for this FSM! Please fix!%s", VTY_NEWLINE); + vty_out(vty, "%s No event names are defined for this FSM! Please fix!%s", prefix, VTY_NEWLINE); /* list the states */ - vty_out(vty, " Number of States: %u%s", fsm->num_states, VTY_NEWLINE); + vty_out(vty, "%s Number of States: %u%s", prefix, fsm->num_states, VTY_NEWLINE); for (i = 0; i < fsm->num_states; i++) { const struct osmo_fsm_state *state = &fsm->states[i]; - vty_out(vty, " State %-20s InEvtMask: 0x%08x, OutStateMask: 0x%08x%s", + vty_out(vty, "%s State %-20s InEvtMask: 0x%08x, OutStateMask: 0x%08x%s", prefix, state->name, state->in_event_mask, state->out_state_mask, VTY_NEWLINE); } } +/*! Print information about a FSM [class] to the given VTY + * \param vty The VTY to which to print + * \param[in] fsm The FSM class to print + */ +void vty_out_fsm(struct vty *vty, struct osmo_fsm *fsm) +{ + vty_out_fsm2(vty, "", fsm); +} + /*! Print a FSM instance to the given VTY * \param vty The VTY to which to print + * \param[in] prefix prefix to print at start of each line (typically indenting) * \param[in] fsmi The FSM instance to print */ -void vty_out_fsm_inst(struct vty *vty, struct osmo_fsm_inst *fsmi) +void vty_out_fsm_inst2(struct vty *vty, const char *prefix, struct osmo_fsm_inst *fsmi) { struct osmo_fsm_inst *child; - vty_out(vty, "FSM Instance Name: '%s', ID: '%s'%s", + vty_out(vty, "%sFSM Instance Name: '%s', ID: '%s'%s", prefix, fsmi->name, fsmi->id, VTY_NEWLINE); - vty_out(vty, " Log-Level: '%s', State: '%s'%s", + vty_out(vty, "%s Log-Level: '%s', State: '%s'%s", prefix, log_level_str(fsmi->log_level), osmo_fsm_state_name(fsmi->fsm, fsmi->state), VTY_NEWLINE); if (fsmi->T) - vty_out(vty, " Timer: %u%s", fsmi->T, VTY_NEWLINE); + vty_out(vty, "%s Timer: %u%s", prefix, fsmi->T, VTY_NEWLINE); if (fsmi->proc.parent) { - vty_out(vty, " Parent: '%s', Term-Event: '%s'%s", + vty_out(vty, "%s Parent: '%s', Term-Event: '%s'%s", prefix, fsmi->proc.parent->name, osmo_fsm_event_name(fsmi->proc.parent->fsm, fsmi->proc.parent_term_event), VTY_NEWLINE); } llist_for_each_entry(child, &fsmi->proc.children, proc.child) { - vty_out(vty, " Child: '%s'%s", child->name, VTY_NEWLINE); + vty_out(vty, "%s Child: '%s'%s", prefix, child->name, VTY_NEWLINE); } } +/*! Print a FSM instance to the given VTY + * \param vty The VTY to which to print + * \param[in] fsmi The FSM instance to print + */ +void vty_out_fsm_inst(struct vty *vty, struct osmo_fsm_inst *fsmi) +{ + vty_out_fsm_inst2(vty, "", fsmi); +} + #define SH_FSM_STR SHOW_STR "Show information about finite state machines\n" #define SH_FSMI_STR SHOW_STR "Show information about finite state machine instances\n" @@ -196,9 +212,9 @@ void osmo_fsm_vty_add_cmds(void) if (osmo_fsm_vty_cmds_installed) return; - install_element_ve(&show_fsm_cmd); - install_element_ve(&show_fsms_cmd); - install_element_ve(&show_fsm_inst_cmd); - install_element_ve(&show_fsm_insts_cmd); + install_lib_element_ve(&show_fsm_cmd); + install_lib_element_ve(&show_fsms_cmd); + install_lib_element_ve(&show_fsm_inst_cmd); + install_lib_element_ve(&show_fsm_insts_cmd); osmo_fsm_vty_cmds_installed = true; } diff --git a/src/vty/logging_vty.c b/src/vty/logging_vty.c index 88ee330a..678ae686 100644 --- a/src/vty/logging_vty.c +++ b/src/vty/logging_vty.c @@ -15,16 +15,12 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdlib.h> #include <string.h> -#include "../../config.h" +#include "config.h" #include <osmocom/core/talloc.h> #include <osmocom/core/logging.h> @@ -33,6 +29,7 @@ #include <osmocom/core/strrb.h> #include <osmocom/core/loggingrb.h> #include <osmocom/core/gsmtap.h> +#include <osmocom/core/application.h> #include <osmocom/vty/command.h> #include <osmocom/vty/buffer.h> @@ -130,7 +127,7 @@ DEFUN(enable_logging, conn = (struct telnet_connection *) vty->priv; if (conn->dbg) { - vty_out(vty, "Logging already enabled.%s", VTY_NEWLINE); + vty_out(vty, "%% Logging already enabled.%s", VTY_NEWLINE); return CMD_WARNING; } @@ -157,7 +154,7 @@ struct log_target *osmo_log_vty2tgt(struct vty *vty) conn = (struct telnet_connection *) vty->priv; if (!conn->dbg) - vty_out(vty, "Logging was not enabled.%s", VTY_NEWLINE); + vty_out(vty, "%% Logging was not enabled.%s", VTY_NEWLINE); return conn->dbg; } @@ -224,6 +221,22 @@ DEFUN(logging_prnt_ext_timestamp, RET_WITH_UNLOCK(CMD_SUCCESS); } +DEFUN(logging_prnt_tid, + logging_prnt_tid_cmd, + "logging print thread-id (0|1)", + LOGGING_STR "Log output settings\n" + "Configure log message logging Thread ID\n" + "Don't prefix each log message\n" + "Prefix each log message with current Thread ID\n") +{ + struct log_target *tgt; + + ACQUIRE_VTY_LOG_TGT_WITH_LOCK(vty, tgt); + + log_set_print_tid(tgt, atoi(argv[0])); + RET_WITH_UNLOCK(CMD_SUCCESS); +} + DEFUN(logging_prnt_cat, logging_prnt_cat_cmd, "logging print category (0|1)", @@ -337,6 +350,9 @@ static void gen_logging_level_cmd_strs(struct cmd_element *cmd, osmo_talloc_asprintf(tall_log_ctx, cmd_str, ") %s", level_args); osmo_talloc_asprintf(tall_log_ctx, doc_str, "%s", level_strs); + talloc_set_name_const(cmd_str, "vty_log_level_cmd_str"); + talloc_set_name_const(doc_str, "vty_log_level_doc_str"); + cmd->string = cmd_str; cmd->doc = doc_str; } @@ -351,21 +367,25 @@ DEFUN(logging_level, int category = log_parse_category(argv[0]); int level = log_parse_level(argv[1]); - ACQUIRE_VTY_LOG_TGT_WITH_LOCK(vty, tgt); - if (level < 0) { - vty_out(vty, "Invalid level `%s'%s", argv[1], VTY_NEWLINE); - RET_WITH_UNLOCK(CMD_WARNING); + vty_out(vty, "%% Invalid level '%s'%s", argv[1], VTY_NEWLINE); + return CMD_WARNING; } if (category < 0) { - vty_out(vty, "Invalid category `%s'%s", argv[0], VTY_NEWLINE); - RET_WITH_UNLOCK(CMD_WARNING); + vty_out(vty, "%% Invalid category '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; } + ACQUIRE_VTY_LOG_TGT_WITH_LOCK(vty, tgt); + tgt->categories[category].enabled = 1; tgt->categories[category].loglevel = level; +#if !defined(EMBEDDED) + log_cache_update(category, 1, level); +#endif + RET_WITH_UNLOCK(CMD_SUCCESS); } @@ -390,6 +410,9 @@ DEFUN(logging_level_set_all, logging_level_set_all_cmd, cat->enabled = 1; cat->loglevel = level; +#if !defined(EMBEDDED) + log_cache_update(i, 1, level); +#endif } RET_WITH_UNLOCK(CMD_SUCCESS); } @@ -575,7 +598,7 @@ gDEFUN(cfg_description, cfg_description_cmd, char **dptr = vty->index_sub; if (!dptr) { - vty_out(vty, "vty->index_sub == NULL%s", VTY_NEWLINE); + vty_out(vty, "%% vty->index_sub == NULL%s", VTY_NEWLINE); return CMD_WARNING; } @@ -596,7 +619,7 @@ gDEFUN(cfg_no_description, cfg_no_description_cmd, char **dptr = vty->index_sub; if (!dptr) { - vty_out(vty, "vty->index_sub == NULL%s", VTY_NEWLINE); + vty_out(vty, "%% vty->index_sub == NULL%s", VTY_NEWLINE); return CMD_WARNING; } @@ -728,6 +751,64 @@ DEFUN(cfg_no_log_syslog, cfg_no_log_syslog_cmd, } #endif /* HAVE_SYSLOG_H */ +DEFUN(cfg_log_systemd_journal, cfg_log_systemd_journal_cmd, + "log systemd-journal [raw]", + LOG_STR "Logging to systemd-journal\n" + "Offload rendering of the meta information (location, category) to systemd\n") +{ +#ifdef ENABLE_SYSTEMD_LOGGING + struct log_target *tgt; + bool raw = argc > 0; + + log_tgt_mutex_lock(); + tgt = log_target_find(LOG_TGT_TYPE_SYSTEMD, NULL); + if (tgt == NULL) { + tgt = log_target_create_systemd(raw); + if (tgt == NULL) { + vty_out(vty, "%% Unable to create systemd-journal " + "log target%s", VTY_NEWLINE); + RET_WITH_UNLOCK(CMD_WARNING); + } + log_add_target(tgt); + } else if (tgt->sd_journal.raw != raw) { + log_target_systemd_set_raw(tgt, raw); + } + + vty->index = tgt; + vty->node = CFG_LOG_NODE; + + RET_WITH_UNLOCK(CMD_SUCCESS); +#else + vty_out(vty, "%% systemd-journal logging is not available " + "in this build of libosmocore%s", VTY_NEWLINE); + return CMD_WARNING; +#endif /* ENABLE_SYSTEMD_LOGGING */ +} + +DEFUN(cfg_no_log_systemd_journal, cfg_no_log_systemd_journal_cmd, + "no log systemd-journal", + NO_STR LOG_STR "Logging to systemd-journal\n") +{ +#ifdef ENABLE_SYSTEMD_LOGGING + struct log_target *tgt; + + log_tgt_mutex_lock(); + tgt = log_target_find(LOG_TGT_TYPE_SYSTEMD, NULL); + if (!tgt) { + vty_out(vty, "%% No systemd-journal logging active%s", VTY_NEWLINE); + RET_WITH_UNLOCK(CMD_WARNING); + } + + log_target_destroy(tgt); + + RET_WITH_UNLOCK(CMD_SUCCESS); +#else + vty_out(vty, "%% systemd-journal logging is not available " + "in this build of libosmocore%s", VTY_NEWLINE); + return CMD_WARNING; +#endif /* ENABLE_SYSTEMD_LOGGING */ +} + DEFUN(cfg_log_gsmtap, cfg_log_gsmtap_cmd, "log gsmtap [HOSTNAME]", LOG_STR "Logging via GSMTAP\n" @@ -756,9 +837,31 @@ DEFUN(cfg_log_gsmtap, cfg_log_gsmtap_cmd, RET_WITH_UNLOCK(CMD_SUCCESS); } +DEFUN(cfg_no_log_gsmtap, cfg_no_log_gsmtap_cmd, + "no log gsmtap [HOSTNAME]", + NO_STR LOG_STR "Logging via GSMTAP\n" + "Host name to send the GSMTAP logging to (UDP port 4729)\n") +{ + const char *hostname = argc ? argv[0] : "127.0.0.1"; + struct log_target *tgt; + + log_tgt_mutex_lock(); + tgt = log_target_find(LOG_TGT_TYPE_GSMTAP, hostname); + if (tgt == NULL) { + vty_out(vty, "%% Unable to find GSMTAP log target for %s%s", + hostname, VTY_NEWLINE); + RET_WITH_UNLOCK(CMD_WARNING); + } + + log_target_destroy(tgt); + + RET_WITH_UNLOCK(CMD_SUCCESS); +} + DEFUN(cfg_log_stderr, cfg_log_stderr_cmd, - "log stderr", - LOG_STR "Logging via STDERR of the process\n") + "log stderr [blocking-io]", + LOG_STR "Logging via STDERR of the process\n" + "Use blocking, synchronous I/O\n") { struct log_target *tgt; @@ -774,6 +877,11 @@ DEFUN(cfg_log_stderr, cfg_log_stderr_cmd, log_add_target(tgt); } + if (argc > 0 && !strcmp(argv[0], "blocking-io")) + log_target_file_switch_to_stream(tgt); + else + log_target_file_switch_to_wqueue(tgt); + vty->index = tgt; vty->node = CFG_LOG_NODE; @@ -794,13 +902,15 @@ DEFUN(cfg_no_log_stderr, cfg_no_log_stderr_cmd, } log_target_destroy(tgt); + osmo_stderr_target = NULL; RET_WITH_UNLOCK(CMD_SUCCESS); } DEFUN(cfg_log_file, cfg_log_file_cmd, - "log file .FILENAME", - LOG_STR "Logging to text file\n" "Filename\n") + "log file FILENAME [blocking-io]", + LOG_STR "Logging to text file\n" "Filename\n" + "Use blocking, synchronous I/O\n") { const char *fname = argv[0]; struct log_target *tgt; @@ -810,13 +920,18 @@ DEFUN(cfg_log_file, cfg_log_file_cmd, if (!tgt) { tgt = log_target_create_file(fname); if (!tgt) { - vty_out(vty, "%% Unable to create file `%s'%s", + vty_out(vty, "%% Unable to create file '%s'%s", fname, VTY_NEWLINE); RET_WITH_UNLOCK(CMD_WARNING); } log_add_target(tgt); } + if (argc > 1 && !strcmp(argv[1], "blocking-io")) + log_target_file_switch_to_stream(tgt); + else + log_target_file_switch_to_wqueue(tgt); + vty->index = tgt; vty->node = CFG_LOG_NODE; @@ -825,7 +940,7 @@ DEFUN(cfg_log_file, cfg_log_file_cmd, DEFUN(cfg_no_log_file, cfg_no_log_file_cmd, - "no log file .FILENAME", + "no log file FILENAME", NO_STR LOG_STR "Logging to text file\n" "Filename\n") { const char *fname = argv[0]; @@ -834,7 +949,7 @@ DEFUN(cfg_no_log_file, cfg_no_log_file_cmd, log_tgt_mutex_lock(); tgt = log_target_find(LOG_TGT_TYPE_FILE, fname); if (!tgt) { - vty_out(vty, "%% No such log file `%s'%s", + vty_out(vty, "%% No such log file '%s'%s", fname, VTY_NEWLINE); RET_WITH_UNLOCK(CMD_WARNING); } @@ -900,7 +1015,10 @@ static int config_write_log_single(struct vty *vty, struct log_target *tgt) return 1; break; case LOG_TGT_TYPE_STDERR: - vty_out(vty, "log stderr%s", VTY_NEWLINE); + if (tgt->tgt_file.wqueue) + vty_out(vty, "log stderr%s", VTY_NEWLINE); + else + vty_out(vty, "log stderr blocking-io%s", VTY_NEWLINE); break; case LOG_TGT_TYPE_SYSLOG: #ifdef HAVE_SYSLOG_H @@ -911,7 +1029,10 @@ static int config_write_log_single(struct vty *vty, struct log_target *tgt) #endif break; case LOG_TGT_TYPE_FILE: - vty_out(vty, "log file %s%s", tgt->tgt_file.fname, VTY_NEWLINE); + if (tgt->tgt_file.wqueue) + vty_out(vty, "log file %s%s", tgt->tgt_file.fname, VTY_NEWLINE); + else + vty_out(vty, "log file %s blocking-io%s", tgt->tgt_file.fname, VTY_NEWLINE); break; case LOG_TGT_TYPE_STRRB: vty_out(vty, "log alarms %zu%s", @@ -921,6 +1042,11 @@ static int config_write_log_single(struct vty *vty, struct log_target *tgt) vty_out(vty, "log gsmtap %s%s", tgt->tgt_gsmtap.hostname, VTY_NEWLINE); break; + case LOG_TGT_TYPE_SYSTEMD: + vty_out(vty, "log systemd-journal%s%s", + tgt->sd_journal.raw ? " raw" : "", + VTY_NEWLINE); + break; } vty_out(vty, " logging filter all %u%s", @@ -935,6 +1061,8 @@ static int config_write_log_single(struct vty *vty, struct log_target *tgt) tgt->print_category_hex ? 1 : 0, VTY_NEWLINE); vty_out(vty, " logging print category %d%s", tgt->print_category ? 1 : 0, VTY_NEWLINE); + vty_out(vty, " logging print thread-id %d%s", + tgt->print_tid ? 1 : 0, VTY_NEWLINE); if (tgt->print_ext_timestamp) vty_out(vty, " logging print extended-timestamp 1%s", VTY_NEWLINE); else @@ -942,8 +1070,9 @@ static int config_write_log_single(struct vty *vty, struct log_target *tgt) tgt->print_timestamp ? 1 : 0, VTY_NEWLINE); if (tgt->print_level) vty_out(vty, " logging print level 1%s", VTY_NEWLINE); - vty_out(vty, " logging print file %s%s", + vty_out(vty, " logging print file %s%s%s", get_value_string(logging_print_file_args, tgt->print_filename2), + tgt->print_filename_pos == LOG_FILENAME_POS_LINE_END ? " last" : "", VTY_NEWLINE); if (tgt->loglevel) { @@ -998,22 +1127,20 @@ static int config_write_log(struct vty *vty) static int log_deprecated_func(struct cmd_element *cmd, struct vty *vty, int argc, const char *argv[]) { vty_out(vty, "%% Ignoring deprecated '%s'%s", cmd->string, VTY_NEWLINE); - return CMD_WARNING; + return CMD_SUCCESS; /* Otherwise the process would terminate */ } void logging_vty_add_deprecated_subsys(void *ctx, const char *name) { struct cmd_element *cmd = talloc_zero(ctx, struct cmd_element); OSMO_ASSERT(cmd); - cmd->string = talloc_asprintf(cmd, "logging level %s (debug|info|notice|error|fatal)", - name); - printf("%s\n", cmd->string); + cmd->string = talloc_asprintf(cmd, "logging level %s " LOG_LEVEL_ARGS, name); cmd->func = log_deprecated_func; cmd->doc = LEVEL_STR "Deprecated Category\n"; cmd->attr = CMD_ATTR_DEPRECATED; - install_element(CFG_LOG_NODE, cmd); + install_lib_element(CFG_LOG_NODE, cmd); } /* logp (<categories>) (debug|...|fatal) .LOGMESSAGE*/ @@ -1025,6 +1152,23 @@ DEFUN(vty_logp, int category = log_parse_category(argv[0]); int level = log_parse_level(argv[1]); char *str = argv_concat(argv, argc, 2); + + if (level < 0) { + vty_out(vty, "%% Invalid level '%s'%s", argv[1], VTY_NEWLINE); + return CMD_WARNING; + } + + if (category < 0) { + vty_out(vty, "%% Invalid category '%s'%s", argv[0], VTY_NEWLINE); + return CMD_WARNING; + } + + /* Properly handle library specific sub-systems */ + if ((unsigned int) category >= osmo_log_info->num_cat_user) { + category -= osmo_log_info->num_cat_user - 1; + category *= -1; + } + LOGP(category, level, "%s\n", str); return CMD_SUCCESS; } @@ -1050,26 +1194,30 @@ static void gen_vty_logp_cmd_strs(struct cmd_element *cmd) osmo_talloc_asprintf(tall_log_ctx, doc_str, "Arbitrary message to log on given category and log level\n"); + talloc_set_name_const(cmd_str, "vty_logp_cmd_str"); + talloc_set_name_const(doc_str, "vty_logp_doc_str"); + cmd->string = cmd_str; cmd->doc = doc_str; } /*! Register logging related commands to the VTY. Call this once from * your application if you want to support those commands. */ -void logging_vty_add_cmds() +void logging_vty_add_cmds(void) { - install_element_ve(&enable_logging_cmd); - install_element_ve(&disable_logging_cmd); - install_element_ve(&logging_fltr_all_cmd); - install_element_ve(&logging_use_clr_cmd); - install_element_ve(&logging_prnt_timestamp_cmd); - install_element_ve(&logging_prnt_ext_timestamp_cmd); - install_element_ve(&logging_prnt_cat_cmd); - install_element_ve(&logging_prnt_cat_hex_cmd); - install_element_ve(&logging_prnt_level_cmd); - install_element_ve(&logging_prnt_file_cmd); - install_element_ve(&logging_set_category_mask_cmd); - install_element_ve(&logging_set_category_mask_old_cmd); + install_lib_element_ve(&enable_logging_cmd); + install_lib_element_ve(&disable_logging_cmd); + install_lib_element_ve(&logging_fltr_all_cmd); + install_lib_element_ve(&logging_use_clr_cmd); + install_lib_element_ve(&logging_prnt_timestamp_cmd); + install_lib_element_ve(&logging_prnt_ext_timestamp_cmd); + install_lib_element_ve(&logging_prnt_tid_cmd); + install_lib_element_ve(&logging_prnt_cat_cmd); + install_lib_element_ve(&logging_prnt_cat_hex_cmd); + install_lib_element_ve(&logging_prnt_level_cmd); + install_lib_element_ve(&logging_prnt_file_cmd); + install_lib_element_ve(&logging_set_category_mask_cmd); + install_lib_element_ve(&logging_set_category_mask_old_cmd); /* logging level (<categories>) (debug|...|fatal) */ gen_logging_level_cmd_strs(&logging_level_cmd, @@ -1079,47 +1227,51 @@ void logging_vty_add_cmds() gen_logging_level_cmd_strs(&deprecated_logging_level_everything_cmd, "everything", EVERYTHING_STR); - install_element_ve(&logging_level_cmd); - install_element_ve(&logging_level_set_all_cmd); - install_element_ve(&logging_level_force_all_cmd); - install_element_ve(&no_logging_level_force_all_cmd); - install_element_ve(&deprecated_logging_level_everything_cmd); - install_element_ve(&deprecated_logging_level_all_cmd); - install_element_ve(&deprecated_logging_level_all_everything_cmd); + install_lib_element_ve(&logging_level_cmd); + install_lib_element_ve(&logging_level_set_all_cmd); + install_lib_element_ve(&logging_level_force_all_cmd); + install_lib_element_ve(&no_logging_level_force_all_cmd); + install_lib_element_ve(&deprecated_logging_level_everything_cmd); + install_lib_element_ve(&deprecated_logging_level_all_cmd); + install_lib_element_ve(&deprecated_logging_level_all_everything_cmd); gen_vty_logp_cmd_strs(&vty_logp_cmd); - install_element_ve(&vty_logp_cmd); + install_lib_element_ve(&vty_logp_cmd); - install_element_ve(&show_logging_vty_cmd); - install_element_ve(&show_alarms_cmd); + install_lib_element_ve(&show_logging_vty_cmd); + install_lib_element_ve(&show_alarms_cmd); install_node(&cfg_log_node, config_write_log); - install_element(CFG_LOG_NODE, &logging_fltr_all_cmd); - install_element(CFG_LOG_NODE, &logging_use_clr_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_timestamp_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_ext_timestamp_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_cat_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_cat_hex_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_level_cmd); - install_element(CFG_LOG_NODE, &logging_prnt_file_cmd); - install_element(CFG_LOG_NODE, &logging_level_cmd); - install_element(CFG_LOG_NODE, &logging_level_set_all_cmd); - install_element(CFG_LOG_NODE, &logging_level_force_all_cmd); - install_element(CFG_LOG_NODE, &no_logging_level_force_all_cmd); - install_element(CFG_LOG_NODE, &deprecated_logging_level_everything_cmd); - install_element(CFG_LOG_NODE, &deprecated_logging_level_all_cmd); - install_element(CFG_LOG_NODE, &deprecated_logging_level_all_everything_cmd); - - install_element(CONFIG_NODE, &cfg_log_stderr_cmd); - install_element(CONFIG_NODE, &cfg_no_log_stderr_cmd); - install_element(CONFIG_NODE, &cfg_log_file_cmd); - install_element(CONFIG_NODE, &cfg_no_log_file_cmd); - install_element(CONFIG_NODE, &cfg_log_alarms_cmd); - install_element(CONFIG_NODE, &cfg_no_log_alarms_cmd); + install_lib_element(CFG_LOG_NODE, &logging_fltr_all_cmd); + install_lib_element(CFG_LOG_NODE, &logging_use_clr_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_timestamp_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_ext_timestamp_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_tid_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_cat_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_cat_hex_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_level_cmd); + install_lib_element(CFG_LOG_NODE, &logging_prnt_file_cmd); + install_lib_element(CFG_LOG_NODE, &logging_level_cmd); + install_lib_element(CFG_LOG_NODE, &logging_level_set_all_cmd); + install_lib_element(CFG_LOG_NODE, &logging_level_force_all_cmd); + install_lib_element(CFG_LOG_NODE, &no_logging_level_force_all_cmd); + install_lib_element(CFG_LOG_NODE, &deprecated_logging_level_everything_cmd); + install_lib_element(CFG_LOG_NODE, &deprecated_logging_level_all_cmd); + install_lib_element(CFG_LOG_NODE, &deprecated_logging_level_all_everything_cmd); + + install_lib_element(CONFIG_NODE, &cfg_log_stderr_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_stderr_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_file_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_file_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_alarms_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_alarms_cmd); #ifdef HAVE_SYSLOG_H - install_element(CONFIG_NODE, &cfg_log_syslog_cmd); - install_element(CONFIG_NODE, &cfg_log_syslog_local_cmd); - install_element(CONFIG_NODE, &cfg_no_log_syslog_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_syslog_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_syslog_local_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_syslog_cmd); #endif - install_element(CONFIG_NODE, &cfg_log_gsmtap_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_systemd_journal_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_systemd_journal_cmd); + install_lib_element(CONFIG_NODE, &cfg_log_gsmtap_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_log_gsmtap_cmd); } diff --git a/src/vty/stats_vty.c b/src/vty/stats_vty.c index 296519c3..f940018f 100644 --- a/src/vty/stats_vty.c +++ b/src/vty/stats_vty.c @@ -1,5 +1,5 @@ /* - * (C) 2009-2010 by Harald Welte <laforge@gnumonks.org> + * (C) 2009-2022 by Harald Welte <laforge@gnumonks.org> * (C) 2009-2014 by Holger Hans Peter Freyther * (C) 2015 by sysmocom - s.f.m.c. GmbH * All Rights Reserved @@ -16,16 +16,12 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdlib.h> #include <string.h> -#include "../../config.h" +#include "config.h" #include <osmocom/vty/command.h> #include <osmocom/vty/buffer.h> @@ -37,11 +33,15 @@ #include <osmocom/core/stats.h> #include <osmocom/core/counter.h> #include <osmocom/core/rate_ctr.h> +#include <osmocom/core/stats_tcp.h> #define CFG_STATS_STR "Configure stats sub-system\n" #define CFG_REPORTER_STR "Configure a stats reporter\n" #define SHOW_STATS_STR "Show statistical values\n" +#define SKIP_ZERO_STR "Skip items with total count zero\n" + +#define STATS_STR "Stats related commands\n" /*! \file stats_vty.c * VTY interface for statsd / statistic items @@ -245,15 +245,42 @@ DEFUN(cfg_stats_reporter_disable, cfg_stats_reporter_disable_cmd, return CMD_SUCCESS; } +DEFUN(cfg_stats_reporter_flush_period, cfg_stats_reporter_flush_period_cmd, + "flush-period <0-65535>", + CFG_STATS_STR "Send all stats even if they have not changed (i.e. force the flush)" + "every N-th reporting interval. Set to 0 to disable regular flush (default).\n" + "0 to disable regular flush (default), 1 to flush every time, 2 to flush every 2nd time, etc\n") +{ + int rc; + unsigned int period = atoi(argv[0]); + struct osmo_stats_reporter *srep = osmo_stats_vty2srep(vty); + OSMO_ASSERT(srep); + + rc = osmo_stats_reporter_set_flush_period(srep, period); + if (rc < 0) { + vty_out(vty, "%% Unable to set force flush period: %s%s", + strerror(-rc), VTY_NEWLINE); + return CMD_WARNING; + } + + return CMD_SUCCESS; +} + DEFUN(cfg_stats_reporter_statsd, cfg_stats_reporter_statsd_cmd, - "stats reporter statsd", - CFG_STATS_STR CFG_REPORTER_STR "Report to a STATSD server\n") + "stats reporter statsd [NAME]", + CFG_STATS_STR CFG_REPORTER_STR + "Report to a STATSD server\n" + "Name of the reporter\n") { struct osmo_stats_reporter *srep; + const char *name = NULL; - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_STATSD, NULL); + if (argc > 0) + name = argv[0]; + + srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_STATSD, name); if (!srep) { - srep = osmo_stats_reporter_create_statsd(NULL); + srep = osmo_stats_reporter_create_statsd(name); if (!srep) { vty_out(vty, "%% Unable to create statsd reporter%s", VTY_NEWLINE); @@ -269,34 +296,22 @@ DEFUN(cfg_stats_reporter_statsd, cfg_stats_reporter_statsd_cmd, return CMD_SUCCESS; } -DEFUN(cfg_stats_interval, cfg_stats_interval_cmd, - "stats interval <1-65535>", - CFG_STATS_STR "Set the reporting interval\n" - "Interval in seconds\n") -{ - int rc; - int interval = atoi(argv[0]); - rc = osmo_stats_set_interval(interval); - if (rc < 0) { - vty_out(vty, "%% Unable to set interval: %s%s", - strerror(-rc), VTY_NEWLINE); - return CMD_WARNING; - } - - return CMD_SUCCESS; -} - - DEFUN(cfg_no_stats_reporter_statsd, cfg_no_stats_reporter_statsd_cmd, - "no stats reporter statsd", - NO_STR CFG_STATS_STR CFG_REPORTER_STR "Report to a STATSD server\n") + "no stats reporter statsd [NAME]", + NO_STR CFG_STATS_STR CFG_REPORTER_STR + "Report to a STATSD server\n" + "Name of the reporter\n") { struct osmo_stats_reporter *srep; + const char *name = NULL; + + if (argc > 0) + name = argv[0]; - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_STATSD, NULL); + srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_STATSD, name); if (!srep) { - vty_out(vty, "%% No statsd logging active%s", - VTY_NEWLINE); + vty_out(vty, "%% There is no such statsd reporter with name '%s'%s", + name ? name : "", VTY_NEWLINE); return CMD_WARNING; } @@ -306,14 +321,20 @@ DEFUN(cfg_no_stats_reporter_statsd, cfg_no_stats_reporter_statsd_cmd, } DEFUN(cfg_stats_reporter_log, cfg_stats_reporter_log_cmd, - "stats reporter log", - CFG_STATS_STR CFG_REPORTER_STR "Report to the logger\n") + "stats reporter log [NAME]", + CFG_STATS_STR CFG_REPORTER_STR + "Report to the logger\n" + "Name of the reporter\n") { struct osmo_stats_reporter *srep; + const char *name = NULL; + + if (argc > 0) + name = argv[0]; - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_LOG, NULL); + srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_LOG, name); if (!srep) { - srep = osmo_stats_reporter_create_log(NULL); + srep = osmo_stats_reporter_create_log(name); if (!srep) { vty_out(vty, "%% Unable to create log reporter%s", VTY_NEWLINE); @@ -330,15 +351,21 @@ DEFUN(cfg_stats_reporter_log, cfg_stats_reporter_log_cmd, } DEFUN(cfg_no_stats_reporter_log, cfg_no_stats_reporter_log_cmd, - "no stats reporter log", - NO_STR CFG_STATS_STR CFG_REPORTER_STR "Report to the logger\n") + "no stats reporter log [NAME]", + NO_STR CFG_STATS_STR CFG_REPORTER_STR + "Report to the logger\n" + "Name of the reporter\n") { struct osmo_stats_reporter *srep; + const char *name = NULL; + + if (argc > 0) + name = argv[0]; - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_LOG, NULL); + srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_LOG, name); if (!srep) { - vty_out(vty, "%% No log reporting active%s", - VTY_NEWLINE); + vty_out(vty, "%% There is no such log reporter with name '%s'%s", + name ? name : "", VTY_NEWLINE); return CMD_WARNING; } @@ -347,27 +374,77 @@ DEFUN(cfg_no_stats_reporter_log, cfg_no_stats_reporter_log_cmd, return CMD_SUCCESS; } +DEFUN(cfg_stats_interval, cfg_stats_interval_cmd, + "stats interval <0-65535>", + CFG_STATS_STR "Set the reporting interval\n" + "Interval in seconds (0 disables the reporting interval)\n") +{ + int rc; + int interval = atoi(argv[0]); + rc = osmo_stats_set_interval(interval); + if (rc < 0) { + vty_out(vty, "%% Unable to set interval: %s%s", + strerror(-rc), VTY_NEWLINE); + return CMD_WARNING; + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_tcp_stats_interval, cfg_tcp_stats_interval_cmd, + "stats-tcp interval <0-65535>", + CFG_STATS_STR "Set the tcp socket stats polling interval\n" + "Interval in seconds (0 disables the polling interval)\n") +{ + int rc; + int interval = atoi(argv[0]); + rc = osmo_stats_tcp_set_interval(interval); + if (rc < 0) { + vty_out(vty, "%% Unable to set interval: %s%s", + strerror(-rc), VTY_NEWLINE); + return CMD_WARNING; + } + + return CMD_SUCCESS; +} + +DEFUN(cfg_tcp_stats_batch_size, cfg_tcp_stats_batch_size_cmd, + "stats-tcp batch-size <1-65535>", + CFG_STATS_STR "Set the number of tcp sockets that are processed per stats polling interval\n" + "Number of sockets per interval\n") +{ + osmo_tcp_stats_config->batch_size = atoi(argv[0]); + return CMD_SUCCESS; +} + DEFUN(show_stats, show_stats_cmd, - "show stats", - SHOW_STR SHOW_STATS_STR) + "show stats [skip-zero]", + SHOW_STR SHOW_STATS_STR SKIP_ZERO_STR) { - vty_out_statistics_full(vty, ""); + bool skip_zero = false; + if (argc > 0) + skip_zero = true; + + vty_out_statistics_full2(vty, "", skip_zero); return CMD_SUCCESS; } DEFUN(show_stats_level, show_stats_level_cmd, - "show stats level (global|peer|subscriber)", + "show stats level (global|peer|subscriber) [skip-zero]", SHOW_STR SHOW_STATS_STR "Set the maximum group level\n" "Show global groups only\n" "Show global and network peer related groups\n" - "Show global, peer, and subscriber groups\n") + "Show global, peer, and subscriber groups\n" SKIP_ZERO_STR) { int level = get_string_value(stats_class_strs, argv[0]); - vty_out_statistics_partial(vty, "", level); + bool skip_zero = false; + if (argc > 1) + skip_zero = true; + vty_out_statistics_partial2(vty, "", level, skip_zero); return CMD_SUCCESS; } @@ -393,13 +470,6 @@ static int asciidoc_handle_counter(struct osmo_counter *counter, void *sctx_) static void asciidoc_counter_generate(struct vty *vty) { - if (osmo_counters_count() == 0) - { - vty_out(vty, "// there are no ungrouped osmo_counters%s", - VTY_NEWLINE); - return; - } - vty_out(vty, "// ungrouped osmo_counters%s", VTY_NEWLINE); vty_out(vty, ".ungrouped osmo counters%s", VTY_NEWLINE); vty_out(vty, "[options=\"header\"]%s", VTY_NEWLINE); @@ -460,10 +530,11 @@ static int asciidoc_osmo_stat_item_handler( { struct vty *vty = sctx_; - char *name = osmo_asciidoc_escape(item->desc->name); - char *description = osmo_asciidoc_escape(item->desc->description); + const struct osmo_stat_item_desc *desc = osmo_stat_item_get_desc(item); + char *name = osmo_asciidoc_escape(desc->name); + char *description = osmo_asciidoc_escape(desc->description); char *group_name_prefix = osmo_asciidoc_escape(statg->desc->group_name_prefix); - char *unit = osmo_asciidoc_escape(item->desc->unit); + char *unit = osmo_asciidoc_escape(desc->unit); /* | Name | Reference | Description | Unit | */ vty_out(vty, "| %s | <<%s_%s>> | %s | %s%s", @@ -520,48 +591,97 @@ DEFUN(show_stats_asciidoc_table, vty_out(vty, "// generating tables for rate_ctr_group%s", VTY_NEWLINE); rate_ctr_for_each_group(asciidoc_rate_ctr_group_handler, vty); - vty_out(vty, "== Osmo Stat Items%s%s", VTY_NEWLINE, VTY_NEWLINE); + vty_out(vty, "=== Osmo Stat Items%s%s", VTY_NEWLINE, VTY_NEWLINE); vty_out(vty, "// generating tables for osmo_stat_items%s", VTY_NEWLINE); osmo_stat_item_for_each_group(asciidoc_osmo_stat_item_group_handler, vty); - vty_out(vty, "== Osmo Counters%s%s", VTY_NEWLINE, VTY_NEWLINE); - vty_out(vty, "// generating tables for osmo_counters%s", VTY_NEWLINE); - asciidoc_counter_generate(vty); + if (osmo_counters_count() == 0) + { + vty_out(vty, "// there are no ungrouped osmo_counters%s", + VTY_NEWLINE); + } else { + vty_out(vty, "=== Osmo Counters%s%s", VTY_NEWLINE, VTY_NEWLINE); + vty_out(vty, "// generating tables for osmo_counters%s", VTY_NEWLINE); + asciidoc_counter_generate(vty); + } return CMD_SUCCESS; } +struct rctr_vty_ctx { + struct vty *vty; + bool skip_zero; +}; + static int rate_ctr_group_handler(struct rate_ctr_group *ctrg, void *sctx_) { - struct vty *vty = sctx_; - vty_out(vty, "%s %u:%s", ctrg->desc->group_description, ctrg->idx, VTY_NEWLINE); - vty_out_rate_ctr_group_fmt(vty, "%25n: %10c (%S/s %M/m %H/h %D/d) %d", ctrg); + struct rctr_vty_ctx *sctx = sctx_; + struct vty *vty = sctx->vty; + vty_out(vty, "%s %u", ctrg->desc->group_description, ctrg->idx); + if (ctrg->name != NULL) + vty_out(vty, " (%s)", ctrg->name); + vty_out(vty, ":%s", VTY_NEWLINE); + vty_out_rate_ctr_group_fmt2(vty, "%25n: %10c (%S/s %M/m %H/h %D/d) %d", ctrg, sctx->skip_zero); return 0; } DEFUN(show_rate_counters, show_rate_counters_cmd, - "show rate-counters", - SHOW_STR "Show all rate counters\n") + "show rate-counters [skip-zero]", + SHOW_STR "Show all rate counters\n" SKIP_ZERO_STR) +{ + struct rctr_vty_ctx rctx = { .vty = vty, .skip_zero = false }; + if (argc > 0) + rctx.skip_zero = true; + rate_ctr_for_each_group(rate_ctr_group_handler, &rctx); + return CMD_SUCCESS; +} + +DEFUN(stats_report, + stats_report_cmd, + "stats report", + STATS_STR "Manurally trigger reporting of stats\n") { - rate_ctr_for_each_group(rate_ctr_group_handler, vty); + osmo_stats_report(); + return CMD_SUCCESS; +} + +static int reset_rate_ctr_group_handler(struct rate_ctr_group *ctrg, void *sctx_) +{ + rate_ctr_group_reset(ctrg); + return 0; +} + +DEFUN(stats_reset, + stats_reset_cmd, + "stats reset", + STATS_STR "Reset all rate counter stats\n") +{ + rate_ctr_for_each_group(reset_rate_ctr_group_handler, NULL); return CMD_SUCCESS; } static int config_write_stats_reporter(struct vty *vty, struct osmo_stats_reporter *srep) { - if (srep == NULL) - return 0; + const char *type = NULL; switch (srep->type) { case OSMO_STATS_REPORTER_STATSD: - vty_out(vty, "stats reporter statsd%s", VTY_NEWLINE); + type = "statsd"; break; case OSMO_STATS_REPORTER_LOG: - vty_out(vty, "stats reporter log%s", VTY_NEWLINE); + type = "log"; break; + default: + /* don't try to save unknown stats reporters to the VTY. Imagine some + * application registering a new application specific stats reporter that + * this VTY code knows nothing about! */ + return 0; } - vty_out(vty, " disable%s", VTY_NEWLINE); + vty_out(vty, "stats reporter %s", type); + if (srep->name != NULL) + vty_out(vty, " %s", srep->name); + vty_out(vty, "%s", VTY_NEWLINE); if (srep->have_net_config) { if (srep->dest_addr_str) @@ -589,8 +709,14 @@ static int config_write_stats_reporter(struct vty *vty, struct osmo_stats_report else vty_out(vty, " no prefix%s", VTY_NEWLINE); + if (srep->flush_period > 0) + vty_out(vty, " flush-period %d%s", + srep->flush_period, VTY_NEWLINE); + if (srep->enabled) vty_out(vty, " enable%s", VTY_NEWLINE); + else + vty_out(vty, " disable%s", VTY_NEWLINE); return 1; } @@ -599,13 +725,15 @@ static int config_write_stats(struct vty *vty) { struct osmo_stats_reporter *srep; - /* TODO: loop through all reporters */ - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_STATSD, NULL); - config_write_stats_reporter(vty, srep); - srep = osmo_stats_reporter_find(OSMO_STATS_REPORTER_LOG, NULL); - config_write_stats_reporter(vty, srep); - vty_out(vty, "stats interval %d%s", osmo_stats_config->interval, VTY_NEWLINE); + if (osmo_tcp_stats_config->interval != TCP_STATS_DEFAULT_INTERVAL) + vty_out(vty, "stats-tcp interval %d%s", osmo_tcp_stats_config->interval, VTY_NEWLINE); + if (osmo_tcp_stats_config->batch_size != TCP_STATS_DEFAULT_BATCH_SIZE) + vty_out(vty, "stats-tcp batch-size %d%s", osmo_tcp_stats_config->batch_size, VTY_NEWLINE); + + /* Loop through all reporters */ + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) + config_write_stats_reporter(vty, srep); return 1; } @@ -614,31 +742,37 @@ static int config_write_stats(struct vty *vty) * Call this once during your application initialization if you would * like to have stats VTY commands enabled. */ -void osmo_stats_vty_add_cmds() +void osmo_stats_vty_add_cmds(void) { - install_element_ve(&show_stats_cmd); - install_element_ve(&show_stats_level_cmd); + install_lib_element_ve(&show_stats_cmd); + install_lib_element_ve(&show_stats_level_cmd); - install_element(CONFIG_NODE, &cfg_stats_reporter_statsd_cmd); - install_element(CONFIG_NODE, &cfg_no_stats_reporter_statsd_cmd); - install_element(CONFIG_NODE, &cfg_stats_reporter_log_cmd); - install_element(CONFIG_NODE, &cfg_no_stats_reporter_log_cmd); - install_element(CONFIG_NODE, &cfg_stats_interval_cmd); + install_lib_element(CONFIG_NODE, &cfg_stats_reporter_statsd_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_stats_reporter_statsd_cmd); + install_lib_element(CONFIG_NODE, &cfg_stats_reporter_log_cmd); + install_lib_element(CONFIG_NODE, &cfg_no_stats_reporter_log_cmd); + install_lib_element(CONFIG_NODE, &cfg_stats_interval_cmd); + install_lib_element(CONFIG_NODE, &cfg_tcp_stats_interval_cmd); + install_lib_element(CONFIG_NODE, &cfg_tcp_stats_batch_size_cmd); install_node(&cfg_stats_node, config_write_stats); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_local_ip_cmd); - install_element(CFG_STATS_NODE, &cfg_no_stats_reporter_local_ip_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_remote_ip_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_remote_port_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_mtu_cmd); - install_element(CFG_STATS_NODE, &cfg_no_stats_reporter_mtu_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_prefix_cmd); - install_element(CFG_STATS_NODE, &cfg_no_stats_reporter_prefix_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_level_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_enable_cmd); - install_element(CFG_STATS_NODE, &cfg_stats_reporter_disable_cmd); - - install_element_ve(&show_stats_asciidoc_table_cmd); - install_element_ve(&show_rate_counters_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_local_ip_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_no_stats_reporter_local_ip_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_remote_ip_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_remote_port_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_mtu_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_no_stats_reporter_mtu_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_prefix_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_no_stats_reporter_prefix_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_level_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_enable_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_disable_cmd); + install_lib_element(CFG_STATS_NODE, &cfg_stats_reporter_flush_period_cmd); + + install_lib_element_ve(&show_stats_asciidoc_table_cmd); + install_lib_element_ve(&show_rate_counters_cmd); + + install_lib_element(ENABLE_NODE, &stats_report_cmd); + install_lib_element(ENABLE_NODE, &stats_reset_cmd); } diff --git a/src/vty/talloc_ctx_vty.c b/src/vty/talloc_ctx_vty.c index 16cb7635..ea8ebe70 100644 --- a/src/vty/talloc_ctx_vty.c +++ b/src/vty/talloc_ctx_vty.c @@ -17,17 +17,13 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdio.h> #include <regex.h> #include <string.h> -#include <talloc.h> +#include <osmocom/core/talloc.h> #include <osmocom/vty/command.h> #include <osmocom/vty/vty.h> @@ -151,6 +147,8 @@ static void talloc_ctx_walk(const char *ctx, const char *depth, /* Determine a context for report */ if (!strncmp(ctx, "app", 3)) talloc_ctx = host.app_info->tall_ctx; + else if (!strcmp(ctx, "global")) + talloc_ctx = OTC_GLOBAL; else if (!strncmp(ctx, "all", 3)) talloc_ctx = NULL; @@ -167,11 +165,12 @@ static void talloc_ctx_walk(const char *ctx, const char *depth, } #define BASE_CMD_STR \ - "show talloc-context (application|all) (full|brief|DEPTH)" + "show talloc-context (application|global|all) (full|brief|DEPTH)" #define BASE_CMD_DESCR \ SHOW_STR "Show talloc memory hierarchy\n" \ "Application's context\n" \ + "Global context (OTC_GLOBAL)\n" \ "All contexts, if NULL-context tracking is enabled\n" \ "Display a full talloc memory hierarchy\n" \ "Display a brief talloc memory hierarchy\n" \ @@ -247,7 +246,7 @@ DEFUN(show_talloc_ctx_tree, show_talloc_ctx_tree_cmd, */ void osmo_talloc_vty_add_cmds(void) { - install_element_ve(&show_talloc_ctx_cmd); - install_element_ve(&show_talloc_ctx_tree_cmd); - install_element_ve(&show_talloc_ctx_filter_cmd); + install_lib_element_ve(&show_talloc_ctx_cmd); + install_lib_element_ve(&show_talloc_ctx_tree_cmd); + install_lib_element_ve(&show_talloc_ctx_filter_cmd); } diff --git a/src/vty/tdef_vty.c b/src/vty/tdef_vty.c index 4549a61c..bd209ae0 100644 --- a/src/vty/tdef_vty.c +++ b/src/vty/tdef_vty.c @@ -50,10 +50,9 @@ */ struct osmo_tdef *osmo_tdef_vty_parse_T_arg(struct vty *vty, struct osmo_tdef *tdefs, const char *T_str) { - long l; + int l; int T; struct osmo_tdef *t; - char *endptr; const char *T_nr_str; int sign = 1; @@ -77,9 +76,7 @@ struct osmo_tdef *osmo_tdef_vty_parse_T_arg(struct vty *vty, struct osmo_tdef *t return NULL; } - errno = 0; - l = strtol(T_nr_str, &endptr, 10); - if (errno || *endptr || l > INT_MAX || l < 0) { + if (osmo_str_to_int(&l, T_nr_str, 10, 0, INT_MAX)) { vty_out(vty, "%% Invalid T timer argument (should be 'T1234' or 'X1234'): '%s'%s", T_str, VTY_NEWLINE); return NULL; } @@ -245,7 +242,7 @@ void osmo_tdef_vty_out_all(struct vty *vty, struct osmo_tdef *tdefs, const char /*! Write current timer configuration arguments to the vty. Skip all entries that reflect their default value. * The passed prefix string must contain both necessary indent and the VTY command the specific implementation is using. - * See tdef_vty_test_config_subnode.c and tdef_vty_test_dynamic.c for examples. + * See tdef_vty_config_subnode_test.c and tdef_vty_dynamic_test.c for examples. * \param[in] vty VTY context. * \param[in] tdefs Array of timers to print, ended with a fully zero-initialized entry. * \param[in] prefix_fmt Arbitrary string to start each line with, with variable printf like arguments. @@ -361,10 +358,10 @@ static char *timer_doc_string(const char *prefix, const char *suffix) * The given timer definitions group is stored in a global pointer, so this can be done only once per main() scope. * It would also be possible to have distinct timer groups on separate VTY subnodes, with a "manual" implementation, but * not with this API. - * \param[in] parent_node VTY node id at which to add the timer group commands, e.g. CONFIG_NODE. + * \param[in] parent_cfg_node VTY node at which to add the timer configuration commands, e.g. CONFIG_NODE. * \param[in] groups Global timer groups definition. */ -void osmo_tdef_vty_groups_init(enum node_type parent_node, struct osmo_tdef_group *groups) +void osmo_tdef_vty_groups_init(unsigned int parent_cfg_node, struct osmo_tdef_group *groups) { struct osmo_tdef_group *g; OSMO_ASSERT(!global_tdef_groups); @@ -379,8 +376,8 @@ void osmo_tdef_vty_groups_init(enum node_type parent_node, struct osmo_tdef_grou cfg_timer_cmd.string = timer_command_string("timer", OSMO_TDEF_VTY_ARG_SET_OPTIONAL); cfg_timer_cmd.doc = timer_doc_string("Configure or show timers\n", OSMO_TDEF_VTY_DOC_SET); - install_element_ve(&show_timer_cmd); - install_element(parent_node, &cfg_timer_cmd); + install_lib_element_ve(&show_timer_cmd); + install_lib_element(parent_cfg_node, &cfg_timer_cmd); } /*! Write the global osmo_tdef_group configuration to VTY, as previously passed to osmo_tdef_vty_groups_init(). diff --git a/src/vty/telnet_interface.c b/src/vty/telnet_interface.c index 9aa36fe4..8fa5dbff 100644 --- a/src/vty/telnet_interface.c +++ b/src/vty/telnet_interface.c @@ -14,10 +14,6 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <sys/socket.h> @@ -46,7 +42,7 @@ * process in order to enable interactive command-line introspection, * interaction and configuration. * - * You typically call \ref telnet_init or \ref telnet_init_dynif once + * You typically call telnet_init_default once * from your application code to enable this. */ @@ -64,26 +60,14 @@ static struct osmo_fd server_socket = { .priv_nr = 0, }; -/*! Initialize telnet based VTY interface listening to 127.0.0.1 - * \param[in] tall_ctx \ref talloc context - * \param[in] priv private data to be passed to callback - * \param[in] port TCP port number to bind to - */ -int telnet_init(void *tall_ctx, void *priv, int port) -{ - return telnet_init_dynif(tall_ctx, priv, "127.0.0.1", port); -} - -/*! Initialize telnet based VTY interface - * \param[in] tall_ctx \ref talloc context - * \param[in] priv private data to be passed to callback - * \param[in] ip IP to listen to ('::1' for localhost, '::0' for all, ...) - * \param[in] port TCP port number to bind to - */ -int telnet_init_dynif(void *tall_ctx, void *priv, const char *ip, int port) +/* Helper for deprecating telnet_init_dynif(), which previously held this code */ +static int _telnet_init_dynif(void *tall_ctx, void *priv, const char *ip, int port) { int rc; + if (port < 0) + return -EINVAL; + tall_telnet_ctx = talloc_named_const(tall_ctx, 1, "telnet_connection"); @@ -98,22 +82,45 @@ int telnet_init_dynif(void *tall_ctx, void *priv, const char *ip, int port) if (rc < 0) { LOGP(DLGLOBAL, LOGL_ERROR, "Cannot bind telnet at %s %d\n", ip, port); - return -1; + return rc; } LOGP(DLGLOBAL, LOGL_NOTICE, "Available via telnet %s %d\n", ip, port); return 0; } +/*! Initialize telnet based VTY interface listening to 127.0.0.1 + * \param[in] tall_ctx \ref talloc context + * \param[in] priv private data to be passed to callback + * \param[in] port TCP port number to bind to + * \deprecated use telnet_init_default() instead + */ +int telnet_init(void *tall_ctx, void *priv, int port) +{ + return _telnet_init_dynif(tall_ctx, priv, "127.0.0.1", port); +} + +/*! Initialize telnet based VTY interface + * \param[in] tall_ctx \ref talloc context + * \param[in] priv private data to be passed to callback + * \param[in] ip IP to listen to ('::1' for localhost, '::0' for all, ...) + * \param[in] port TCP port number to bind to + * \deprecated use telnet_init_default() instead + */ +int telnet_init_dynif(void *tall_ctx, void *priv, const char *ip, int port) +{ + return _telnet_init_dynif(tall_ctx, priv, ip, port); +} + /*! Initializes telnet based VTY interface using the configured bind addr/port. * \param[in] tall_ctx \ref talloc context * \param[in] priv private data to be passed to callback - * \param[in] default_port TCP port number to bind to if not explicitely configured + * \param[in] default_port TCP port number to bind to if not explicitly configured */ int telnet_init_default(void *tall_ctx, void *priv, int default_port) { - return telnet_init_dynif(tall_ctx, priv, vty_get_bind_addr(), - vty_get_bind_port(default_port)); + return _telnet_init_dynif(tall_ctx, priv, vty_get_bind_addr(), + vty_get_bind_port(default_port)); } @@ -256,7 +263,7 @@ void vty_event(enum event event, int sock, struct vty *vty) } /*! Close all telnet connections and release the telnet socket */ -void telnet_exit(void) +void telnet_exit(void) { struct telnet_connection *tc, *tc2; diff --git a/src/vty/utils.c b/src/vty/utils.c index 0358d9bd..0f5a34e4 100644 --- a/src/vty/utils.c +++ b/src/vty/utils.c @@ -18,13 +18,10 @@ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - * */ #include <stdint.h> +#include <stdbool.h> #include <inttypes.h> #include <string.h> #include <ctype.h> @@ -48,6 +45,7 @@ struct vty_out_context { struct vty *vty; const char *prefix; int max_level; + bool skip_zero; }; static int rate_ctr_handler( @@ -57,6 +55,9 @@ static int rate_ctr_handler( struct vty_out_context *vctx = vctx_; struct vty *vty = vctx->vty; + if (vctx->skip_zero && ctr->current == 0) + return 0; + vty_out(vty, " %s%s: %8" PRIu64 " " "(%" PRIu64 "/s %" PRIu64 "/m %" PRIu64 "/h %" PRIu64 "/d)%s", vctx->prefix, desc->description, ctr->current, @@ -73,17 +74,24 @@ static int rate_ctr_handler( * \param[in] vty The VTY to which it should be printed * \param[in] prefix Any additional log prefix ahead of each line * \param[in] ctrg Rate counter group to be printed + * \param[in] skip_zero Skip all zero-valued counters */ -void vty_out_rate_ctr_group(struct vty *vty, const char *prefix, - struct rate_ctr_group *ctrg) +void vty_out_rate_ctr_group2(struct vty *vty, const char *prefix, + struct rate_ctr_group *ctrg, bool skip_zero) { - struct vty_out_context vctx = {vty, prefix}; + struct vty_out_context vctx = {vty, prefix, 0, skip_zero}; vty_out(vty, "%s%s:%s", prefix, ctrg->desc->group_description, VTY_NEWLINE); rate_ctr_for_each_counter(ctrg, rate_ctr_handler, &vctx); } +void vty_out_rate_ctr_group(struct vty *vty, const char *prefix, + struct rate_ctr_group *ctrg) +{ + vty_out_rate_ctr_group2(vty, prefix, ctrg, false); +} + static char * pad_append_str(char *s, const char *a, int minwidth) { @@ -107,7 +115,12 @@ static int rate_ctr_handler_fmt( struct vty_out_context *vctx = vctx_; struct vty *vty = vctx->vty; const char *fmt = vctx->prefix; - char *s = talloc_strdup(vty, ""); + char *s; + + if (vctx->skip_zero && ctr->current == 0) + return 0; + + s = talloc_strdup(vty, ""); OSMO_ASSERT(s); while (*fmt) { @@ -209,14 +222,20 @@ static int rate_ctr_handler_fmt( * \param[in] vty The VTY to which it should be printed * \param[in] ctrg Rate counter group to be printed * \param[in] fmt A format which may contain the above directives. + * \param[in] skip_zero Skip all zero-valued counters */ -void vty_out_rate_ctr_group_fmt(struct vty *vty, const char *fmt, - struct rate_ctr_group *ctrg) +void vty_out_rate_ctr_group_fmt2(struct vty *vty, const char *fmt, + struct rate_ctr_group *ctrg, bool skip_zero) { - struct vty_out_context vctx = {vty, fmt}; + struct vty_out_context vctx = {vty, fmt, 0, skip_zero}; rate_ctr_for_each_counter(ctrg, rate_ctr_handler_fmt, &vctx); } +void vty_out_rate_ctr_group_fmt(struct vty *vty, const char *fmt, + struct rate_ctr_group *ctrg) +{ + vty_out_rate_ctr_group_fmt2(vty, fmt, ctrg, false); +} static int rate_ctr_group_handler(struct rate_ctr_group *ctrg, void *vctx_) { struct vty_out_context *vctx = vctx_; @@ -225,12 +244,10 @@ static int rate_ctr_group_handler(struct rate_ctr_group *ctrg, void *vctx_) if (ctrg->desc->class_id > vctx->max_level) return 0; - if (ctrg->idx) - vty_out(vty, "%s%s (%d):%s", vctx->prefix, - ctrg->desc->group_description, ctrg->idx, VTY_NEWLINE); - else - vty_out(vty, "%s%s:%s", vctx->prefix, - ctrg->desc->group_description, VTY_NEWLINE); + vty_out(vty, "%s%s (%d)", vctx->prefix, ctrg->desc->group_description, ctrg->idx); + if (ctrg->name) + vty_out(vty, "('%s')", ctrg->name); + vty_out(vty, ":%s", VTY_NEWLINE); rate_ctr_for_each_counter(ctrg, rate_ctr_handler, vctx); @@ -249,14 +266,15 @@ static int osmo_stat_item_handler( { struct vty_out_context *vctx = vctx_; struct vty *vty = vctx->vty; - const char *unit = - item->desc->unit != OSMO_STAT_ITEM_NO_UNIT ? - item->desc->unit : ""; + const struct osmo_stat_item_desc *desc = osmo_stat_item_get_desc(item); + int32_t value = osmo_stat_item_get_last(item); + const char *unit = (desc->unit != OSMO_STAT_ITEM_NO_UNIT) ? desc->unit : ""; + + if (vctx->skip_zero && value == 0) + return 0; vty_out(vty, " %s%s: %8" PRIi32 " %s%s", - vctx->prefix, item->desc->description, - osmo_stat_item_get_last(item), - unit, VTY_NEWLINE); + vctx->prefix, desc->description, value, unit, VTY_NEWLINE); return 0; } @@ -265,17 +283,24 @@ static int osmo_stat_item_handler( * \param[in] vty The VTY to which it should be printed * \param[in] prefix Any additional log prefix ahead of each line * \param[in] statg Stat item group to be printed + * \param[in] skip_zero Skip all zero-valued counters */ -void vty_out_stat_item_group(struct vty *vty, const char *prefix, - struct osmo_stat_item_group *statg) +void vty_out_stat_item_group2(struct vty *vty, const char *prefix, + struct osmo_stat_item_group *statg, bool skip_zero) { - struct vty_out_context vctx = {vty, prefix}; + struct vty_out_context vctx = {vty, prefix, 0, skip_zero}; vty_out(vty, "%s%s:%s", prefix, statg->desc->group_description, VTY_NEWLINE); osmo_stat_item_for_each_item(statg, osmo_stat_item_handler, &vctx); } +void vty_out_stat_item_group(struct vty *vty, const char *prefix, + struct osmo_stat_item_group *statg) +{ + return vty_out_stat_item_group2(vty, prefix, statg, false); +} + static int osmo_stat_item_group_handler(struct osmo_stat_item_group *statg, void *vctx_) { struct vty_out_context *vctx = vctx_; @@ -284,13 +309,10 @@ static int osmo_stat_item_group_handler(struct osmo_stat_item_group *statg, void if (statg->desc->class_id > vctx->max_level) return 0; - if (statg->idx) - vty_out(vty, "%s%s (%d):%s", vctx->prefix, - statg->desc->group_description, statg->idx, - VTY_NEWLINE); - else - vty_out(vty, "%s%s:%s", vctx->prefix, - statg->desc->group_description, VTY_NEWLINE); + vty_out(vty, "%s%s (%d)", vctx->prefix, statg->desc->group_description, statg->idx); + if (statg->name) + vty_out(vty, "('%s')", statg->name); + vty_out(vty, ":%s", VTY_NEWLINE); osmo_stat_item_for_each_item(statg, osmo_stat_item_handler, vctx); @@ -308,21 +330,22 @@ static int handle_counter(struct osmo_counter *counter, void *vctx_) struct vty_out_context *vctx = vctx_; struct vty *vty = vctx->vty; const char *description = counter->description; + unsigned long value = osmo_counter_get(counter); + + if (vctx->skip_zero && value == 0) + return 0; if (!counter->description) description = counter->name; - vty_out(vty, " %s%s: %8lu%s", - vctx->prefix, description, - osmo_counter_get(counter), VTY_NEWLINE); + vty_out(vty, " %s%s: %8lu%s", vctx->prefix, description, value, VTY_NEWLINE); return 0; } -void vty_out_statistics_partial(struct vty *vty, const char *prefix, - int max_level) +void vty_out_statistics_partial2(struct vty *vty, const char *prefix, int max_level, bool skip_zero) { - struct vty_out_context vctx = {vty, prefix, max_level}; + struct vty_out_context vctx = {vty, prefix, max_level, skip_zero}; vty_out(vty, "%sUngrouped counters:%s", prefix, VTY_NEWLINE); osmo_counters_for_each(handle_counter, &vctx); @@ -330,9 +353,19 @@ void vty_out_statistics_partial(struct vty *vty, const char *prefix, osmo_stat_item_for_each_group(osmo_stat_item_group_handler, &vctx); } +void vty_out_statistics_partial(struct vty *vty, const char *prefix, int max_level) +{ + return vty_out_statistics_partial2(vty, prefix, max_level, false); +} + +void vty_out_statistics_full2(struct vty *vty, const char *prefix, bool skip_zero) +{ + vty_out_statistics_partial2(vty, prefix, INT_MAX, skip_zero); +} + void vty_out_statistics_full(struct vty *vty, const char *prefix) { - vty_out_statistics_partial(vty, prefix, INT_MAX); + vty_out_statistics_full2(vty, prefix, false); } /*! Generate a VTY command string from value_string */ @@ -382,6 +415,7 @@ char *vty_cmd_string_from_valstr(void *ctx, const struct value_string *vals, if (ret < 0) goto err; OSMO_SNPRINTF_RET(ret, rem, offset, len); + (void)len; /* suppress warnings about len being set but not used */ err: str[size-1] = '\0'; return str; diff --git a/src/vty/vector.c b/src/vty/vector.c index f9e5ec3e..34b161d8 100644 --- a/src/vty/vector.c +++ b/src/vty/vector.c @@ -16,11 +16,6 @@ * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNU Zebra; see the file COPYING. If not, write to the Free - * Software Foundation, Inc., 51 Franklin Street, Fifth Floor, - * Boston, MA 02110-1301, USA. */ #include <stdlib.h> diff --git a/src/vty/vty.c b/src/vty/vty.c index babe0ef6..3a549b43 100644 --- a/src/vty/vty.c +++ b/src/vty/vty.c @@ -66,6 +66,8 @@ #include <osmocom/vty/command.h> #include <osmocom/vty/buffer.h> #include <osmocom/core/talloc.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/utils.h> #ifndef MAXPATHLEN #define MAXPATHLEN 4096 @@ -128,6 +130,7 @@ struct vty *vty_new(void) goto out_obuf; new->max = VTY_BUFSIZ; + new->fd = -1; return new; @@ -205,6 +208,12 @@ static void vty_auth(struct vty *vty) } } +void vty_flush(struct vty *vty) +{ + if (vty->obuf) + buffer_flush_all(vty->obuf, vty->fd); +} + /*! Close a given vty interface. */ void vty_close(struct vty *vty) { @@ -230,8 +239,8 @@ void vty_close(struct vty *vty) /* Unset vector. */ vector_unset(vtyvec, vty->fd); - /* Close socket. */ - if (vty->fd > 0) { + /* Close socket (ignore standard I/O streams). */ + if (vty->fd > 2) { close(vty->fd); vty->fd = -1; } @@ -330,6 +339,25 @@ int vty_out_newline(struct vty *vty) return 0; } +/*! calculates the time difference of a give timespec to the current time + * and prints in a human readable format (days, hours, minutes, seconds). + */ +int vty_out_uptime(struct vty *vty, const struct timespec *starttime) +{ + struct timespec now; + struct timespec uptime; + + osmo_clock_gettime(CLOCK_MONOTONIC, &now); + timespecsub(&now, starttime, &uptime); + + int d = uptime.tv_sec / (3600 * 24); + int h = uptime.tv_sec / 3600 % 24; + int m = uptime.tv_sec / 60 % 60; + int s = uptime.tv_sec % 60; + + return vty_out(vty, "%dd %dh %dm %ds", d, h, m, s); +} + /*! return the current index of a given VTY */ void *vty_current_index(struct vty *vty) { @@ -457,6 +485,7 @@ static int vty_command(struct vty *vty) static const char telnet_backward_char = 0x08; static const char telnet_space_char = ' '; +static const char telnet_escape_char = 0x1B; /* Basic function to write buffer to vty. */ static void vty_write(struct vty *vty, const char *buf, size_t nbytes) @@ -856,6 +885,19 @@ static void vty_down_level(struct vty *vty) vty->cp = 0; } +/* When '^L' is typed, clear all lines above the current one. */ +static void vty_clear_screen(struct vty *vty) +{ + vty_out(vty, "%c%s%c%s", + telnet_escape_char, + "[2J", /* Erase Screen */ + telnet_escape_char, + "[H" /* Cursor Home */ + ); + vty_prompt(vty); + vty_redraw_line(vty); +} + /* When '^Z' is received from vty, move down to the enable mode. */ static void vty_end_config(struct vty *vty) { @@ -1118,7 +1160,7 @@ static void vty_describe_command(struct vty *vty) int ret; vector vline; vector describe; - unsigned int i, width, desc_width; + unsigned int i, cmd_width, desc_width; struct desc *desc, *desc_cr = NULL; vline = cmd_make_strvec(vty->buf); @@ -1142,19 +1184,17 @@ static void vty_describe_command(struct vty *vty) vty_prompt(vty); vty_redraw_line(vty); return; - break; case CMD_ERR_NO_MATCH: cmd_free_strvec(vline); vty_out(vty, "%% There is no matched command.%s", VTY_NEWLINE); vty_prompt(vty); vty_redraw_line(vty); return; - break; } /* Get width of command string. */ - width = 0; - for (i = 0; i < vector_active(describe); i++) + cmd_width = 0; + for (i = 0; i < vector_active(describe); i++) { if ((desc = vector_slot(describe, i)) != NULL) { unsigned int len; @@ -1165,15 +1205,16 @@ static void vty_describe_command(struct vty *vty) if (desc->cmd[0] == '.') len--; - if (width < len) - width = len; + if (cmd_width < len) + cmd_width = len; } + } /* Get width of description string. */ - desc_width = vty->width - (width + 6); + desc_width = vty->width - (cmd_width + 6); /* Print out description. */ - for (i = 0; i < vector_active(describe); i++) + for (i = 0; i < vector_active(describe); i++) { if ((desc = vector_slot(describe, i)) != NULL) { if (desc->cmd[0] == '\0') continue; @@ -1189,19 +1230,20 @@ static void vty_describe_command(struct vty *vty) '.' ? desc->cmd + 1 : desc->cmd, VTY_NEWLINE); else if (desc_width >= strlen(desc->str)) - vty_out(vty, " %-*s %s%s", width, + vty_out(vty, " %-*s %s%s", cmd_width, desc->cmd[0] == '.' ? desc->cmd + 1 : desc->cmd, desc->str, VTY_NEWLINE); else - vty_describe_fold(vty, width, desc_width, desc); + vty_describe_fold(vty, cmd_width, desc_width, desc); #if 0 - vty_out(vty, " %-*s %s%s", width + vty_out(vty, " %-*s %s%s", cmd_width desc->cmd[0] == '.' ? desc->cmd + 1 : desc->cmd, desc->str ? desc->str : "", VTY_NEWLINE); #endif /* 0 */ } + } if ((desc = desc_cr)) { if (!desc->str) @@ -1209,11 +1251,11 @@ static void vty_describe_command(struct vty *vty) desc->cmd[0] == '.' ? desc->cmd + 1 : desc->cmd, VTY_NEWLINE); else if (desc_width >= strlen(desc->str)) - vty_out(vty, " %-*s %s%s", width, + vty_out(vty, " %-*s %s%s", cmd_width, desc->cmd[0] == '.' ? desc->cmd + 1 : desc->cmd, desc->str, VTY_NEWLINE); else - vty_describe_fold(vty, width, desc_width, desc); + vty_describe_fold(vty, cmd_width, desc_width, desc); } cmd_free_strvec(vline); @@ -1400,6 +1442,9 @@ int vty_read(struct vty *vty) case CONTROL('K'): vty_kill_line(vty); break; + case CONTROL('L'): + vty_clear_screen(vty); + break; case CONTROL('N'): vty_next_line(vty); break; @@ -1460,19 +1505,27 @@ int vty_read(struct vty *vty) return 0; } -/* Read up configuration file */ -static int -vty_read_file(FILE *confp, void *priv) +/* Read up configuration from a file stream */ +/*! Read up VTY configuration from a file stream + * \param[in] confp file pointer of the stream for the configuration file + * \param[in] priv private data to be passed to \ref vty_read_file + * \returns Zero on success, non-zero on error + */ +int vty_read_config_filep(FILE *confp, void *priv) { int ret; struct vty *vty; vty = vty_new(); - vty->fd = 0; vty->type = VTY_FILE; vty->node = CONFIG_NODE; vty->priv = priv; + /* By default, write to stderr. Otherwise, during parsing of the logging + * configuration, all invocations to vty_out() would make the process + * write() to its own stdin (fd=0)! */ + vty->fd = fileno(stderr); + ret = config_from_file(vty, confp); if (ret != CMD_SUCCESS) { @@ -1789,6 +1842,8 @@ void vty_init_vtysh(void) /* Install vty's own commands like `who' command. */ void vty_init(struct vty_app_info *app_info) { + unsigned int i, j; + tall_vty_ctx = talloc_named_const(NULL, 0, "vty"); tall_vty_vec_ctx = talloc_named_const(tall_vty_ctx, 0, "vty_vector"); tall_vty_cmd_ctx = talloc_named_const(tall_vty_ctx, 0, "vty_command"); @@ -1797,6 +1852,36 @@ void vty_init(struct vty_app_info *app_info) host.app_info = app_info; + /* Check for duplicate flags in application specific attributes (if any) */ + for (i = 0; i < ARRAY_SIZE(app_info->usr_attr_letters); i++) { + if (app_info->usr_attr_letters[i] == '\0') + continue; + + /* Some flag characters are reserved for global attributes */ + const char rafc[] = VTY_CMD_ATTR_FLAGS_RESERVED; + for (j = 0; j < ARRAY_SIZE(rafc); j++) { + if (app_info->usr_attr_letters[i] != rafc[j]) + continue; + fprintf(stderr, "Attribute flag character '%c' is reserved " + "for globals! Please fix.\n", app_info->usr_attr_letters[i]); + } + + /* Upper case flag letters are reserved for libraries */ + if (app_info->usr_attr_letters[i] >= 'A' && + app_info->usr_attr_letters[i] <= 'Z') { + fprintf(stderr, "Attribute flag letter '%c' is reserved " + "for libraries! Please fix.\n", app_info->usr_attr_letters[i]); + } + + for (j = i + 1; j < ARRAY_SIZE(app_info->usr_attr_letters); j++) { + if (app_info->usr_attr_letters[j] != app_info->usr_attr_letters[i]) + continue; + fprintf(stderr, "Found duplicate flag letter '%c' in application " + "specific attributes (index %u vs %u)! Please fix.\n", + app_info->usr_attr_letters[i], i, j); + } + } + /* For further configuration read, preserve current directory. */ vty_save_cwd(); @@ -1805,18 +1890,18 @@ void vty_init(struct vty_app_info *app_info) /* Install bgp top node. */ install_node(&vty_node, vty_config_write); - install_element_ve(&config_who_cmd); - install_element_ve(&show_history_cmd); - install_element(CONFIG_NODE, &line_vty_cmd); - install_element(CONFIG_NODE, &service_advanced_vty_cmd); - install_element(CONFIG_NODE, &no_service_advanced_vty_cmd); - install_element(CONFIG_NODE, &show_history_cmd); - install_element(ENABLE_NODE, &terminal_monitor_cmd); - install_element(ENABLE_NODE, &terminal_no_monitor_cmd); + install_lib_element_ve(&config_who_cmd); + install_lib_element_ve(&show_history_cmd); + install_lib_element(CONFIG_NODE, &line_vty_cmd); + install_lib_element(CONFIG_NODE, &service_advanced_vty_cmd); + install_lib_element(CONFIG_NODE, &no_service_advanced_vty_cmd); + install_lib_element(CONFIG_NODE, &show_history_cmd); + install_lib_element(ENABLE_NODE, &terminal_monitor_cmd); + install_lib_element(ENABLE_NODE, &terminal_no_monitor_cmd); - install_element(VTY_NODE, &vty_login_cmd); - install_element(VTY_NODE, &no_vty_login_cmd); - install_element(VTY_NODE, &vty_bind_cmd); + install_lib_element(VTY_NODE, &vty_login_cmd); + install_lib_element(VTY_NODE, &no_vty_login_cmd); + install_lib_element(VTY_NODE, &vty_bind_cmd); } /*! Read the configuration file using the VTY code @@ -1832,7 +1917,7 @@ int vty_read_config_file(const char *file_name, void *priv) if (!cfile) return -ENOENT; - rc = vty_read_file(cfile, priv); + rc = vty_read_config_filep(cfile, priv); fclose(cfile); host_config_set(file_name); |