diff options
Diffstat (limited to 'src/core')
69 files changed, 25629 insertions, 0 deletions
diff --git a/src/core/Makefile.am b/src/core/Makefile.am new file mode 100644 index 00000000..326c68d3 --- /dev/null +++ b/src/core/Makefile.am @@ -0,0 +1,165 @@ +# 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 \ + 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/core/application.c b/src/core/application.c new file mode 100644 index 00000000..f7e5816f --- /dev/null +++ b/src/core/application.c @@ -0,0 +1,191 @@ +/*! \file application.c + * Routines for helping with the osmocom application setup. */ +/* + * (C) 2010 by Harald Welte <laforge@gnumonks.org> + * (C) 2011 by Holger Hans Peter Freyther + * + * 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. + * + */ + +/*! \mainpage libosmocore Documentation + * \section sec_intro Introduction + * This library is a collection of common code used in various + * sub-projects inside the Osmocom family of projects. It includes a + * logging framework, select() loop abstraction, timers with callbacks, + * bit vectors, bit packing/unpacking, convolutional decoding, GSMTAP, a + * generic plugin interface, statistics counters, memory allocator, + * socket abstraction, message buffers, etc. + * \n\n + * libosmocodec is developed as part of the Osmocom (Open Source Mobile + * Communications) project, a community-based, collaborative development + * project to create Free and Open Source implementations of mobile + * communications systems. For more information about Osmocom, please + * see https://osmocom.org/ + * + * Please note that C language projects inside Osmocom are typically + * single-threaded event-loop state machine designs. As such, + * routines in libosmocore are not thread-safe. If you must use them in + * a multi-threaded context, you have to add your own locking. + * + * \section sec_copyright Copyright and License + * Copyright © 2008-2017 - Harald Welte, Holger Freyther and contributors\n + * All rights reserved. \n\n + * The source code of libosmocore is licensed 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.\n + * See <http://www.gnu.org/licenses/> or COPYING included in the source + * code package istelf.\n + * The information detailed here is provided AS IS with NO WARRANTY OF + * ANY KIND, INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE. + * \n\n + * + * \section sec_tracker Homepage + Issue Tracker + * The libosmocore project home page can be found at + * https://osmocom.org/projects/libosmocore + * + * An Issue Tracker can be found at + * https://osmocom.org/projects/libosmocore/issues + * + * \section sec_contact Contact and Support + * Community-based support is available at the OpenBSC mailing list + * <http://lists.osmocom.org/mailman/listinfo/openbsc>\n + * Commercial support options available upon request from + * <http://sysmocom.de/> + */ + +#include <osmocom/core/application.h> +#include <osmocom/core/logging.h> + +#include <signal.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <errno.h> +#include <sys/stat.h> + +struct log_target *osmo_stderr_target; + +static void sighup_hdlr(int signal) +{ + log_targets_reopen(); +} + +/*! Ignore SIGPIPE, SIGALRM, SIGHUP and SIGIO */ +void osmo_init_ignore_signals(void) +{ + /* Signals that by default would terminate */ +#ifdef SIGPIPE + signal(SIGPIPE, SIG_IGN); +#endif + signal(SIGALRM, SIG_IGN); +#ifdef SIGHUP + signal(SIGHUP, &sighup_hdlr); +#endif +#ifdef SIGIO + signal(SIGIO, SIG_IGN); +#endif +} + +/*! Initialize the osmocom logging framework + * \param[in] log_info Array of available logging sub-systems + * \returns 0 on success, -1 in case of error + * + * This function initializes the osmocom logging systems. It also + * creates the default (stderr) logging target. + */ +int osmo_init_logging(const struct log_info *log_info) +{ + return osmo_init_logging2(NULL, log_info); +} + +int osmo_init_logging2(void *ctx, const struct log_info *log_info) +{ + static int logging_initialized = 0; + + if (logging_initialized) + return -EEXIST; + + logging_initialized = 1; + log_init(log_info, ctx); + osmo_stderr_target = log_target_create_stderr(); + if (!osmo_stderr_target) + return -1; + + log_add_target(osmo_stderr_target); + log_set_all_filter(osmo_stderr_target, 1); + return 0; +} + +/*! Turn the current process into a background daemon + * + * This function will fork the process, exit the parent and set umask, + * create a new session, close stdin/stdout/stderr and chdir to /tmp + */ +int osmo_daemonize(void) +{ + int rc; + pid_t pid, sid; + + /* Check if parent PID == init, in which case we are already a daemon */ + if (getppid() == 1) + return 0; + + /* Fork from the parent process */ + pid = fork(); + if (pid < 0) { + /* some error happened */ + return pid; + } + + if (pid > 0) { + /* if we have received a positive PID, then we are the parent + * and can exit */ + exit(0); + } + + /* FIXME: do we really want this? */ + umask(0); + + /* Create a new session and set process group ID */ + sid = setsid(); + if (sid < 0) + return sid; + + /* Change to the /tmp directory, which prevents the CWD from being locked + * and unable to remove it */ + rc = chdir("/tmp"); + if (rc < 0) + return rc; + + /* Redirect stdio to /dev/null */ +/* since C89/C99 says stderr is a macro, we can safely do this! */ +#ifdef stderr +/* + * it does not make sense to check the return code here, so we just + * ignore the compiler warning from gcc + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" + freopen("/dev/null", "r", stdin); + freopen("/dev/null", "w", stdout); + freopen("/dev/null", "w", stderr); +#pragma GCC diagnostic pop +#endif + + return 0; +} diff --git a/src/core/backtrace.c b/src/core/backtrace.c new file mode 100644 index 00000000..60bd2381 --- /dev/null +++ b/src/core/backtrace.c @@ -0,0 +1,88 @@ +/*! \file backtrace.c + * Routines related to generating call back traces. */ +/* + * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2012 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 <stdio.h> +#include <stdlib.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> +#include "config.h" + +#ifdef HAVE_EXECINFO_H +#include <execinfo.h> + +static void _osmo_backtrace(int use_printf, int subsys, int level) +{ + int i, nptrs; + void *buffer[100]; + char **strings; + + nptrs = backtrace(buffer, ARRAY_SIZE(buffer)); + if (use_printf) + printf("backtrace() returned %d addresses\n", nptrs); + else + LOGP(subsys, level, "backtrace() returned %d addresses\n", + nptrs); + + strings = backtrace_symbols(buffer, nptrs); + if (!strings) + return; + + for (i = 1; i < nptrs; i++) { + if (use_printf) + printf("%s\n", strings[i]); + else + LOGP(subsys, level, "\t%s\n", strings[i]); + } + + free(strings); +} + +/*! Generate and print a call back-trace + * + * This function will generate a function call back-trace of the + * current process and print it to stdout. */ +void osmo_generate_backtrace(void) +{ + _osmo_backtrace(1, 0, 0); +} + +/*! Generate and log a call back-trace + * \param[in] subsys Logging sub-system + * \param[in] level Logging level + * + * This function will generate a function call back-trace of the + * current process and log it to the specified subsystem and + * level using the libosmocore logging subsystem */ +void osmo_log_backtrace(int subsys, int level) +{ + _osmo_backtrace(0, subsys, level); +} +#else +void osmo_generate_backtrace(void) +{ + printf("This platform has no backtrace function\n"); +} +void osmo_log_backtrace(int subsys, int level) +{ + LOGP(subsys, level, "This platform has no backtrace function\n"); +} +#endif 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/core/bitcomp.c b/src/core/bitcomp.c new file mode 100644 index 00000000..5fb2cba2 --- /dev/null +++ b/src/core/bitcomp.c @@ -0,0 +1,350 @@ +/*! \file bitcomp.c + * Osmocom bit compression routines */ +/* + * (C) 2016 by sysmocom - s.f.m.c. GmbH + * Author: Max Suraev <msuraev@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. + * + */ + +/*! \defgroup bitcomp Bit compression + * @{ + * \file bitcomp.c */ + +#include <stdint.h> +#include <stdbool.h> +#include <errno.h> +#include <string.h> + +#include <osmocom/core/bitvec.h> +#include <osmocom/core/bitcomp.h> + +/* + * Terminating codes for uninterrupted sequences of 0 and 1 up to 64 bit length + * according to TS 44.060 9.1.10 + */ +static const unsigned t4_term[2][64] = { + { + 0b0000110111, + 0b10, + 0b11, + 0b010, + 0b011, + 0b0011, + 0b0010, + 0b00011, + 0b000101, + 0b000100, + 0b0000100, + 0b0000101, + 0b0000111, + 0b00000100, + 0b00000111, + 0b000011000, + 0b0000010111, + 0b0000011000, + 0b0000001000, + 0b00001100111, + 0b00001101000, + 0b00001101100, + 0b00000110111, + 0b00000101000, + 0b00000010111, + 0b00000011000, + 0b000011001010, + 0b000011001011, + 0b000011001100, + 0b000011001101, + 0b000001101000, + 0b000001101001, + 0b000001101010, + 0b000001101011, + 0b000011010010, + 0b000011010011, + 0b000011010100, + 0b000011010101, + 0b000011010110, + 0b000011010111, + 0b000001101100, + 0b000001101101, + 0b000011011010, + 0b000011011011, + 0b000001010100, + 0b000001010101, + 0b000001010110, + 0b000001010111, + 0b000001100100, + 0b000001100101, + 0b000001010010, + 0b000001010011, + 0b000000100100, + 0b000000110111, + 0b000000111000, + 0b000000100111, + 0b000000101000, + 0b000001011000, + 0b000001011001, + 0b000000101011, + 0b000000101100, + 0b000001011010, + 0b000001100110, + 0b000001100111 + }, + { + 0b00110101, + 0b000111, + 0b0111, + 0b1000, + 0b1011, + 0b1100, + 0b1110, + 0b1111, + 0b10011, + 0b10100, + 0b00111, + 0b01000, + 0b001000, + 0b000011, + 0b110100, + 0b110101, + 0b101010, + 0b101011, + 0b0100111, + 0b0001100, + 0b0001000, + 0b0010111, + 0b0000011, + 0b0000100, + 0b0101000, + 0b0101011, + 0b0010011, + 0b0100100, + 0b0011000, + 0b00000010, + 0b00000011, + 0b00011010, + 0b00011011, + 0b00010010, + 0b00010011, + 0b00010100, + 0b00010101, + 0b00010110, + 0b00010111, + 0b00101000, + 0b00101001, + 0b00101010, + 0b00101011, + 0b00101100, + 0b00101101, + 0b00000100, + 0b00000101, + 0b00001010, + 0b00001011, + 0b01010010, + 0b01010011, + 0b01010100, + 0b01010101, + 0b00100100, + 0b00100101, + 0b01011000, + 0b01011001, + 0b01011010, + 0b01011011, + 0b01001010, + 0b01001011, + 0b00110010, + 0b00110011, + 0b00110100 + } +}; + +static const unsigned t4_term_length[2][64] = { + {10, 2, 2, 3, 3, 4, 4, 5, 6, 6, 7, 7, 7, 8, 8, 9, 10, 10, 10, 11, 11, 11, 11, 11, 11, 11, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12}, + {8, 6, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8} +}; + +static const unsigned t4_make_up_length[2][15] = { + {10, 12, 12, 12, 12, 12, 12, 13, 13, 13, 13, 13, 13, 13, 13}, + {5, 5, 6, 7, 8, 8, 8, 8, 8, 8, 9, 9, 9, 9, 9} +}; + +static const unsigned t4_make_up[2][15] = { + { + 0b0000001111, + 0b000011001000, + 0b000011001001, + 0b000001011011, + 0b000000110011, + 0b000000110100, + 0b000000110101, + 0b0000001101100, + 0b0000001101101, + 0b0000001001010, + 0b0000001001011, + 0b0000001001100, + 0b0000001001101, + 0b0000001110010, + 0b0000001110011 + }, + { + 0b11011, + 0b10010, + 0b010111, + 0b0110111, + 0b00110110, + 0b00110111, + 0b01100100, + 0b01100101, + 0b01101000, + 0b01100111, + 0b011001100, + 0b011001101, + 0b011010010, + 0b011010011, + 0b011010100 + } +}; + +/*! Make-up codes for a given length + * + * \return Return proper make-up code word for an uninterrupted + * sequence of b bits of length len according to modified ITU-T T.4 + * from TS 44.060 Table 9.1.10.2 */ +static inline int t4_rle(struct bitvec *bv, unsigned len, bool b) +{ + if (len >= 960) { + bitvec_set_uint(bv, t4_make_up[b][14], t4_make_up_length[b][14]); + return bitvec_set_uint(bv, t4_term[b][len - 960], t4_term_length[b][len - 960]); + } + + if (len >= 896) { + bitvec_set_uint(bv, t4_make_up[b][13], t4_make_up_length[b][13]); + return bitvec_set_uint(bv, t4_term[b][len - 896], t4_term_length[b][len - 896]); + } + + if (len >= 832) { + bitvec_set_uint(bv, t4_make_up[b][12], t4_make_up_length[b][12]); + return bitvec_set_uint(bv, t4_term[b][len - 832], t4_term_length[b][len - 832]); + } + + if (len >= 768) { + bitvec_set_uint(bv, t4_make_up[b][11], t4_make_up_length[b][11]); + return bitvec_set_uint(bv, t4_term[b][len - 768], t4_term_length[b][len - 768]); + } + + if (len >= 704) { + bitvec_set_uint(bv, t4_make_up[b][10], t4_make_up_length[b][10]); + return bitvec_set_uint(bv, t4_term[b][len - 704], t4_term_length[b][len - 704]); + } + + if (len >= 640) { + bitvec_set_uint(bv, t4_make_up[b][9], t4_make_up_length[b][9]); + return bitvec_set_uint(bv, t4_term[b][len - 640], t4_term_length[b][len - 640]); + } + + if (len >= 576) { + bitvec_set_uint(bv, t4_make_up[b][8], t4_make_up_length[b][8]); + return bitvec_set_uint(bv, t4_term[b][len - 576], t4_term_length[b][len - 576]); + } + + if (len >= 512) { + bitvec_set_uint(bv, t4_make_up[b][7], t4_make_up_length[b][7]); + return bitvec_set_uint(bv, t4_term[b][len - 512], t4_term_length[b][len - 512]); + } + + if (len >= 448) { + bitvec_set_uint(bv, t4_make_up[b][6], t4_make_up_length[b][6]); + return bitvec_set_uint(bv, t4_term[b][len - 448], t4_term_length[b][len - 448]); + } + + if (len >= 384) { + bitvec_set_uint(bv, t4_make_up[b][5], t4_make_up_length[b][5]); + return bitvec_set_uint(bv, t4_term[b][len - 384], t4_term_length[b][len - 384]); + } + + if (len >= 320) { + bitvec_set_uint(bv, t4_make_up[b][4], t4_make_up_length[b][4]); + return bitvec_set_uint(bv, t4_term[b][len - 320], t4_term_length[b][len - 320]); + } + + if (len >= 256) { + bitvec_set_uint(bv, t4_make_up[b][3], t4_make_up_length[b][3]); + return bitvec_set_uint(bv, t4_term[b][len - 256], t4_term_length[b][len - 256]); + } + + if (len >= 192) { + bitvec_set_uint(bv, t4_make_up[b][2], t4_make_up_length[b][2]); + return bitvec_set_uint(bv, t4_term[b][len - 192], t4_term_length[b][len - 192]); + } + + if (len >= 128) { + bitvec_set_uint(bv, t4_make_up[b][1], t4_make_up_length[b][1]); + return bitvec_set_uint(bv, t4_term[b][len - 128], t4_term_length[b][len - 128]); + } + + if (len >= 64) { + bitvec_set_uint(bv, t4_make_up[b][0], t4_make_up_length[b][0]); + return bitvec_set_uint(bv, t4_term[b][len - 64], t4_term_length[b][len - 64]); + } + + return bitvec_set_uint(bv, t4_term[b][len], t4_term_length[b][len]); +} + +/*! encode bit vector in-place using T4 encoding + * Assumes MSB first encoding. + * \param[in] bv bit vector to be encoded + * \return color code (if the encoding started with 0 or 1) or -1 on + * failure (encoded is bigger than original) + */ +int osmo_t4_encode(struct bitvec *bv) +{ + unsigned rl0 = bitvec_rl(bv, false), rl1 = bitvec_rl(bv, true); + int r = (rl0 > rl1) ? 0 : 1; + uint8_t orig[bv->data_len], tmp[bv->data_len * 2]; /* FIXME: better estimate max possible encoding overhead */ + struct bitvec comp, vec; + comp.data = tmp; + comp.data_len = bv->data_len * 2; + bitvec_zero(&comp); + vec.data = orig; + vec.data_len = bv->data_len; + bitvec_zero(&vec); + memcpy(vec.data, bv->data, bv->data_len); + vec.cur_bit = bv->cur_bit; + + while (vec.cur_bit > 0) { + if (rl0 > rl1) { + bitvec_shiftl(&vec, rl0); + t4_rle(&comp, rl0, false); + } else { + bitvec_shiftl(&vec, rl1); + t4_rle(&comp, rl1, true); + } + /* + TODO: implement backtracking for optimal encoding + printf(" -> [%d/%d]", comp.cur_bit + vec.cur_bit, bv->cur_bit); + */ + rl0 = bitvec_rl(&vec, false); + rl1 = bitvec_rl(&vec, true); + } + if (comp.cur_bit < bv->cur_bit) { + memcpy(bv->data, tmp, bv->data_len); + bv->cur_bit = comp.cur_bit; + return r; + } + return -1; +} + +/*! @} */ diff --git a/src/core/bits.c b/src/core/bits.c new file mode 100644 index 00000000..3da7d9b9 --- /dev/null +++ b/src/core/bits.c @@ -0,0 +1,313 @@ +/* + * (C) 2011 by Harald Welte <laforge@gnumonks.org> + * (C) 2011 by Sylvain Munaut <tnt@246tNt.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. + * + */ + +#include <stdint.h> + +#include <osmocom/core/bits.h> + +/*! \addtogroup bits + * @{ + * Osmocom bit level support code. + * + * This module implements the notion of different bit-fields, such as + * - unpacked bits (\ref ubit_t), i.e. 1 bit per byte + * - packed bits (\ref pbit_t), i.e. 8 bits per byte + * - soft bits (\ref sbit_t), 1 bit per byte from -127 to 127 + * + * \file bits.c */ + +/*! convert unpacked bits to packed bits, return length in bytes + * \param[out] out output buffer of packed bits + * \param[in] in input buffer of unpacked bits + * \param[in] num_bits number of bits + */ +int osmo_ubit2pbit(pbit_t *out, const ubit_t *in, unsigned int num_bits) +{ + unsigned int i; + uint8_t curbyte = 0; + pbit_t *outptr = out; + + for (i = 0; i < num_bits; i++) { + uint8_t bitnum = 7 - (i % 8); + + curbyte |= (in[i] << bitnum); + + if(i % 8 == 7){ + *outptr++ = curbyte; + curbyte = 0; + } + } + /* we have a non-modulo-8 bitcount */ + if (i % 8) + *outptr++ = curbyte; + + return outptr - out; +} + +/*! Shift unaligned input to octet-aligned output + * \param[out] out output buffer, unaligned + * \param[in] in input buffer, octet-aligned + * \param[in] num_nibbles number of nibbles + */ +void osmo_nibble_shift_right(uint8_t *out, const uint8_t *in, + unsigned int num_nibbles) +{ + unsigned int i, num_whole_bytes = num_nibbles / 2; + if (!num_whole_bytes) + return; + + /* first byte: upper nibble empty, lower nibble from src */ + out[0] = (in[0] >> 4); + + /* bytes 1.. */ + for (i = 1; i < num_whole_bytes; i++) + out[i] = ((in[i - 1] & 0xF) << 4) | (in[i] >> 4); + + /* shift the last nibble, in case there's an odd count */ + i = num_whole_bytes; + if (num_nibbles & 1) + out[i] = ((in[i - 1] & 0xF) << 4) | (in[i] >> 4); + else + out[i] = (in[i - 1] & 0xF) << 4; +} + +/*! Shift unaligned input to octet-aligned output + * \param[out] out output buffer, octet-aligned + * \param[in] in input buffer, unaligned + * \param[in] num_nibbles number of nibbles + */ +void osmo_nibble_shift_left_unal(uint8_t *out, const uint8_t *in, + unsigned int num_nibbles) +{ + unsigned int i, num_whole_bytes = num_nibbles / 2; + if (!num_whole_bytes) + return; + + for (i = 0; i < num_whole_bytes; i++) + out[i] = ((in[i] & 0xF) << 4) | (in[i + 1] >> 4); + + /* shift the last nibble, in case there's an odd count */ + i = num_whole_bytes; + if (num_nibbles & 1) + out[i] = (in[i] & 0xF) << 4; +} + +/*! convert unpacked bits to soft bits + * \param[out] out output buffer of soft bits + * \param[in] in input buffer of unpacked bits + * \param[in] num_bits number of bits + */ +void osmo_ubit2sbit(sbit_t *out, const ubit_t *in, unsigned int num_bits) +{ + unsigned int i; + for (i = 0; i < num_bits; i++) + out[i] = in[i] ? -127 : 127; +} + +/*! convert soft bits to unpacked bits + * \param[out] out output buffer of unpacked bits + * \param[in] in input buffer of soft bits + * \param[in] num_bits number of bits + */ +void osmo_sbit2ubit(ubit_t *out, const sbit_t *in, unsigned int num_bits) +{ + unsigned int i; + for (i = 0; i < num_bits; i++) + out[i] = in[i] < 0; +} + +/*! convert packed bits to unpacked bits, return length in bytes + * \param[out] out output buffer of unpacked bits + * \param[in] in input buffer of packed bits + * \param[in] num_bits number of bits + * \return number of bytes used in \ref out + */ +int osmo_pbit2ubit(ubit_t *out, const pbit_t *in, unsigned int num_bits) +{ + unsigned int i; + ubit_t *cur = out; + ubit_t *limit = out + num_bits; + + for (i = 0; i < (num_bits/8)+1; i++) { + pbit_t byte = in[i]; + *cur++ = (byte >> 7) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 6) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 5) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 4) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 3) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 2) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 1) & 1; + if (cur >= limit) + break; + *cur++ = (byte >> 0) & 1; + if (cur >= limit) + break; + } + return cur - out; +} + +/*! convert unpacked bits to packed bits (extended options) + * \param[out] out output buffer of packed bits + * \param[in] out_ofs offset into output buffer + * \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 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) +{ + unsigned int i, op, bn; + for (i=0; i<num_bits; i++) { + op = out_ofs + i; + bn = lsb_mode ? (op&7) : (7-(op&7)); + if (in[in_ofs+i]) + out[op>>3] |= 1 << bn; + else + out[op>>3] &= ~(1 << bn); + } + return ((out_ofs + num_bits - 1) >> 3) + 1; +} + +/*! convert packed bits to unpacked bits (extended options) + * \param[out] out output buffer of unpacked bits + * \param[in] out_ofs offset into output buffer + * \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 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) +{ + unsigned int i, ip, bn; + for (i=0; i<num_bits; i++) { + ip = in_ofs + i; + bn = lsb_mode ? (ip&7) : (7-(ip&7)); + out[out_ofs+i] = !!(in[ip>>3] & (1<<bn)); + } + 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 + * \returns the reversed 32bit dword + * + * This function reverses the bit order within a 32bit word. Depending + * on "k", it either reverses all bits in a 32bit dword, or the bytes in + * the dword, or the bits in each byte of a dword, or simply swaps the + * two 16bit words in a dword. See Chapter 7 "Hackers Delight" + */ +uint32_t osmo_bit_reversal(uint32_t x, enum osmo_br_mode k) +{ + if (k & 1) x = (x & 0x55555555) << 1 | (x & 0xAAAAAAAA) >> 1; + if (k & 2) x = (x & 0x33333333) << 2 | (x & 0xCCCCCCCC) >> 2; + if (k & 4) x = (x & 0x0F0F0F0F) << 4 | (x & 0xF0F0F0F0) >> 4; + if (k & 8) x = (x & 0x00FF00FF) << 8 | (x & 0xFF00FF00) >> 8; + if (k & 16) x = (x & 0x0000FFFF) << 16 | (x & 0xFFFF0000) >> 16; + + return x; +} + +/*! reverse the bit-order in each byte of a dword + * \param[in] x 32bit input value + * \returns 32bit value where bits of each byte have been reversed + * + * See Chapter 7 "Hackers Delight" + */ +uint32_t osmo_revbytebits_32(uint32_t x) +{ + x = (x & 0x55555555) << 1 | (x & 0xAAAAAAAA) >> 1; + x = (x & 0x33333333) << 2 | (x & 0xCCCCCCCC) >> 2; + x = (x & 0x0F0F0F0F) << 4 | (x & 0xF0F0F0F0) >> 4; + + return x; +} + +/*! reverse the bit order in a byte + * \param[in] x 8bit input value + * \returns 8bit value where bits order has been reversed + */ +uint32_t osmo_revbytebits_8(uint8_t x) +{ + return flip_table[x]; +} + +/*! reverse bit-order of each byte in a buffer + * \param[in] buf buffer containing bytes to be bit-reversed + * \param[in] len length of buffer in bytes + * + * This function reverses the bits in each byte of the buffer + */ +void osmo_revbytebits_buf(uint8_t *buf, int len) +{ + int i; + + for (i = 0; i < len; i++) + buf[i] = flip_table[buf[i]]; +} + +/*! @} */ diff --git a/src/core/bitvec.c b/src/core/bitvec.c new file mode 100644 index 00000000..38ea1bb5 --- /dev/null +++ b/src/core/bitvec.c @@ -0,0 +1,708 @@ +/* (C) 2009 by Harald Welte <laforge@gnumonks.org> + * (C) 2012 Ivan Klyuchnikov + * (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 bitvec + * @{ + * Osmocom bit vector abstraction utility routines. + * + * These functions assume a MSB (most significant bit) first layout of the + * bits, so that for instance the 5 bit number abcde (a is MSB) can be + * embedded into a byte sequence like in xxxxxxab cdexxxxx. The bit count + * starts with the MSB, so the bits in a byte are numbered (MSB) 01234567 (LSB). + * Note that there are other incompatible encodings, like it is used + * for the EGPRS RLC data block headers (there the bits are numbered from LSB + * to MSB). + * + * \file bitvec.c */ + +#include <errno.h> +#include <stdint.h> +#include <string.h> +#include <stdio.h> +#include <stdbool.h> + +#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) + +static inline unsigned int bytenum_from_bitnum(unsigned int bitnum) +{ + unsigned int bytenum = bitnum / 8; + + return bytenum; +} + +/* convert ZERO/ONE/L/H to a bitmask at given pos in a byte */ +static uint8_t bitval2mask(enum bit_value bit, uint8_t bitnum) +{ + switch (bit) { + case ZERO: + return (0 << bitnum); + case ONE: + return (1 << bitnum); + case L: + return ((0x2b ^ (0 << bitnum)) & (1 << bitnum)); + case H: + return ((0x2b ^ (1 << bitnum)) & (1 << bitnum)); + default: + return 0; + } +} + +/*! check if the bit is 0 or 1 for a given position inside a bitvec + * \param[in] bv the bit vector on which to check + * \param[in] bitnr the bit number inside the bit vector to check + * \return value of the requested bit + */ +enum bit_value bitvec_get_bit_pos(const struct bitvec *bv, unsigned int bitnr) +{ + unsigned int bytenum = bytenum_from_bitnum(bitnr); + unsigned int bitnum = 7 - (bitnr % 8); + uint8_t bitval; + + if (bytenum >= bv->data_len) + return -EINVAL; + + bitval = bitval2mask(ONE, bitnum); + + if (bv->data[bytenum] & bitval) + return ONE; + + return ZERO; +} + +/*! check if the bit is L or H for a given position inside a bitvec + * \param[in] bv the bit vector on which to check + * \param[in] bitnr the bit number inside the bit vector to check + * \return value of the requested bit + */ +enum bit_value bitvec_get_bit_pos_high(const struct bitvec *bv, + unsigned int bitnr) +{ + unsigned int bytenum = bytenum_from_bitnum(bitnr); + unsigned int bitnum = 7 - (bitnr % 8); + uint8_t bitval; + + if (bytenum >= bv->data_len) + return -EINVAL; + + bitval = bitval2mask(H, bitnum); + + if ((bv->data[bytenum] & (1 << bitnum)) == bitval) + return H; + + return L; +} + +/*! get the Nth set bit inside the bit vector + * \param[in] bv the bit vector to use + * \param[in] n the bit number to get + * \returns the bit number (offset) of the Nth set bit in \a bv + */ +unsigned int bitvec_get_nth_set_bit(const struct bitvec *bv, unsigned int n) +{ + unsigned int i, k = 0; + + for (i = 0; i < bv->data_len*8; i++) { + if (bitvec_get_bit_pos(bv, i) == ONE) { + k++; + if (k == n) + return i; + } + } + + return 0; +} + +/*! set a bit at given position in a bit vector + * \param[in] bv bit vector on which to operate + * \param[in] bitnr number of bit to be set + * \param[in] bit value to which the bit is to be set + * \returns 0 on success, negative value on error + */ +inline int bitvec_set_bit_pos(struct bitvec *bv, unsigned int bitnr, + enum bit_value bit) +{ + unsigned int bytenum = bytenum_from_bitnum(bitnr); + unsigned int bitnum = 7 - (bitnr % 8); + uint8_t bitval; + + if (bytenum >= bv->data_len) + return -EINVAL; + + /* first clear the bit */ + bitval = bitval2mask(ONE, bitnum); + bv->data[bytenum] &= ~bitval; + + /* then set it to desired value */ + bitval = bitval2mask(bit, bitnum); + bv->data[bytenum] |= bitval; + + return 0; +} + +/*! set the next bit inside a bitvec + * \param[in] bv bit vector to be used + * \param[in] bit value of the bit to be set + * \returns 0 on success, negative value on error + */ +inline int bitvec_set_bit(struct bitvec *bv, enum bit_value bit) +{ + int rc; + + rc = bitvec_set_bit_pos(bv, bv->cur_bit, bit); + if (!rc) + bv->cur_bit++; + + return rc; +} + +/*! get the next bit (low/high) inside a bitvec + * \return value of th next bit in the vector */ +int bitvec_get_bit_high(struct bitvec *bv) +{ + int rc; + + rc = bitvec_get_bit_pos_high(bv, bv->cur_bit); + if (rc >= 0) + bv->cur_bit++; + + return rc; +} + +/*! set multiple bits (based on array of bitvals) at current pos + * \param[in] bv bit vector + * \param[in] bits array of \ref bit_value + * \param[in] count number of bits to set + * \return 0 on success; negative in case of error */ +int bitvec_set_bits(struct bitvec *bv, const enum bit_value *bits, unsigned int count) +{ + unsigned int i; + int rc; + + for (i = 0; i < count; i++) { + rc = bitvec_set_bit(bv, bits[i]); + if (rc) + return rc; + } + + return 0; +} + +/*! set multiple bits (based on numeric value) at current pos. + * \param[in] bv bit vector. + * \param[in] v mask representing which bits needs to be set. + * \param[in] num_bits number of meaningful bits in the mask. + * \param[in] use_lh whether to interpret the bits as L/H values or as 0/1. + * \return 0 on success; negative in case of error. */ +int bitvec_set_u64(struct bitvec *bv, uint64_t v, uint8_t num_bits, bool use_lh) +{ + uint8_t i; + + if (num_bits > 64) + return -E2BIG; + + for (i = 0; i < num_bits; i++) { + int rc; + enum bit_value bit = use_lh ? L : 0; + + if (v & ((uint64_t)1 << (num_bits - i - 1))) + bit = use_lh ? H : 1; + + rc = bitvec_set_bit(bv, bit); + if (rc != 0) + return rc; + } + + return 0; +} + +/*! set multiple bits (based on numeric value) at current pos. + * \return 0 in case of success; negative in case of error. */ +int bitvec_set_uint(struct bitvec *bv, unsigned int ui, unsigned int num_bits) +{ + return bitvec_set_u64(bv, ui, num_bits, false); +} + +/*! get multiple bits (num_bits) from beginning of vector (MSB side) + * \return 16bit signed integer retrieved from bit vector */ +int16_t bitvec_get_int16_msb(const struct bitvec *bv, unsigned int num_bits) +{ + if (num_bits > 15 || bv->cur_bit < num_bits) + return -EINVAL; + + if (num_bits < 9) + return bv->data[0] >> (8 - num_bits); + + return osmo_load16be(bv->data) >> (16 - num_bits); +} + +/*! get multiple bits (based on numeric value) from current pos + * \return integer value retrieved from bit vector */ +int bitvec_get_uint(struct bitvec *bv, unsigned int num_bits) +{ + unsigned int i; + unsigned int ui = 0; + + for (i = 0; i < num_bits; i++) { + int bit = bitvec_get_bit_pos(bv, bv->cur_bit); + if (bit < 0) + return bit; + if (bit) + ui |= ((unsigned)1 << (num_bits - i - 1)); + bv->cur_bit++; + } + + return ui; +} + +/*! fill num_bits with \fill starting from the current position + * \return 0 on success; negative otherwise (out of vector boundary) + */ +int bitvec_fill(struct bitvec *bv, unsigned int num_bits, enum bit_value fill) +{ + unsigned i, stop = bv->cur_bit + num_bits; + for (i = bv->cur_bit; i < stop; i++) + if (bitvec_set_bit(bv, fill) < 0) + return -EINVAL; + + return 0; +} + +/*! 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) +{ + int n = up_to_bit - bv->cur_bit + 1; + if (n < 1) + return 0; + + return bitvec_fill(bv, n, L); +} + +/*! find first bit set in bit vector + * \return 0 on success; negative otherwise */ +int bitvec_find_bit_pos(const struct bitvec *bv, unsigned int n, + enum bit_value val) +{ + unsigned int i; + + for (i = n; i < bv->data_len*8; i++) { + if (bitvec_get_bit_pos(bv, i) == val) + return i; + } + + return -1; +} + +/*! get multiple bytes from current pos + * Assumes MSB first encoding. + * \param[in] bv bit vector + * \param[in] bytes array + * \param[in] count number of bytes to copy + * \return 0 on success; negative otherwise + */ +int bitvec_get_bytes(struct bitvec *bv, uint8_t *bytes, unsigned int count) +{ + int byte_offs = bytenum_from_bitnum(bv->cur_bit); + int bit_offs = bv->cur_bit % 8; + uint8_t c, last_c; + int i; + uint8_t *src; + + if (byte_offs + count + (bit_offs ? 1 : 0) > bv->data_len) + return -EINVAL; + + if (bit_offs == 0) { + memcpy(bytes, bv->data + byte_offs, count); + } else { + src = bv->data + byte_offs; + last_c = *(src++); + for (i = count; i > 0; i--) { + c = *(src++); + *(bytes++) = + (last_c << bit_offs) | + (c >> (8 - bit_offs)); + last_c = c; + } + } + + bv->cur_bit += count * 8; + return 0; +} + +/*! set multiple bytes at current pos + * Assumes MSB first encoding. + * \param[in] bv bit vector + * \param[in] bytes array + * \param[in] count number of bytes to copy + * \return 0 on success; negative otherwise + */ +int bitvec_set_bytes(struct bitvec *bv, const uint8_t *bytes, unsigned int count) +{ + int byte_offs = bytenum_from_bitnum(bv->cur_bit); + int bit_offs = bv->cur_bit % 8; + uint8_t c, last_c; + int i; + uint8_t *dst; + + if (byte_offs + count + (bit_offs ? 1 : 0) > bv->data_len) + return -EINVAL; + + if (bit_offs == 0) { + memcpy(bv->data + byte_offs, bytes, count); + } else if (count > 0) { + dst = bv->data + byte_offs; + /* Get lower bits of first dst byte */ + last_c = *dst >> (8 - bit_offs); + for (i = count; i > 0; i--) { + c = *(bytes++); + *(dst++) = + (last_c << (8 - bit_offs)) | + (c >> bit_offs); + last_c = c; + } + /* Overwrite lower bits of N+1 dst byte */ + *dst = (*dst & ((1 << (8 - bit_offs)) - 1)) | + (last_c << (8 - bit_offs)); + } + + bv->cur_bit += count * 8; + return 0; +} + +/*! Allocate a bit vector + * \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, void *ctx) +{ + struct bitvec *bv = talloc(ctx, struct bitvec); + if (!bv) + return NULL; + + bv->data = talloc_zero_array(bv, uint8_t, size); + if (!(bv->data)) { + talloc_free(bv); + return NULL; + } + + bv->data_len = size; + bv->cur_bit = 0; + return bv; +} + +/*! Free a bit vector (release its memory) + * \param[in] bit vector to free */ +void bitvec_free(struct bitvec *bv) +{ + if (bv == NULL) + return; + talloc_free(bv->data); + talloc_free(bv); +} + +/*! Export a bit vector to a buffer + * \param[in] bitvec (unpacked bits) + * \param[out] buffer for the unpacked bits + * \return number of bytes (= bits) copied */ +unsigned int bitvec_pack(const struct bitvec *bv, uint8_t *buffer) +{ + unsigned int i; + for (i = 0; i < bv->data_len; i++) + buffer[i] = bv->data[i]; + + return i; +} + +/*! Copy buffer of unpacked bits into bit vector + * \param[in] buffer unpacked input bits + * \param[out] bv unpacked bit vector + * \return number of bytes (= bits) copied */ +unsigned int bitvec_unpack(struct bitvec *bv, const uint8_t *buffer) +{ + unsigned int i; + for (i = 0; i < bv->data_len; i++) + bv->data[i] = buffer[i]; + + return i; +} + +/*! read hexadecimap string into a bit vector + * \param[in] src string containing hex digits + * \param[out] bv unpacked bit vector + * \return 0 in case of success; 1 in case of error + */ +int bitvec_unhex(struct bitvec *bv, const char *src) +{ + int rc; + + 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; +} + +/*! read part of the vector + * \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 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++) { + 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++; + } + *read_index += len; + return ui; +} + +/*! write into the vector + * \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 0 on success, negative value on error + */ +int bitvec_write_field(struct bitvec *bv, unsigned int *write_index, uint64_t val, unsigned int len) +{ + int rc; + + bv->cur_bit = *write_index; + + rc = bitvec_set_u64(bv, val, len, false); + if (rc != 0) + return rc; + + *write_index += len; + + return 0; +} + +/*! convert enum to corresponding character + * \param v input value (bit) + * \return single character, either 0, 1, L or H */ +char bit_value_to_char(enum bit_value v) +{ + switch (v) { + case ZERO: return '0'; + case ONE: return '1'; + case L: return 'L'; + case H: return 'H'; + default: osmo_panic("unexpected input in bit_value_to_char"); return 'X'; + } +} + +/*! prints bit vector to provided string + * It's caller's responsibility to ensure that we won't shoot him in the foot: + * the provided buffer should be at lest cur_bit + 1 bytes long + */ +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++) { + if (0 == i % 8) + *cur++ = ' '; + *cur++ = bit_value_to_char(bitvec_get_bit_pos(bv, i)); + pos++; + } + *cur = 0; +} + +/* we assume that x have at least 1 non-b bit */ +static inline unsigned leading_bits(uint8_t x, bool b) +{ + if (b) { + if (x < 0x80) return 0; + if (x < 0xC0) return 1; + if (x < 0xE0) return 2; + if (x < 0xF0) return 3; + if (x < 0xF8) return 4; + if (x < 0xFC) return 5; + if (x < 0xFE) return 6; + } else { + if (x > 0x7F) return 0; + if (x > 0x3F) return 1; + if (x > 0x1F) return 2; + if (x > 0xF) return 3; + if (x > 7) return 4; + if (x > 3) return 5; + if (x > 1) return 6; + } + return 7; +} +/*! force bit vector to all 0 and current bit to the beginnig of the vector */ +void bitvec_zero(struct bitvec *bv) +{ + bv->cur_bit = 0; + memset(bv->data, 0, bv->data_len); +} + +/*! Return number (bits) of uninterrupted bit run in vector starting from the MSB + * \param[in] bv The boolean vector to work on + * \param[in] b The boolean, sequence of which is looked at from the vector start + * \returns Number of consecutive bits of \p b in \p bv + */ +unsigned bitvec_rl(const struct bitvec *bv, bool b) +{ + unsigned i; + for (i = 0; i < (bv->cur_bit % 8 ? bv->cur_bit / 8 + 1 : bv->cur_bit / 8); i++) { + if ( (b ? 0xFF : 0) != bv->data[i]) + return i * 8 + leading_bits(bv->data[i], b); + } + + return bv->cur_bit; +} + +/*! Return number (bits) of uninterrupted bit run in vector + * starting from the current bit + * \param[in] bv The boolean vector to work on + * \param[in] b The boolean, sequence of 1's or 0's to be checked + * \param[in] max_bits Total Number of Uncmopresed bits + * \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, unsigned int max_bits) +{ + unsigned i = 0; + unsigned j = 8; + int temp_res = 0; + int count = 0; + unsigned readIndex = bv->cur_bit; + unsigned remaining_bits = max_bits % 8; + unsigned remaining_bytes = max_bits / 8; + unsigned byte_mask = 0xFF; + + if (readIndex % 8) { + for (j -= (readIndex % 8) ; j > 0 ; j--) { + if (readIndex < max_bits && bitvec_read_field(bv, &readIndex, 1) == b) + temp_res++; + else { + bv->cur_bit--; + return temp_res; + } + } + } + for (i = (readIndex / 8); + i < (remaining_bits ? remaining_bytes + 1 : remaining_bytes); + i++, count++) { + if ((b ? byte_mask : 0) != bv->data[i]) { + bv->cur_bit = (count * 8 + + leading_bits(bv->data[i], b) + readIndex); + return count * 8 + + leading_bits(bv->data[i], b) + temp_res; + } + } + bv->cur_bit = (temp_res + (count * 8)) + readIndex; + if (bv->cur_bit > max_bits) + bv->cur_bit = max_bits; + return (bv->cur_bit - readIndex + temp_res); +} + +/*! Shifts bitvec to the left, n MSB bits lost */ +void bitvec_shiftl(struct bitvec *bv, unsigned n) +{ + if (0 == n) + return; + if (n >= bv->cur_bit) { + bitvec_zero(bv); + return; + } + + memmove(bv->data, bv->data + n / 8, bv->data_len - n / 8); + + uint8_t tmp[2]; + unsigned i; + for (i = 0; i < bv->data_len - 2; i++) { + uint16_t t = osmo_load16be(bv->data + i); + osmo_store16be(t << (n % 8), &tmp); + bv->data[i] = tmp[0]; + } + + bv->data[bv->data_len - 1] <<= (n % 8); + bv->cur_bit -= n; +} + +/*! Add given array to bitvec + * \param[in,out] bv bit vector to work with + * \param[in] array elements to be added + * \param[in] array_len length of array + * \param[in] dry_run indicates whether to return number of bits required + * instead of adding anything to bv for real + * \param[in] num_bits number of bits to consider in each element of array + * \returns number of bits necessary to add array elements if dry_run is true, + * 0 otherwise (only in this case bv is actually changed) + * + * N. B: no length checks are performed on bv - it's caller's job to ensure + * enough space is available - for example by calling with dry_run = true first. + * + * Useful for common pattern in CSN.1 spec which looks like: + * { 1 < XXX : bit (num_bits) > } ** 0 + * which means repeat any times (between 0 and infinity), + * start each repetition with 1, mark end of repetitions with 0 bit + * see app. note in 3GPP TS 24.007 § B.2.1 Rule A2 + */ +unsigned int bitvec_add_array(struct bitvec *bv, const uint32_t *array, + unsigned int array_len, bool dry_run, + unsigned int num_bits) +{ + unsigned i, bits = 1; /* account for stop bit */ + for (i = 0; i < array_len; i++) { + if (dry_run) { + bits += (1 + num_bits); + } else { + bitvec_set_bit(bv, 1); + bitvec_set_uint(bv, array[i], num_bits); + } + } + + if (dry_run) + return bits; + + bitvec_set_bit(bv, 0); /* stop bit - end of the sequence */ + return 0; +} + +/*! @} */ diff --git a/src/core/context.c b/src/core/context.c new file mode 100644 index 00000000..a0b3a555 --- /dev/null +++ b/src/core/context.c @@ -0,0 +1,47 @@ +/*! \file context.c + * talloc context handling. + * + * (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 <string.h> +#include <errno.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> + +__thread struct osmo_talloc_contexts *osmo_ctx; + +int osmo_ctx_init(const char *id) +{ + osmo_ctx = talloc_named(NULL, sizeof(*osmo_ctx), "global-%s", id); + if (!osmo_ctx) + return -ENOMEM; + memset(osmo_ctx, 0, sizeof(*osmo_ctx)); + osmo_ctx->global = osmo_ctx; + osmo_ctx->select = talloc_named_const(osmo_ctx->global, 0, "select"); + if (!osmo_ctx->select) { + talloc_free(osmo_ctx); + return -ENOMEM; + } + return 0; +} + +/* initialize osmo_ctx on main tread */ +static __attribute__((constructor(101))) void on_dso_load_ctx(void) +{ + OSMO_ASSERT(osmo_ctx_init("main") == 0); +} + +/*! @} */ diff --git a/src/core/conv.c b/src/core/conv.c new file mode 100644 index 00000000..8963018b --- /dev/null +++ b/src/core/conv.c @@ -0,0 +1,674 @@ +/*! \file conv.c + * Generic convolutional encoding / decoding. */ +/* + * Copyright (C) 2011 Sylvain Munaut <tnt@246tNt.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 conv + * @{ + * Osmocom convolutional encoder and decoder. + * + * \file conv.c */ + +#include "config.h" +#ifdef HAVE_ALLOCA_H +#include <alloca.h> +#endif +#include <stdint.h> +#include <stdlib.h> +#include <string.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/bits.h> +#include <osmocom/core/conv.h> + + +/* ------------------------------------------------------------------------ */ +/* Common */ +/* ------------------------------------------------------------------------ */ + +int +osmo_conv_get_input_length(const struct osmo_conv_code *code, int len) +{ + return len <= 0 ? code->len : len; +} + +int +osmo_conv_get_output_length(const struct osmo_conv_code *code, int len) +{ + int pbits, in_len, out_len; + + /* Input length */ + in_len = osmo_conv_get_input_length(code, len); + + /* Output length */ + out_len = in_len * code->N; + + if (code->term == CONV_TERM_FLUSH) + out_len += code->N * (code->K - 1); + + /* Count punctured bits */ + if (code->puncture) { + for (pbits = 0; code->puncture[pbits] >= 0; pbits++) {} + out_len -= pbits; + } + + return out_len; +} + + +/* ------------------------------------------------------------------------ */ +/* Encoding */ +/* ------------------------------------------------------------------------ */ + +/*! Initialize a convolutional encoder + * \param[in,out] encoder Encoder state to initialize + * \param[in] code Description of convolutional code + */ +void +osmo_conv_encode_init(struct osmo_conv_encoder *encoder, + 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) +{ + int i; + uint8_t state = 0; + + for (i = 0; i < (encoder->code->K - 1); i++) + state = (state << 1) | input[i]; + + encoder->state = state; +} + +static inline int +_conv_encode_do_output(struct osmo_conv_encoder *encoder, + 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++) { + int bit_no = code->N - j - 1; + int r_idx = encoder->i_idx * code->N + j; + + if (code->puncture[encoder->p_idx] == r_idx) + encoder->p_idx++; + else + output[o_idx++] = (out >> bit_no) & 1; + } + } else { + for (j = 0; j < code->N; j++) { + int bit_no = code->N - j - 1; + output[o_idx++] = (out >> bit_no) & 1; + } + } + + return o_idx; +} + +int +osmo_conv_encode_raw(struct osmo_conv_encoder *encoder, + const ubit_t *input, ubit_t *output, int n) +{ + const struct osmo_conv_code *code = encoder->code; + uint8_t state; + int i; + int o_idx; + + o_idx = 0; + state = encoder->state; + + for (i = 0; i < n; i++) { + int bit = input[i]; + uint8_t out; + + out = code->next_output[state][bit]; + state = code->next_state[state][bit]; + + o_idx += _conv_encode_do_output(encoder, out, &output[o_idx]); + + encoder->i_idx++; + } + + encoder->state = state; + + return o_idx; +} + +int +osmo_conv_encode_flush(struct osmo_conv_encoder *encoder, ubit_t *output) +{ + const struct osmo_conv_code *code = encoder->code; + uint8_t state; + int n; + int i; + int o_idx; + + n = code->K - 1; + + o_idx = 0; + state = encoder->state; + + for (i = 0; i < n; i++) { + uint8_t out; + + if (code->next_term_output) { + out = code->next_term_output[state]; + state = code->next_term_state[state]; + } else { + out = code->next_output[state][0]; + state = code->next_state[state][0]; + } + + o_idx += _conv_encode_do_output(encoder, out, &output[o_idx]); + + encoder->i_idx++; + } + + encoder->state = state; + + return o_idx; +} + +/*! All-in-one convolutional encoding function + * \param[in] code description of convolutional code to be used + * \param[in] input array of unpacked bits (uncoded) + * \param[out] output array of unpacked bits (encoded) + * \return Number of produced output bits + * + * This is an all-in-one function, taking care of + * \ref osmo_conv_init, \ref osmo_conv_encode_load_state, + * \ref osmo_conv_encode_raw and \ref osmo_conv_encode_flush as needed. + */ +int +osmo_conv_encode(const struct osmo_conv_code *code, + const ubit_t *input, ubit_t *output) +{ + struct osmo_conv_encoder encoder; + int l; + + osmo_conv_encode_init(&encoder, code); + + if (code->term == CONV_TERM_TAIL_BITING) { + int eidx = code->len - code->K + 1; + osmo_conv_encode_load_state(&encoder, &input[eidx]); + } + + l = osmo_conv_encode_raw(&encoder, input, output, code->len); + + if (code->term == CONV_TERM_FLUSH) + l += osmo_conv_encode_flush(&encoder, &output[l]); + + return l; +} + + +/* ------------------------------------------------------------------------ */ +/* Decoding (viterbi) */ +/* ------------------------------------------------------------------------ */ + +#define MAX_AE 0x00ffffff + +/* 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); + +void +osmo_conv_decode_init(struct osmo_conv_decoder *decoder, + const struct osmo_conv_code *code, int len, + int start_state) +{ + int n_states; + + /* Init */ + if (len <= 0) + len = code->len; + + n_states = 1 << (code->K - 1); + + memset(decoder, 0x00, sizeof(struct osmo_conv_decoder)); + + decoder->code = code; + decoder->n_states = n_states; + decoder->len = len; + + /* Allocate arrays */ + 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)); + + /* Classic reset */ + osmo_conv_decode_reset(decoder, start_state); +} + +void +osmo_conv_decode_reset(struct osmo_conv_decoder *decoder, int start_state) +{ + int i; + + /* Reset indexes */ + decoder->o_idx = 0; + decoder->p_idx = 0; + + /* Initial error */ + if (start_state < 0) { + /* All states possible */ + memset(decoder->ae, 0x00, sizeof(unsigned int) * decoder->n_states); + } else { + /* Fixed start state */ + for (i = 0; i < decoder->n_states; i++) { + decoder->ae[i] = (i == start_state) ? 0 : MAX_AE; + } + } +} + +void +osmo_conv_decode_rewind(struct osmo_conv_decoder *decoder) +{ + int i; + unsigned int min_ae = MAX_AE; + + /* Reset indexes */ + decoder->o_idx = 0; + decoder->p_idx = 0; + + /* Initial error normalize (remove constant) */ + 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++) + decoder->ae[i] -= min_ae; +} + +void +osmo_conv_decode_deinit(struct osmo_conv_decoder *decoder) +{ + free(decoder->ae); + free(decoder->ae_next); + free(decoder->state_history); + + memset(decoder, 0x00, sizeof(struct osmo_conv_decoder)); +} + +int +osmo_conv_decode_scan(struct osmo_conv_decoder *decoder, + const sbit_t *input, int n) +{ + const struct osmo_conv_code *code = decoder->code; + + int i, s, b, j; + + int n_states; + unsigned int *ae; + unsigned int *ae_next; + uint8_t *state_history; + sbit_t *in_sym; + + int i_idx, p_idx; + + /* Prepare */ + n_states = decoder->n_states; + + 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); + + i_idx = 0; + p_idx = decoder->p_idx; + + /* Scan the treillis */ + for (i = 0; i < n; i++) { + /* Reset next accumulated error */ + 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++) { + int idx = ((decoder->o_idx + i) * code->N) + j; + if (idx == code->puncture[p_idx]) { + in_sym[j] = 0; /* Undefined */ + p_idx++; + } else { + in_sym[j] = input[i_idx]; + i_idx++; + } + } + } else { + /* Easy, just copy N bits */ + memcpy(in_sym, &input[i_idx], code->N); + i_idx += code->N; + } + + /* Scan all state */ + for (s = 0; s < n_states; s++) { + /* Scan possible input bits */ + 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 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++) { + int is = (int)in_sym[j]; + if (is) { + ov = (out & m) ? -127 : 127; /* sbit_t value for it */ + e = is - ov; /* raw error for this bit */ + nae += (e * e) >> 9; /* acc the squared/scaled value */ + } + m >>= 1; /* next mask bit */ + } + + /* Is it survivor ? */ + if (ae_next[state] > nae) { + ae_next[state] = nae; + state_history[(n_states * i) + state] = s; + } + } + } + + /* Copy accumulated error */ + memcpy(ae, ae_next, sizeof(unsigned int) * n_states); + } + + /* Update decoder state */ + decoder->p_idx = p_idx; + decoder->o_idx += n; + + return i_idx; +} + +int +osmo_conv_decode_flush(struct osmo_conv_decoder *decoder, const sbit_t *input) +{ + const struct osmo_conv_code *code = decoder->code; + + int i, s, j; + + int n_states; + unsigned int *ae; + unsigned int *ae_next; + uint8_t *state_history; + sbit_t *in_sym; + + int i_idx, p_idx; + + /* Prepare */ + n_states = decoder->n_states; + + 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); + + i_idx = 0; + p_idx = decoder->p_idx; + + /* Scan the treillis */ + for (i = 0; i < code->K - 1; i++) { + /* Reset next accumulated error */ + 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++) { + int idx = ((decoder->o_idx + i) * code->N) + j; + if (idx == code->puncture[p_idx]) { + in_sym[j] = 0; /* Undefined */ + p_idx++; + } else { + in_sym[j] = input[i_idx]; + i_idx++; + } + } + } else { + /* Easy, just copy N bits */ + memcpy(in_sym, &input[i_idx], code->N); + i_idx += code->N; + } + + /* Scan all state */ + for (s = 0; s < n_states; s++) { + int nae, ov, e; + uint8_t m; + + /* Next output and state */ + uint8_t out; + uint8_t state; + + if (code->next_term_output) { + out = code->next_term_output[s]; + state = code->next_term_state[s]; + } else { + out = code->next_output[s][0]; + state = code->next_state[s][0]; + } + + /* 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++) { + int is = (int)in_sym[j]; + if (is) { + ov = (out & m) ? -127 : 127; /* sbit_t value for it */ + e = is - ov; /* raw error for this bit */ + nae += (e * e) >> 9; /* acc the squared/scaled value */ + } + m >>= 1; /* next mask bit */ + } + + /* Is it survivor ? */ + if (ae_next[state] > nae) { + ae_next[state] = nae; + state_history[(n_states * i) + state] = s; + } + } + + /* Copy accumulated error */ + memcpy(ae, ae_next, sizeof(unsigned int) * n_states); + } + + /* Update decoder state */ + decoder->p_idx = p_idx; + decoder->o_idx += code->K - 1; + + return i_idx; +} + +int +osmo_conv_decode_get_best_end_state(struct osmo_conv_decoder *decoder) +{ + const struct osmo_conv_code *code = decoder->code; + + int min_ae, min_state; + int s; + + /* If flushed, we _know_ the end state */ + if (code->term == CONV_TERM_FLUSH) + return 0; + + /* Search init */ + min_state = -1; + min_ae = MAX_AE; + + /* 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_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; + + n = decoder->o_idx; + + sh_ptr = &decoder->state_history[decoder->n_states * (n-1)]; + + /* No output for the K-1 termination input bits */ + if (has_flush) { + 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--) { + min_state = cur_state; + cur_state = sh_ptr[cur_state]; + + sh_ptr -= decoder->n_states; + + if (code->next_state[cur_state][0] == min_state) + output[i] = 0; + else + output[i] = 1; + } + + return min_ae; +} + +/*! All-in-one convolutional decoding function + * \param[in] code description of convolutional code to be used + * \param[in] input array of soft bits (coded) + * \param[out] output array of unpacked bits (decoded) + * + * 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_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) +{ + struct osmo_conv_decoder decoder; + int rv, l; + + /* Use accelerated implementation for supported codes */ + if ((code->N <= 4) && ((code->K == 5) || (code->K == 7))) + return osmo_conv_decode_acc(code, input, output); + + osmo_conv_decode_init(&decoder, code, 0, 0); + + if (code->term == CONV_TERM_TAIL_BITING) { + osmo_conv_decode_scan(&decoder, input, code->len); + osmo_conv_decode_rewind(&decoder); + } + + l = osmo_conv_decode_scan(&decoder, input, code->len); + + if (code->term == CONV_TERM_FLUSH) + osmo_conv_decode_flush(&decoder, &input[l]); + + rv = osmo_conv_decode_get_output(&decoder, output, + code->term == CONV_TERM_FLUSH, /* has_flush */ + -1 /* end_state */ + ); + + osmo_conv_decode_deinit(&decoder); + + return rv; +} + +/*! @} */ diff --git a/src/core/conv_acc.c b/src/core/conv_acc.c new file mode 100644 index 00000000..4bd3b076 --- /dev/null +++ b/src/core/conv_acc.c @@ -0,0 +1,754 @@ +/*! \file conv_acc.c + * Accelerated Viterbi decoder implementation. */ +/* + * Copyright (C) 2013, 2014 Thomas Tsou <tom@tsou.cc> + * + * 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 <string.h> +#include <errno.h> + +#include "config.h" + +#include <osmocom/core/conv.h> + +#define BIT2NRZ(REG,N) (((REG >> N) & 0x01) * 2 - 1) * -1 +#define NUM_STATES(K) (K == 7 ? 64 : 16) + +#define INIT_POINTERS(simd) \ +{ \ + osmo_conv_metrics_k5_n2 = osmo_conv_##simd##_metrics_k5_n2; \ + osmo_conv_metrics_k5_n3 = osmo_conv_##simd##_metrics_k5_n3; \ + osmo_conv_metrics_k5_n4 = osmo_conv_##simd##_metrics_k5_n4; \ + osmo_conv_metrics_k7_n2 = osmo_conv_##simd##_metrics_k7_n2; \ + osmo_conv_metrics_k7_n3 = osmo_conv_##simd##_metrics_k7_n3; \ + osmo_conv_metrics_k7_n4 = osmo_conv_##simd##_metrics_k7_n4; \ + vdec_malloc = &osmo_conv_##simd##_vdec_malloc; \ + vdec_free = &osmo_conv_##simd##_vdec_free; \ +} + +static int init_complete = 0; + +__attribute__ ((visibility("hidden"))) int avx2_supported = 0; +__attribute__ ((visibility("hidden"))) int ssse3_supported = 0; +__attribute__ ((visibility("hidden"))) int sse41_supported = 0; + +/** + * These pointers are being initialized at runtime by the + * osmo_conv_init() depending on supported SIMD extensions. + */ +static int16_t *(*vdec_malloc)(size_t n); +static void (*vdec_free)(int16_t *ptr); + +void (*osmo_conv_metrics_k5_n2)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); +void (*osmo_conv_metrics_k5_n3)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); +void (*osmo_conv_metrics_k5_n4)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); +void (*osmo_conv_metrics_k7_n2)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); +void (*osmo_conv_metrics_k7_n3)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); +void (*osmo_conv_metrics_k7_n4)(const int8_t *seq, + const int16_t *out, int16_t *sums, int16_t *paths, int norm); + +/* Forward malloc wrappers */ +int16_t *osmo_conv_gen_vdec_malloc(size_t n); +void osmo_conv_gen_vdec_free(int16_t *ptr); + +#if defined(HAVE_SSSE3) +int16_t *osmo_conv_sse_vdec_malloc(size_t n); +void osmo_conv_sse_vdec_free(int16_t *ptr); +#endif + +#if defined(HAVE_SSSE3) && defined(HAVE_AVX2) +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); +void osmo_conv_gen_metrics_k5_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_gen_metrics_k5_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_gen_metrics_k7_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_gen_metrics_k7_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_gen_metrics_k7_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); + +#if defined(HAVE_SSSE3) +void osmo_conv_sse_metrics_k5_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_metrics_k5_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_metrics_k5_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_metrics_k7_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_metrics_k7_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_metrics_k7_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +#endif + +#if defined(HAVE_SSSE3) && defined(HAVE_AVX2) +void osmo_conv_sse_avx_metrics_k5_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_avx_metrics_k5_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_avx_metrics_k5_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_avx_metrics_k7_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +void osmo_conv_sse_avx_metrics_k7_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm); +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 + */ +struct vstate { + unsigned state; + unsigned prev[2]; +}; + +/* Trellis Object + * num_states - Number of states in the trellis + * sums - Accumulated path metrics + * outputs - Trellis output values + * vals - Input value that led to each state + */ +struct vtrellis { + int num_states; + int16_t *sums; + int16_t *outputs; + uint8_t *vals; +}; + +/* Viterbi Decoder + * n - Code order + * k - Constraint length + * len - Horizontal length of trellis + * recursive - Set to '1' if the code is recursive + * intrvl - Normalization interval + * trellis - Trellis object + * paths - Trellis paths + */ +struct vdecoder { + int n; + int k; + int len; + int recursive; + int intrvl; + struct vtrellis trellis; + int16_t **paths; + + void (*metric_func)(const int8_t *, const int16_t *, + int16_t *, int16_t *, int); +}; + +/* Accessor calls */ +static inline int conv_code_recursive(const struct osmo_conv_code *code) +{ + return code->next_term_output ? 1 : 0; +} + +/* Left shift and mask for finding the previous state */ +static unsigned vstate_lshift(unsigned reg, int k, int val) +{ + unsigned mask; + + if (k == 5) + mask = 0x0e; + else if (k == 7) + mask = 0x3e; + else + mask = 0; + + return ((reg << 1) & mask) | val; +} + +/* Bit endian manipulators */ +static inline unsigned bitswap2(unsigned v) +{ + return ((v & 0x02) >> 1) | ((v & 0x01) << 1); +} + +static inline unsigned bitswap3(unsigned v) +{ + return ((v & 0x04) >> 2) | ((v & 0x02) >> 0) | + ((v & 0x01) << 2); +} + +static inline unsigned bitswap4(unsigned v) +{ + return ((v & 0x08) >> 3) | ((v & 0x04) >> 1) | + ((v & 0x02) << 1) | ((v & 0x01) << 3); +} + +static inline unsigned bitswap5(unsigned v) +{ + return ((v & 0x10) >> 4) | ((v & 0x08) >> 2) | ((v & 0x04) >> 0) | + ((v & 0x02) << 2) | ((v & 0x01) << 4); +} + +static inline unsigned bitswap6(unsigned v) +{ + return ((v & 0x20) >> 5) | ((v & 0x10) >> 3) | ((v & 0x08) >> 1) | + ((v & 0x04) << 1) | ((v & 0x02) << 3) | ((v & 0x01) << 5); +} + +static unsigned bitswap(unsigned v, unsigned n) +{ + switch (n) { + case 1: + return v; + case 2: + return bitswap2(v); + case 3: + return bitswap3(v); + case 4: + return bitswap4(v); + case 5: + return bitswap5(v); + case 6: + return bitswap6(v); + default: + return 0; + } +} + +/* Generate non-recursive state output from generator state table + * Note that the shift register moves right (i.e. the most recent bit is + * shifted into the register at k-1 bit of the register), which is typical + * textbook representation. The API transition table expects the most recent + * bit in the low order bit, or left shift. A bitswap operation is required + * to accommodate the difference. + */ +static unsigned gen_output(struct vstate *state, int val, + const struct osmo_conv_code *code) +{ + unsigned out, prev; + + prev = bitswap(state->prev[0], code->K - 1); + out = code->next_output[prev][val]; + out = bitswap(out, code->N); + + return out; +} + +/* Populate non-recursive trellis state + * For a given state defined by the k-1 length shift register, find the + * value of the input bit that drove the trellis to that state. Also + * generate the N outputs of the generator polynomial at that state. + */ +static int gen_state_info(uint8_t *val, unsigned reg, + int16_t *output, const struct osmo_conv_code *code) +{ + int i; + unsigned out; + struct vstate state; + + /* Previous '0' state */ + state.state = reg; + state.prev[0] = vstate_lshift(reg, code->K, 0); + state.prev[1] = vstate_lshift(reg, code->K, 1); + + *val = (reg >> (code->K - 2)) & 0x01; + + /* Transition output */ + out = gen_output(&state, *val, code); + + /* Unpack to NRZ */ + for (i = 0; i < code->N; i++) + output[i] = BIT2NRZ(out, i); + + return 0; +} + +/* Generate recursive state output from generator state table */ +static unsigned gen_recursive_output(struct vstate *state, + uint8_t *val, unsigned reg, + const struct osmo_conv_code *code, int pos) +{ + int val0, val1; + unsigned out, prev; + + /* Previous '0' state */ + prev = vstate_lshift(reg, code->K, 0); + prev = bitswap(prev, code->K - 1); + + /* Input value */ + val0 = (reg >> (code->K - 2)) & 0x01; + val1 = (code->next_term_output[prev] >> pos) & 0x01; + *val = val0 == val1 ? 0 : 1; + + /* Wrapper for osmocom state access */ + prev = bitswap(state->prev[0], code->K - 1); + + /* Compute the transition output */ + out = code->next_output[prev][*val]; + out = bitswap(out, code->N); + + return out; +} + +/* Populate recursive trellis state + * The bit position of the systematic bit is not explicitly marked by the + * API, so it must be extracted from the generator table. Otherwise, + * populate the trellis similar to the non-recursive version. + * Non-systematic recursive codes are not supported. + */ +static int gen_recursive_state_info(uint8_t *val, + unsigned reg, int16_t *output, const struct osmo_conv_code *code) +{ + int i, j, pos = -1; + int ns = NUM_STATES(code->K); + unsigned out; + struct vstate state; + + /* Previous '0' and '1' states */ + state.state = reg; + state.prev[0] = vstate_lshift(reg, code->K, 0); + state.prev[1] = vstate_lshift(reg, code->K, 1); + + /* Find recursive bit location */ + for (i = 0; i < code->N; i++) { + for (j = 0; j < ns; j++) { + if ((code->next_output[j][0] >> i) & 0x01) + break; + } + + if (j == ns) { + pos = i; + break; + } + } + + /* Non-systematic recursive code not supported */ + if (pos < 0) + return -EPROTO; + + /* Transition output */ + out = gen_recursive_output(&state, val, reg, code, pos); + + /* Unpack to NRZ */ + for (i = 0; i < code->N; i++) + output[i] = BIT2NRZ(out, i); + + return 0; +} + +/* Release the trellis */ +static void free_trellis(struct vtrellis *trellis) +{ + if (!trellis) + return; + + vdec_free(trellis->outputs); + vdec_free(trellis->sums); + free(trellis->vals); +} + +/* Initialize the trellis object + * Initialization consists of generating the outputs and output value of a + * given state. Due to trellis symmetry and anti-symmetry, only one of the + * transition paths is utilized by the butterfly operation in the forward + * recursion, so only one set of N outputs is required per state variable. + */ +static int generate_trellis(struct vdecoder *dec, + const struct osmo_conv_code *code) +{ + struct vtrellis *trellis = &dec->trellis; + int16_t *outputs; + int i, rc; + + int ns = NUM_STATES(code->K); + int olen = (code->N == 2) ? 2 : 4; + + trellis->num_states = ns; + trellis->sums = vdec_malloc(ns); + trellis->outputs = vdec_malloc(ns * olen); + trellis->vals = (uint8_t *) malloc(ns * sizeof(uint8_t)); + + if (!trellis->sums || !trellis->outputs || !trellis->vals) { + rc = -ENOMEM; + goto fail; + } + + /* Populate the trellis state objects */ + for (i = 0; i < ns; i++) { + outputs = &trellis->outputs[olen * i]; + if (dec->recursive) { + rc = gen_recursive_state_info(&trellis->vals[i], + i, outputs, code); + } else { + rc = gen_state_info(&trellis->vals[i], + i, outputs, code); + } + + if (rc < 0) + goto fail; + + /* Set accumulated path metrics to zero */ + trellis->sums[i] = 0; + } + + /** + * For termination other than tail-biting, initialize the zero state + * as the encoder starting state. Initialize with the maximum + * accumulated sum at length equal to the constraint length. + */ + if (code->term != CONV_TERM_TAIL_BITING) + trellis->sums[0] = INT8_MAX * code->N * code->K; + + return 0; + +fail: + free_trellis(trellis); + return rc; +} + +static void _traceback(struct vdecoder *dec, + unsigned state, uint8_t *out, int len) +{ + int i; + unsigned path; + + for (i = len - 1; i >= 0; i--) { + path = dec->paths[i][state] + 1; + out[i] = dec->trellis.vals[state]; + state = vstate_lshift(state, dec->k, path); + } +} + +static void _traceback_rec(struct vdecoder *dec, + unsigned state, uint8_t *out, int len) +{ + int i; + unsigned path; + + for (i = len - 1; i >= 0; i--) { + path = dec->paths[i][state] + 1; + out[i] = path ^ dec->trellis.vals[state]; + state = vstate_lshift(state, dec->k, path); + } +} + +/* Traceback and generate decoded output + * Find the largest accumulated path metric at the final state except for + * the zero terminated case, where we assume the final state is always zero. + */ +static int traceback(struct vdecoder *dec, uint8_t *out, int term, int len) +{ + int i, j, sum, max = -1; + unsigned path, state = 0, state_scan; + + 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) { + max = sum; + state = i; + } + } + + if (max < 0) + return -EPROTO; + } + + for (i = dec->len - 1; i >= len; i--) { + path = dec->paths[i][state] + 1; + state = vstate_lshift(state, dec->k, path); + } + + if (dec->recursive) + _traceback_rec(dec, state, out, len); + else + _traceback(dec, state, out, len); + + return 0; +} + +/* Release decoder object */ +static void vdec_deinit(struct vdecoder *dec) +{ + if (!dec) + return; + + free_trellis(&dec->trellis); + + if (dec->paths != NULL) { + vdec_free(dec->paths[0]); + free(dec->paths); + } +} + +/* Initialize decoder object with code specific params + * Subtract the constraint length K on the normalization interval to + * accommodate the initialization path metric at state zero. + */ +static int vdec_init(struct vdecoder *dec, const struct osmo_conv_code *code) +{ + int i, ns, rc; + + ns = NUM_STATES(code->K); + + dec->n = code->N; + dec->k = code->K; + dec->recursive = conv_code_recursive(code); + dec->intrvl = INT16_MAX / (dec->n * INT8_MAX) - dec->k; + + 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: + dec->metric_func = osmo_conv_metrics_k5_n3; + break; + case 4: + dec->metric_func = osmo_conv_metrics_k5_n4; + break; + default: + return -EINVAL; + } + } else if (dec->k == 7) { + switch (dec->n) { + case 2: + dec->metric_func = osmo_conv_metrics_k7_n2; + break; + case 3: + dec->metric_func = osmo_conv_metrics_k7_n3; + break; + case 4: + dec->metric_func = osmo_conv_metrics_k7_n4; + break; + default: + return -EINVAL; + } + } else { + return -EINVAL; + } + + if (code->term == CONV_TERM_FLUSH) + dec->len = code->len + code->K - 1; + else + dec->len = code->len; + + rc = generate_trellis(dec, code); + if (rc) + return rc; + + dec->paths = (int16_t **) malloc(sizeof(int16_t *) * dec->len); + if (!dec->paths) + goto enomem; + + dec->paths[0] = vdec_malloc(ns * dec->len); + if (!dec->paths[0]) + goto enomem; + + for (i = 1; i < dec->len; i++) + dec->paths[i] = &dec->paths[0][i * ns]; + + return 0; + +enomem: + vdec_deinit(dec); + return -ENOMEM; +} + +/* Depuncture sequence with nagative value terminated puncturing matrix */ +static int depuncture(const int8_t *in, const int *punc, int8_t *out, int len) +{ + int i, n = 0, m = 0; + + for (i = 0; i < len; i++) { + if (i == punc[n]) { + out[i] = 0; + n++; + continue; + } + + out[i] = in[m++]; + } + + return 0; +} + +/* Forward trellis recursion + * Generate branch metrics and path metrics with a combined function. Only + * accumulated path metric sums and path selections are stored. Normalize on + * the interval specified by the decoder. + */ +static void forward_traverse(struct vdecoder *dec, const int8_t *seq) +{ + int i; + + for (i = 0; i < dec->len; i++) { + dec->metric_func(&seq[dec->n * i], + dec->trellis.outputs, + dec->trellis.sums, + dec->paths[i], + !(i % dec->intrvl)); + } +} + +/* Convolutional decode with a decoder object + * Initial puncturing run if necessary followed by the forward recursion. + * For tail-biting perform a second pass before running the backward + * traceback operation. + */ +static int conv_decode(struct vdecoder *dec, const int8_t *seq, + const int *punc, uint8_t *out, int len, int term) +{ + int8_t depunc[dec->len * dec->n]; + + if (punc) { + depuncture(seq, punc, depunc, dec->len * dec->n); + seq = depunc; + } + + /* Propagate through the trellis with interval normalization */ + forward_traverse(dec, seq); + + if (term == CONV_TERM_TAIL_BITING) + forward_traverse(dec, seq); + + return traceback(dec, out, term, len); +} + +static void osmo_conv_init(void) +{ + init_complete = 1; + +#ifdef HAVE___BUILTIN_CPU_SUPPORTS + /* Detect CPU capabilities */ + #ifdef HAVE_AVX2 + avx2_supported = __builtin_cpu_supports("avx2"); + #endif + + #ifdef HAVE_SSSE3 + ssse3_supported = __builtin_cpu_supports("ssse3"); + #endif + + #ifdef HAVE_SSE4_1 + sse41_supported = __builtin_cpu_supports("sse4.1"); + #endif +#endif + +/** + * Usage of curly braces is mandatory, + * because we use multi-line define. + */ +#if defined(HAVE_SSSE3) && defined(HAVE_AVX2) + if (ssse3_supported && avx2_supported) { + INIT_POINTERS(sse_avx); + } else if (ssse3_supported) { + INIT_POINTERS(sse); + } else { + INIT_POINTERS(gen); + } +#elif defined(HAVE_SSSE3) + if (ssse3_supported) { + INIT_POINTERS(sse); + } else { + INIT_POINTERS(gen); + } +#elif defined(HAVE_NEON) + INIT_POINTERS(neon); +#else + INIT_POINTERS(gen); +#endif +} + +/* All-in-one Viterbi decoding */ +int osmo_conv_decode_acc(const struct osmo_conv_code *code, + const sbit_t *input, ubit_t *output) +{ + int rc; + struct vdecoder dec; + + if (!init_complete) + osmo_conv_init(); + + if ((code->N < 2) || (code->N > 4) || (code->len < 1) || + ((code->K != 5) && (code->K != 7))) + return -EINVAL; + + rc = vdec_init(&dec, code); + if (rc) + return rc; + + rc = conv_decode(&dec, input, code->puncture, + output, code->len, code->term); + + vdec_deinit(&dec); + + return rc; +} diff --git a/src/core/conv_acc_generic.c b/src/core/conv_acc_generic.c new file mode 100644 index 00000000..2257e6a9 --- /dev/null +++ b/src/core/conv_acc_generic.c @@ -0,0 +1,206 @@ +/*! \file conv_acc_generic.c + * Accelerated Viterbi decoder implementation + * for generic architectures without SSE support. */ +/* + * Copyright (C) 2013, 2014 Thomas Tsou <tom@tsou.cc> + * + * 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 <string.h> + +/* Add-Compare-Select (ACS-Butterfly) + * Compute 4 accumulated path metrics and 4 path selections. Note that path + * selections are store as -1 and 0 rather than 0 and 1. This is to match + * the output format of the SSE packed compare instruction 'pmaxuw'. + */ + +static void acs_butterfly(int state, int num_states, + int16_t metric, int16_t *sum, + int16_t *new_sum, int16_t *path) +{ + int state0, state1; + int sum0, sum1, sum2, sum3; + + state0 = *(sum + (2 * state + 0)); + state1 = *(sum + (2 * state + 1)); + + sum0 = state0 + metric; + sum1 = state1 - metric; + sum2 = state0 - metric; + sum3 = state1 + metric; + + if (sum0 >= sum1) { + *new_sum = sum0; + *path = -1; + } else { + *new_sum = sum1; + *path = 0; + } + + if (sum2 >= sum3) { + *(new_sum + num_states / 2) = sum2; + *(path + num_states / 2) = -1; + } else { + *(new_sum + num_states / 2) = sum3; + *(path + num_states / 2) = 0; + } +} + +/* Branch metrics unit N=2 */ +static void gen_branch_metrics_n2(int num_states, const int8_t *seq, + const int16_t *out, int16_t *metrics) +{ + int i; + + for (i = 0; i < num_states / 2; i++) { + metrics[i] = seq[0] * out[2 * i + 0] + + seq[1] * out[2 * i + 1]; + } +} + +/* Branch metrics unit N=3 */ +static void gen_branch_metrics_n3(int num_states, const int8_t *seq, + const int16_t *out, int16_t *metrics) +{ + int i; + + for (i = 0; i < num_states / 2; i++) { + metrics[i] = seq[0] * out[4 * i + 0] + + seq[1] * out[4 * i + 1] + + seq[2] * out[4 * i + 2]; + } +} + +/* Branch metrics unit N=4 */ +static void gen_branch_metrics_n4(int num_states, const int8_t *seq, + const int16_t *out, int16_t *metrics) +{ + int i; + + for (i = 0; i < num_states / 2; i++) { + metrics[i] = seq[0] * out[4 * i + 0] + + seq[1] * out[4 * i + 1] + + seq[2] * out[4 * i + 2] + + seq[3] * out[4 * i + 3]; + } +} + +/* Path metric unit */ +static void gen_path_metrics(int num_states, int16_t *sums, + int16_t *metrics, int16_t *paths, int norm) +{ + int i; + int16_t min; + int16_t new_sums[num_states]; + + for (i = 0; i < num_states / 2; i++) + acs_butterfly(i, num_states, metrics[i], + sums, &new_sums[i], &paths[i]); + + if (norm) { + min = new_sums[0]; + + for (i = 1; i < num_states; i++) + if (new_sums[i] < min) + min = new_sums[i]; + + for (i = 0; i < num_states; i++) + new_sums[i] -= min; + } + + memcpy(sums, new_sums, num_states * sizeof(int16_t)); +} + +/* Not-aligned Memory Allocator */ +__attribute__ ((visibility("hidden"))) +int16_t *osmo_conv_gen_vdec_malloc(size_t n) +{ + return (int16_t *) malloc(sizeof(int16_t) * n); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_vdec_free(int16_t *ptr) +{ + free(ptr); +} + +/* 16-state branch-path metrics units (K=5) */ +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k5_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[8]; + + gen_branch_metrics_n2(16, seq, out, metrics); + gen_path_metrics(16, sums, metrics, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k5_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[8]; + + gen_branch_metrics_n3(16, seq, out, metrics); + gen_path_metrics(16, sums, metrics, paths, norm); + +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k5_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[8]; + + gen_branch_metrics_n4(16, seq, out, metrics); + gen_path_metrics(16, sums, metrics, paths, norm); + +} + +/* 64-state branch-path metrics units (K=7) */ +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k7_n2(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[32]; + + gen_branch_metrics_n2(64, seq, out, metrics); + gen_path_metrics(64, sums, metrics, paths, norm); + +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k7_n3(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[32]; + + gen_branch_metrics_n3(64, seq, out, metrics); + gen_path_metrics(64, sums, metrics, paths, norm); + +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_gen_metrics_k7_n4(const int8_t *seq, const int16_t *out, + int16_t *sums, int16_t *paths, int norm) +{ + int16_t metrics[32]; + + gen_branch_metrics_n4(64, seq, out, metrics); + gen_path_metrics(64, sums, metrics, paths, norm); +} 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/core/conv_acc_sse.c b/src/core/conv_acc_sse.c new file mode 100644 index 00000000..513ab052 --- /dev/null +++ b/src/core/conv_acc_sse.c @@ -0,0 +1,128 @@ +/*! \file conv_acc_sse.c + * Accelerated Viterbi decoder implementation + * for architectures with only SSSE3 available. */ +/* + * Copyright (C) 2013, 2014 Thomas Tsou <tom@tsou.cc> + * + * 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 "config.h" + +#include <emmintrin.h> +#include <tmmintrin.h> +#include <xmmintrin.h> + +#if defined(HAVE_SSE4_1) +#include <smmintrin.h> +#endif + +#define SSE_ALIGN 16 + +/* Broadcast 16-bit integer + * Repeat the low 16-bit integer to all elements of the 128-bit SSE + * register. Only AVX2 has a dedicated broadcast instruction; use repeat + * unpacks for SSE only architectures. This is a destructive operation and + * the source register is overwritten. + * + * Input: + * M0 - Low 16-bit element is read + * + * Output: + * M0 - Contains broadcasted values + */ +#define SSE_BROADCAST(M0) \ +{ \ + M0 = _mm_unpacklo_epi16(M0, M0); \ + M0 = _mm_unpacklo_epi32(M0, M0); \ + M0 = _mm_unpacklo_epi64(M0, M0); \ +} + +/** + * Include common SSE implementation + */ +#include <conv_acc_sse_impl.h> + +/* Aligned Memory Allocator + * SSE requires 16-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_sse_vdec_malloc(size_t n) +{ + return (int16_t *) _mm_malloc(sizeof(int16_t) * n, SSE_ALIGN); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_vdec_free(int16_t *ptr) +{ + _mm_free(ptr); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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] }; + + _sse_metrics_k5_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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 }; + + _sse_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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] }; + + _sse_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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] }; + + _sse_metrics_k7_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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 }; + + _sse_metrics_k7_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_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] }; + + _sse_metrics_k7_n4(_val, out, sums, paths, norm); +} diff --git a/src/core/conv_acc_sse_avx.c b/src/core/conv_acc_sse_avx.c new file mode 100644 index 00000000..82b4fa62 --- /dev/null +++ b/src/core/conv_acc_sse_avx.c @@ -0,0 +1,128 @@ +/*! \file conv_acc_sse_avx.c + * Accelerated Viterbi decoder implementation + * for architectures with both SSSE3 and AVX2 support. */ +/* + * Copyright (C) 2013, 2014 Thomas Tsou <tom@tsou.cc> + * + * 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 "config.h" + +#include <emmintrin.h> +#include <tmmintrin.h> +#include <xmmintrin.h> +#include <immintrin.h> + +#if defined(HAVE_SSE4_1) +#include <smmintrin.h> +#endif + +#define SSE_ALIGN 16 + + +/* Broadcast 16-bit integer + * Repeat the low 16-bit integer to all elements of the 128-bit SSE + * register. Only AVX2 has a dedicated broadcast instruction; use repeat + * unpacks for SSE only architectures. This is a destructive operation and + * the source register is overwritten. + * + * Input: + * M0 - Low 16-bit element is read + * + * Output: + * M0 - Contains broadcasted values + */ +#define SSE_BROADCAST(M0) \ +{ \ + M0 = _mm_broadcastw_epi16(M0); \ +} + +/** + * Include common SSE implementation + */ +#include <conv_acc_sse_impl.h> + +/* Aligned Memory Allocator + * SSE requires 16-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_sse_avx_vdec_malloc(size_t n) +{ + return (int16_t *) _mm_malloc(sizeof(int16_t) * n, SSE_ALIGN); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_vdec_free(int16_t *ptr) +{ + _mm_free(ptr); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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] }; + + _sse_metrics_k5_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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 }; + + _sse_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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] }; + + _sse_metrics_k5_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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] }; + + _sse_metrics_k7_n2(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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 }; + + _sse_metrics_k7_n4(_val, out, sums, paths, norm); +} + +__attribute__ ((visibility("hidden"))) +void osmo_conv_sse_avx_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] }; + + _sse_metrics_k7_n4(_val, out, sums, paths, norm); +} diff --git a/src/core/conv_acc_sse_impl.h b/src/core/conv_acc_sse_impl.h new file mode 100644 index 00000000..807dbe5e --- /dev/null +++ b/src/core/conv_acc_sse_impl.h @@ -0,0 +1,501 @@ +/*! \file conv_acc_sse_impl.h + * Accelerated Viterbi decoder implementation: + * Actual definitions which are being included + * from both conv_acc_sse.c and conv_acc_sse_avx.c. */ +/* + * Copyright (C) 2013, 2014 Thomas Tsou <tom@tsou.cc> + * + * 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 + +extern int sse41_supported; + +/* Octo-Viterbi butterfly + * Compute 8-wide butterfly generating 16 path decisions and 16 accumulated + * sums. Inputs all packed 16-bit integers in three 128-bit XMM registers. + * Two intermediate registers are used and results are set in the upper 4 + * registers. + * + * Input: + * M0 - Path metrics 0 (packed 16-bit integers) + * M1 - Path metrics 1 (packed 16-bit integers) + * M2 - Branch metrics (packed 16-bit integers) + * + * Output: + * M2 - Selected and accumulated path metrics 0 + * M4 - Selected and accumulated path metrics 1 + * M3 - Path selections 0 + * M1 - Path selections 1 + */ +#define SSE_BUTTERFLY(M0, M1, M2, M3, M4) \ +{ \ + M3 = _mm_adds_epi16(M0, M2); \ + M4 = _mm_subs_epi16(M1, M2); \ + M0 = _mm_subs_epi16(M0, M2); \ + M1 = _mm_adds_epi16(M1, M2); \ + M2 = _mm_max_epi16(M3, M4); \ + M3 = _mm_or_si128(_mm_cmpgt_epi16(M3, M4), _mm_cmpeq_epi16(M3, M4)); \ + M4 = _mm_max_epi16(M0, M1); \ + M1 = _mm_or_si128(_mm_cmpgt_epi16(M0, M1), _mm_cmpeq_epi16(M0, M1)); \ +} + +/* Two lane deinterleaving K = 5: + * Take 16 interleaved 16-bit integers and deinterleave to 2 packed 128-bit + * registers. The operation summarized below. Four registers are used with + * the lower 2 as input and upper 2 as output. + * + * In - 10101010 10101010 10101010 10101010 + * Out - 00000000 11111111 00000000 11111111 + * + * Input: + * M0:1 - Packed 16-bit integers + * + * Output: + * M2:3 - Deinterleaved packed 16-bit integers + */ +#define _I8_SHUFFLE_MASK 15, 14, 11, 10, 7, 6, 3, 2, 13, 12, 9, 8, 5, 4, 1, 0 + +#define SSE_DEINTERLEAVE_K5(M0, M1, M2, M3) \ +{ \ + M2 = _mm_set_epi8(_I8_SHUFFLE_MASK); \ + M0 = _mm_shuffle_epi8(M0, M2); \ + M1 = _mm_shuffle_epi8(M1, M2); \ + M2 = _mm_unpacklo_epi64(M0, M1); \ + M3 = _mm_unpackhi_epi64(M0, M1); \ +} + +/* Two lane deinterleaving K = 7: + * Take 64 interleaved 16-bit integers and deinterleave to 8 packed 128-bit + * registers. The operation summarized below. 16 registers are used with the + * lower 8 as input and upper 8 as output. + * + * In - 10101010 10101010 10101010 10101010 ... + * Out - 00000000 11111111 00000000 11111111 ... + * + * Input: + * M0:7 - Packed 16-bit integers + * + * Output: + * M8:15 - Deinterleaved packed 16-bit integers + */ +#define SSE_DEINTERLEAVE_K7(M0, M1, M2, M3, M4, M5, M6, M7, \ + M8, M9, M10, M11, M12, M13, M14, M15) \ +{ \ + M8 = _mm_set_epi8(_I8_SHUFFLE_MASK); \ + M0 = _mm_shuffle_epi8(M0, M8); \ + M1 = _mm_shuffle_epi8(M1, M8); \ + M2 = _mm_shuffle_epi8(M2, M8); \ + M3 = _mm_shuffle_epi8(M3, M8); \ + M4 = _mm_shuffle_epi8(M4, M8); \ + M5 = _mm_shuffle_epi8(M5, M8); \ + M6 = _mm_shuffle_epi8(M6, M8); \ + M7 = _mm_shuffle_epi8(M7, M8); \ + M8 = _mm_unpacklo_epi64(M0, M1); \ + M9 = _mm_unpackhi_epi64(M0, M1); \ + M10 = _mm_unpacklo_epi64(M2, M3); \ + M11 = _mm_unpackhi_epi64(M2, M3); \ + M12 = _mm_unpacklo_epi64(M4, M5); \ + M13 = _mm_unpackhi_epi64(M4, M5); \ + M14 = _mm_unpacklo_epi64(M6, M7); \ + M15 = _mm_unpackhi_epi64(M6, M7); \ +} + +/* Generate branch metrics N = 2: + * Compute 16 branch metrics from trellis outputs and input values. + * + * Input: + * M0:3 - 16 x 2 packed 16-bit trellis outputs + * M4 - Expanded and packed 16-bit input value + * + * Output: + * M6:7 - 16 computed 16-bit branch metrics + */ +#define SSE_BRANCH_METRIC_N2(M0, M1, M2, M3, M4, M6, M7) \ +{ \ + M0 = _mm_sign_epi16(M4, M0); \ + M1 = _mm_sign_epi16(M4, M1); \ + M2 = _mm_sign_epi16(M4, M2); \ + M3 = _mm_sign_epi16(M4, M3); \ + M6 = _mm_hadds_epi16(M0, M1); \ + M7 = _mm_hadds_epi16(M2, M3); \ +} + +/* Generate branch metrics N = 4: + * Compute 8 branch metrics from trellis outputs and input values. This + * macro is reused for N less than 4 where the extra soft input bits are + * padded. + * + * Input: + * M0:3 - 8 x 4 packed 16-bit trellis outputs + * M4 - Expanded and packed 16-bit input value + * + * Output: + * M5 - 8 computed 16-bit branch metrics + */ +#define SSE_BRANCH_METRIC_N4(M0, M1, M2, M3, M4, M5) \ +{ \ + M0 = _mm_sign_epi16(M4, M0); \ + M1 = _mm_sign_epi16(M4, M1); \ + M2 = _mm_sign_epi16(M4, M2); \ + M3 = _mm_sign_epi16(M4, M3); \ + M0 = _mm_hadds_epi16(M0, M1); \ + M1 = _mm_hadds_epi16(M2, M3); \ + M5 = _mm_hadds_epi16(M0, M1); \ +} + +/* Horizontal minimum + * Compute horizontal minimum of packed unsigned 16-bit integers and place + * result in the low 16-bit element of the source register. Only SSE 4.1 + * has a dedicated minpos instruction. One intermediate register is used + * if SSE 4.1 is not available. This is a destructive operation and the + * source register is overwritten. + * + * Input: + * M0 - Packed unsigned 16-bit integers + * + * Output: + * M0 - Minimum value placed in low 16-bit element + */ +#if defined(HAVE_SSE4_1) || defined(HAVE_SSE41) +#define SSE_MINPOS(M0, M1) \ +{ \ + if (sse41_supported) { \ + M0 = _mm_minpos_epu16(M0); \ + } else { \ + M1 = _mm_shuffle_epi32(M0, _MM_SHUFFLE(0, 0, 3, 2)); \ + M0 = _mm_min_epi16(M0, M1); \ + M1 = _mm_shufflelo_epi16(M0, _MM_SHUFFLE(0, 0, 3, 2)); \ + M0 = _mm_min_epi16(M0, M1); \ + M1 = _mm_shufflelo_epi16(M0, _MM_SHUFFLE(0, 0, 0, 1)); \ + M0 = _mm_min_epi16(M0, M1); \ + } \ +} +#else +#define SSE_MINPOS(M0, M1) \ +{ \ + M1 = _mm_shuffle_epi32(M0, _MM_SHUFFLE(0, 0, 3, 2)); \ + M0 = _mm_min_epi16(M0, M1); \ + M1 = _mm_shufflelo_epi16(M0, _MM_SHUFFLE(0, 0, 3, 2)); \ + M0 = _mm_min_epi16(M0, M1); \ + M1 = _mm_shufflelo_epi16(M0, _MM_SHUFFLE(0, 0, 0, 1)); \ + M0 = _mm_min_epi16(M0, M1); \ +} +#endif + +/* Normalize state metrics K = 5: + * Compute 16-wide normalization by subtracting the smallest value from + * all values. Inputs are 16 packed 16-bit integers across 2 XMM registers. + * Two intermediate registers are used and normalized results are placed + * in the originating locations. + * + * Input: + * M0:1 - Path metrics 0:1 (packed 16-bit integers) + * + * Output: + * M0:1 - Normalized path metrics 0:1 + */ +#define SSE_NORMALIZE_K5(M0, M1, M2, M3) \ +{ \ + M2 = _mm_min_epi16(M0, M1); \ + SSE_MINPOS(M2, M3) \ + SSE_BROADCAST(M2) \ + M0 = _mm_subs_epi16(M0, M2); \ + M1 = _mm_subs_epi16(M1, M2); \ +} + +/* Normalize state metrics K = 7: + * Compute 64-wide normalization by subtracting the smallest value from + * all values. Inputs are 8 registers of accumulated sums and 4 temporary + * registers. Normalized results are returned in the originating locations. + * + * Input: + * M0:7 - Path metrics 0:7 (packed 16-bit integers) + * + * Output: + * M0:7 - Normalized path metrics 0:7 + */ +#define SSE_NORMALIZE_K7(M0, M1, M2, M3, M4, M5, M6, M7, M8, M9, M10, M11) \ +{ \ + M8 = _mm_min_epi16(M0, M1); \ + M9 = _mm_min_epi16(M2, M3); \ + M10 = _mm_min_epi16(M4, M5); \ + M11 = _mm_min_epi16(M6, M7); \ + M8 = _mm_min_epi16(M8, M9); \ + M10 = _mm_min_epi16(M10, M11); \ + M8 = _mm_min_epi16(M8, M10); \ + SSE_MINPOS(M8, M9) \ + SSE_BROADCAST(M8) \ + M0 = _mm_subs_epi16(M0, M8); \ + M1 = _mm_subs_epi16(M1, M8); \ + M2 = _mm_subs_epi16(M2, M8); \ + M3 = _mm_subs_epi16(M3, M8); \ + M4 = _mm_subs_epi16(M4, M8); \ + M5 = _mm_subs_epi16(M5, M8); \ + M6 = _mm_subs_epi16(M6, M8); \ + M7 = _mm_subs_epi16(M7, M8); \ +} + +/* Combined BMU/PMU (K=5, N=2) + * Compute branch metrics followed by path metrics for half rate 16-state + * trellis. 8 butterflies are computed. Accumulated path sums are not + * preserved and read and written into the same memory location. Normalize + * sums if requires. + */ +__always_inline static void _sse_metrics_k5_n2(const int16_t *val, + const int16_t *out, int16_t *sums, int16_t *paths, int norm) +{ + __m128i m0, m1, m2, m3, m4, m5, m6; + + /* (BMU) Load input sequence */ + m2 = _mm_castpd_si128(_mm_loaddup_pd((double const *) val)); + + /* (BMU) Load trellis outputs */ + m0 = _mm_load_si128((__m128i *) &out[0]); + m1 = _mm_load_si128((__m128i *) &out[8]); + + /* (BMU) Compute branch metrics */ + m0 = _mm_sign_epi16(m2, m0); + m1 = _mm_sign_epi16(m2, m1); + m2 = _mm_hadds_epi16(m0, m1); + + /* (PMU) Load accumulated path metrics */ + m0 = _mm_load_si128((__m128i *) &sums[0]); + m1 = _mm_load_si128((__m128i *) &sums[8]); + + SSE_DEINTERLEAVE_K5(m0, m1, m3, m4) + + /* (PMU) Butterflies: 0-7 */ + SSE_BUTTERFLY(m3, m4, m2, m5, m6) + + if (norm) + SSE_NORMALIZE_K5(m2, m6, m0, m1) + + _mm_store_si128((__m128i *) &sums[0], m2); + _mm_store_si128((__m128i *) &sums[8], m6); + _mm_store_si128((__m128i *) &paths[0], m5); + _mm_store_si128((__m128i *) &paths[8], m4); +} + +/* Combined BMU/PMU (K=5, N=3 and N=4) + * Compute branch metrics followed by path metrics for 16-state and rates + * to 1/4. 8 butterflies are computed. The input sequence is read four 16-bit + * values at a time, and extra values should be set to zero for rates other + * than 1/4. Normally only rates 1/3 and 1/4 are used as there is a + * dedicated implementation of rate 1/2. + */ +__always_inline static void _sse_metrics_k5_n4(const int16_t *val, + const int16_t *out, int16_t *sums, int16_t *paths, int norm) +{ + __m128i m0, m1, m2, m3, m4, m5, m6; + + /* (BMU) Load input sequence */ + m4 = _mm_castpd_si128(_mm_loaddup_pd((double const *) val)); + + /* (BMU) Load trellis outputs */ + m0 = _mm_load_si128((__m128i *) &out[0]); + m1 = _mm_load_si128((__m128i *) &out[8]); + m2 = _mm_load_si128((__m128i *) &out[16]); + m3 = _mm_load_si128((__m128i *) &out[24]); + + SSE_BRANCH_METRIC_N4(m0, m1, m2, m3, m4, m2) + + /* (PMU) Load accumulated path metrics */ + m0 = _mm_load_si128((__m128i *) &sums[0]); + m1 = _mm_load_si128((__m128i *) &sums[8]); + + SSE_DEINTERLEAVE_K5(m0, m1, m3, m4) + + /* (PMU) Butterflies: 0-7 */ + SSE_BUTTERFLY(m3, m4, m2, m5, m6) + + if (norm) + SSE_NORMALIZE_K5(m2, m6, m0, m1) + + _mm_store_si128((__m128i *) &sums[0], m2); + _mm_store_si128((__m128i *) &sums[8], m6); + _mm_store_si128((__m128i *) &paths[0], m5); + _mm_store_si128((__m128i *) &paths[8], m4); +} + +/* Combined BMU/PMU (K=7, N=2) + * Compute branch metrics followed by path metrics for half rate 64-state + * trellis. 32 butterfly operations are computed. Deinterleaving path + * metrics requires usage of the full SSE register file, so separate sums + * before computing branch metrics to avoid register spilling. + */ +__always_inline static void _sse_metrics_k7_n2(const int16_t *val, + const int16_t *out, int16_t *sums, int16_t *paths, int norm) +{ + __m128i m0, m1, m2, m3, m4, m5, m6, m7, m8, + m9, m10, m11, m12, m13, m14, m15; + + /* (PMU) Load accumulated path metrics */ + m0 = _mm_load_si128((__m128i *) &sums[0]); + m1 = _mm_load_si128((__m128i *) &sums[8]); + m2 = _mm_load_si128((__m128i *) &sums[16]); + m3 = _mm_load_si128((__m128i *) &sums[24]); + m4 = _mm_load_si128((__m128i *) &sums[32]); + m5 = _mm_load_si128((__m128i *) &sums[40]); + m6 = _mm_load_si128((__m128i *) &sums[48]); + m7 = _mm_load_si128((__m128i *) &sums[56]); + + /* (PMU) Deinterleave to even-odd registers */ + SSE_DEINTERLEAVE_K7(m0, m1, m2, m3 ,m4 ,m5, m6, m7, + m8, m9, m10, m11, m12, m13, m14, m15) + + /* (BMU) Load input symbols */ + m7 = _mm_castpd_si128(_mm_loaddup_pd((double const *) val)); + + /* (BMU) Load trellis outputs */ + m0 = _mm_load_si128((__m128i *) &out[0]); + m1 = _mm_load_si128((__m128i *) &out[8]); + m2 = _mm_load_si128((__m128i *) &out[16]); + m3 = _mm_load_si128((__m128i *) &out[24]); + + SSE_BRANCH_METRIC_N2(m0, m1, m2, m3, m7, m4, m5) + + m0 = _mm_load_si128((__m128i *) &out[32]); + m1 = _mm_load_si128((__m128i *) &out[40]); + m2 = _mm_load_si128((__m128i *) &out[48]); + m3 = _mm_load_si128((__m128i *) &out[56]); + + SSE_BRANCH_METRIC_N2(m0, m1, m2, m3, m7, m6, m7) + + /* (PMU) Butterflies: 0-15 */ + SSE_BUTTERFLY(m8, m9, m4, m0, m1) + SSE_BUTTERFLY(m10, m11, m5, m2, m3) + + _mm_store_si128((__m128i *) &paths[0], m0); + _mm_store_si128((__m128i *) &paths[8], m2); + _mm_store_si128((__m128i *) &paths[32], m9); + _mm_store_si128((__m128i *) &paths[40], m11); + + /* (PMU) Butterflies: 17-31 */ + SSE_BUTTERFLY(m12, m13, m6, m0, m2) + SSE_BUTTERFLY(m14, m15, m7, m9, m11) + + _mm_store_si128((__m128i *) &paths[16], m0); + _mm_store_si128((__m128i *) &paths[24], m9); + _mm_store_si128((__m128i *) &paths[48], m13); + _mm_store_si128((__m128i *) &paths[56], m15); + + if (norm) + SSE_NORMALIZE_K7(m4, m1, m5, m3, m6, m2, + m7, m11, m0, m8, m9, m10) + + _mm_store_si128((__m128i *) &sums[0], m4); + _mm_store_si128((__m128i *) &sums[8], m5); + _mm_store_si128((__m128i *) &sums[16], m6); + _mm_store_si128((__m128i *) &sums[24], m7); + _mm_store_si128((__m128i *) &sums[32], m1); + _mm_store_si128((__m128i *) &sums[40], m3); + _mm_store_si128((__m128i *) &sums[48], m2); + _mm_store_si128((__m128i *) &sums[56], m11); +} + +/* Combined BMU/PMU (K=7, N=3 and N=4) + * Compute branch metrics followed by path metrics for half rate 64-state + * trellis. 32 butterfly operations are computed. Deinterleave path + * metrics before computing branch metrics as in the half rate case. + */ +__always_inline static void _sse_metrics_k7_n4(const int16_t *val, + const int16_t *out, int16_t *sums, int16_t *paths, int norm) +{ + __m128i m0, m1, m2, m3, m4, m5, m6, m7; + __m128i m8, m9, m10, m11, m12, m13, m14, m15; + + /* (PMU) Load accumulated path metrics */ + m0 = _mm_load_si128((__m128i *) &sums[0]); + m1 = _mm_load_si128((__m128i *) &sums[8]); + m2 = _mm_load_si128((__m128i *) &sums[16]); + m3 = _mm_load_si128((__m128i *) &sums[24]); + m4 = _mm_load_si128((__m128i *) &sums[32]); + m5 = _mm_load_si128((__m128i *) &sums[40]); + m6 = _mm_load_si128((__m128i *) &sums[48]); + m7 = _mm_load_si128((__m128i *) &sums[56]); + + /* (PMU) Deinterleave into even and odd packed registers */ + SSE_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 */ + m7 = _mm_castpd_si128(_mm_loaddup_pd((double const *) val)); + + /* (BMU) Load and compute branch metrics */ + m0 = _mm_load_si128((__m128i *) &out[0]); + m1 = _mm_load_si128((__m128i *) &out[8]); + m2 = _mm_load_si128((__m128i *) &out[16]); + m3 = _mm_load_si128((__m128i *) &out[24]); + + SSE_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m4) + + m0 = _mm_load_si128((__m128i *) &out[32]); + m1 = _mm_load_si128((__m128i *) &out[40]); + m2 = _mm_load_si128((__m128i *) &out[48]); + m3 = _mm_load_si128((__m128i *) &out[56]); + + SSE_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m5) + + m0 = _mm_load_si128((__m128i *) &out[64]); + m1 = _mm_load_si128((__m128i *) &out[72]); + m2 = _mm_load_si128((__m128i *) &out[80]); + m3 = _mm_load_si128((__m128i *) &out[88]); + + SSE_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m6) + + m0 = _mm_load_si128((__m128i *) &out[96]); + m1 = _mm_load_si128((__m128i *) &out[104]); + m2 = _mm_load_si128((__m128i *) &out[112]); + m3 = _mm_load_si128((__m128i *) &out[120]); + + SSE_BRANCH_METRIC_N4(m0, m1, m2, m3, m7, m7) + + /* (PMU) Butterflies: 0-15 */ + SSE_BUTTERFLY(m8, m9, m4, m0, m1) + SSE_BUTTERFLY(m10, m11, m5, m2, m3) + + _mm_store_si128((__m128i *) &paths[0], m0); + _mm_store_si128((__m128i *) &paths[8], m2); + _mm_store_si128((__m128i *) &paths[32], m9); + _mm_store_si128((__m128i *) &paths[40], m11); + + /* (PMU) Butterflies: 17-31 */ + SSE_BUTTERFLY(m12, m13, m6, m0, m2) + SSE_BUTTERFLY(m14, m15, m7, m9, m11) + + _mm_store_si128((__m128i *) &paths[16], m0); + _mm_store_si128((__m128i *) &paths[24], m9); + _mm_store_si128((__m128i *) &paths[48], m13); + _mm_store_si128((__m128i *) &paths[56], m15); + + if (norm) + SSE_NORMALIZE_K7(m4, m1, m5, m3, m6, m2, + m7, m11, m0, m8, m9, m10) + + _mm_store_si128((__m128i *) &sums[0], m4); + _mm_store_si128((__m128i *) &sums[8], m5); + _mm_store_si128((__m128i *) &sums[16], m6); + _mm_store_si128((__m128i *) &sums[24], m7); + _mm_store_si128((__m128i *) &sums[32], m1); + _mm_store_si128((__m128i *) &sums[40], m3); + _mm_store_si128((__m128i *) &sums[48], m2); + _mm_store_si128((__m128i *) &sums[56], m11); +} diff --git a/src/core/counter.c b/src/core/counter.c new file mode 100644 index 00000000..dace15f3 --- /dev/null +++ b/src/core/counter.c @@ -0,0 +1,108 @@ +/*! \file counter.c + * utility routines for keeping some statistics. */ +/* + * (C) 2009 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 <string.h> + +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/counter.h> + +static LLIST_HEAD(counters); + +/*! Global talloc context for all osmo_counter allocations. */ +void *tall_ctr_ctx; + +/*! Allocate a new counter with given name. Allocates from tall_ctr_ctx + * \param[in] name Human-readable string name for the counter + * \returns Allocated counter on success; NULL on error */ +struct osmo_counter *osmo_counter_alloc(const char *name) +{ + struct osmo_counter *ctr = talloc_zero(tall_ctr_ctx, struct osmo_counter); + + if (!ctr) + return NULL; + + ctr->name = name; + llist_add_tail(&ctr->list, &counters); + + return ctr; +} + +/*! Release/Destroy a given counter + * \param[in] ctr Counter to be destroyed */ +void osmo_counter_free(struct osmo_counter *ctr) +{ + llist_del(&ctr->list); + talloc_free(ctr); +} + +/*! Iterate over all counters; call \a handle_cunter call-back for each. + * \param[in] handle_counter Call-back to be called for each counter; aborts if rc < 0 + * \param[in] data Opaque data passed through to \a handle_counter function + * \returns 0 if all \a handle_counter calls successfull; negative on error */ +int osmo_counters_for_each(int (*handle_counter)(struct osmo_counter *, void *), + void *data) +{ + struct osmo_counter *ctr; + int rc = 0; + + llist_for_each_entry(ctr, &counters, list) { + rc = handle_counter(ctr, data); + if (rc < 0) + return rc; + } + + return rc; +} + +/*! Counts the registered counter + * \returns amount of counters */ +int osmo_counters_count(void) +{ + return llist_count(&counters); +} + +/*! Find a counter by its name. + * \param[in] name Name used to look-up/search counter + * \returns Counter on success; NULL if not found */ +struct osmo_counter *osmo_counter_get_by_name(const char *name) +{ + struct osmo_counter *ctr; + + llist_for_each_entry(ctr, &counters, list) { + if (!strcmp(ctr->name, name)) + return ctr; + } + return NULL; +} + +/*! Compute difference between current and previous counter value. + * \param[in] ctr Counter of which the difference is to be computed + * \returns Delta value between current counter and previous counter. Please + * note that the actual counter values are unsigned long, while the + * difference is computed as signed integer! */ +int osmo_counter_difference(struct osmo_counter *ctr) +{ + int delta = ctr->value - ctr->previous; + ctr->previous = ctr->value; + + return delta; +} diff --git a/src/core/crc16.c b/src/core/crc16.c new file mode 100644 index 00000000..29dace2e --- /dev/null +++ b/src/core/crc16.c @@ -0,0 +1,115 @@ +/*! \addtogroup crc + * @{ + * \file crc16.c + * This was copied from the linux kernel and adjusted for our types. + */ +/* + * crc16.c + * + * This source code is licensed under the GNU General Public License, + * Version 2. See the file COPYING for more details. + * + * SPDX-License-Identifier: GPL-2.0 + */ + +#include <osmocom/core/crc16.h> + +/*! CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ +uint16_t const osmo_crc16_table[256] = { + 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, + 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, + 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, + 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, + 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, + 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, + 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, + 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, + 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, + 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, + 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, + 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, + 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, + 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, + 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, + 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, + 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, + 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, + 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, + 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, + 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, + 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, + 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, + 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, + 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, + 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, + 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, + 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, + 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, + 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, + 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, + 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 +}; + +/*! Compute 16bit CCITT polynome 0x8408 (x^0 + x^5 + x^12) over given buffer. + * \param crc[in] previous CRC value + * \param buffer[in] data pointer + * \param len[in] number of bytes in input \ref buffer + * \return updated CRC value + */ +uint16_t osmo_crc16(uint16_t crc, uint8_t const *buffer, size_t len) +{ + while (len--) + crc = osmo_crc16_byte(crc, *buffer++); + return crc; +} + +/*! CRC table for the CCITT CRC-6. The poly is 0x8408 (x^0 + x^5 + x^12) */ +uint16_t const osmo_crc16_ccitt_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + + +/*! Compute 16bit CCITT polynome 0x8408 (x^0 + x^5 + x^12) over given buffer. + * \param[in] crc initial value of CRC + * \param[in] buffer pointer to buffer of input data + * \param[in] len length of \a buffer in bytes + * \returns 16bit CRC */ +uint16_t osmo_crc16_ccitt(uint16_t crc, uint8_t const *buffer, size_t len) +{ + while (len--) + crc = osmo_crc16_ccitt_byte(crc, *buffer++); + return crc; +} + +/*! @} */ diff --git a/src/core/crcXXgen.c.tpl b/src/core/crcXXgen.c.tpl new file mode 100644 index 00000000..154291cc --- /dev/null +++ b/src/core/crcXXgen.c.tpl @@ -0,0 +1,114 @@ +/*! \file crcXXgen.c + * Osmocom generic CRC routines (for max XX bits poly). */ +/* + * Copyright (C) 2011 Sylvain Munaut <tnt@246tNt.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 crc + * @{ + * Osmocom generic CRC routines (for max XX bits poly). + * + * \file crcXXgen.c.tpl */ + +#include <stdint.h> + +#include <osmocom/core/bits.h> +#include <osmocom/core/crcXXgen.h> + + +/*! Compute the CRC value of a given array of hard-bits + * \param[in] code The CRC code description to apply + * \param[in] in Array of hard bits + * \param[in] len Length of the array of hard bits + * \returns The CRC value + */ +uintXX_t +osmo_crcXXgen_compute_bits(const struct osmo_crcXXgen_code *code, + const ubit_t *in, int len) +{ + const uintXX_t poly = code->poly; + uintXX_t crc = code->init; + int i, n = code->bits-1; + + for (i=0; i<len; i++) { + uintXX_t bit = in[i] & 1; + crc ^= (bit << n); + if (crc & ((uintXX_t)1 << n)) { + crc <<= 1; + crc ^= poly; + } else { + crc <<= 1; + } + crc &= ((uintXX_t)1 << code->bits) - 1; + } + + crc ^= code->remainder; + + return crc; +} + + +/*! Checks the CRC value of a given array of hard-bits + * \param[in] code The CRC code description to apply + * \param[in] in Array of hard bits + * \param[in] len Length of the array of hard bits + * \param[in] crc_bits Array of hard bits with the alleged CRC + * \returns 0 if CRC matches. 1 in case of error. + * + * The crc_bits array must have a length of code->len + */ +int +osmo_crcXXgen_check_bits(const struct osmo_crcXXgen_code *code, + const ubit_t *in, int len, const ubit_t *crc_bits) +{ + uintXX_t crc; + int i; + + crc = osmo_crcXXgen_compute_bits(code, in, len); + + for (i=0; i<code->bits; i++) + if (crc_bits[i] ^ ((crc >> (code->bits-i-1)) & 1)) + return 1; + + return 0; +} + + +/*! Computes and writes the CRC value of a given array of bits + * \param[in] code The CRC code description to apply + * \param[in] in Array of hard bits + * \param[in] len Length of the array of hard bits + * \param[in] crc_bits Array of hard bits to write the computed CRC to + * + * The crc_bits array must have a length of code->len + */ +void +osmo_crcXXgen_set_bits(const struct osmo_crcXXgen_code *code, + const ubit_t *in, int len, ubit_t *crc_bits) +{ + uintXX_t crc; + int i; + + crc = osmo_crcXXgen_compute_bits(code, in, len); + + for (i=0; i<code->bits; i++) + crc_bits[i] = ((crc >> (code->bits-i-1)) & 1); +} + +/*! @} */ + +/* vim: set syntax=c: */ 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/core/fsm.c b/src/core/fsm.c new file mode 100644 index 00000000..9333cac5 --- /dev/null +++ b/src/core/fsm.c @@ -0,0 +1,1046 @@ +/*! \file fsm.c + * Osmocom generic Finite State Machine implementation. */ +/* + * (C) 2016-2019 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 <stdbool.h> +#include <string.h> +#include <inttypes.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/utils.h> + +/*! \addtogroup fsm + * @{ + * Finite State Machine abstraction + * + * This is a generic C-language abstraction for implementing finite + * state machines within the Osmocom framework. It is intended to + * replace existing hand-coded or even only implicitly existing FSMs + * all over the existing code base. + * + * An libosmocore FSM is described by its \ref osmo_fsm description, + * which in turn refers to an array of \ref osmo_fsm_state descriptor, + * each describing a single state in the FSM. + * + * The general idea is that all actions performed within one state are + * located at one position in the code (the state's action function), + * as opposed to the 'message-centric' view of e.g. the existing + * state machines of the LAPD(m) core, where there is one message for + * each possible event (primitive), and the function then needs to + * concern itself on how to handle that event over all possible states. + * + * For each state, there is a bit-mask of permitted input events for + * this state, as well as a bit-mask of permitted new output states to + * which the state can change. Furthermore, there is a function + * pointer implementing the actual handling of the input events + * occurring whilst in that state. + * + * Furthermore, each state offers a function pointer that can be + * executed just before leaving a state, and another one just after + * entering a state. + * + * When transitioning into a new state, an optional timer number and + * time-out can be passed along. The timer is started just after + * entering the new state, and will call the \ref osmo_fsm timer_cb + * function once it expires. This is intended to be used in telecom + * state machines where a given timer (identified by a certain number) + * is started to terminate the fsm or terminate the fsm once expected + * events are not happening before timeout expiration. + * + * As there can often be many concurrent FSMs of one given class, we + * introduce the concept of \ref osmo_fsm_inst, i.e. an FSM instance. + * The instance keeps the actual state, while the \ref osmo_fsm + * descriptor contains the static/const descriptor of the FSM's states + * and possible transitions. + * + * osmo_fsm are integrated with the libosmocore logging system. The + * logging sub-system is determined by the FSM descriptor, as we assume + * one FSM (let's say one related to a location update procedure) is + * inevitably always tied to a sub-system. The logging level however + * is configurable for each FSM instance, to ensure that e.g. DEBUG + * logging can be used for the LU procedure of one subscriber, while + * NOTICE level is used for all other subscribers. + * + * In order to attach private state to the \ref osmo_fsm_inst, it + * offers an opaque private pointer. + * + * \file fsm.c */ + +LLIST_HEAD(osmo_g_fsms); +static bool fsm_log_addr = true; +static bool fsm_log_timeouts = false; +/*! See osmo_fsm_term_safely(). */ +static bool fsm_term_safely_enabled = false; + +/*! Internal state for FSM instance termination cascades. */ +static __thread struct { + /*! The first FSM instance that invoked osmo_fsm_inst_term() in the current cascade. */ + struct osmo_fsm_inst *root_fi; + /*! 2 if a secondary FSM terminates, 3 if a secondary FSM causes a tertiary FSM to terminate, and so on. */ + unsigned int depth; + /*! Talloc context to collect all deferred deallocations (FSM instances, and talloc objects if any). */ + void *collect_ctx; + /*! See osmo_fsm_set_dealloc_ctx() */ + void *fsm_dealloc_ctx; +} fsm_term_safely; + +/*! Internal call to free an FSM instance, which redirects to the context set by osmo_fsm_set_dealloc_ctx() if any. + */ +static void fsm_free_or_steal(void *talloc_object) +{ + if (fsm_term_safely.fsm_dealloc_ctx) + talloc_steal(fsm_term_safely.fsm_dealloc_ctx, talloc_object); + else + talloc_free(talloc_object); +} + +/*! specify if FSM instance addresses should be logged or not + * + * By default, the FSM name includes the pointer address of the \ref + * osmo_fsm_inst. This behavior can be disabled (and re-enabled) + * using this function. + * + * \param[in] log_addr Indicate if FSM instance address shall be logged + */ +void osmo_fsm_log_addr(bool log_addr) +{ + fsm_log_addr = log_addr; +} + +/*! Enable or disable logging of timeout values for FSM instance state changes. + * + * By default, state changes are logged by state name only, omitting the timeout. When passing true, each state change + * will also log the T number (or Osmocom-specific X number) and the chosen timeout in seconds. + * osmo_fsm_inst_state_chg_keep_timer() will log remaining timeout in millisecond precision. + * + * The default for this is false to reflect legacy behavior. Since various C tests that verify logging output already + * existed prior to this option, keeping timeout logging off makes sure that they continue to pass. Particularly, + * osmo_fsm_inst_state_chg_keep_timer() may cause non-deterministic logging of remaining timeout values. + * + * For any program that does not explicitly require deterministic logging output, i.e. anything besides regression tests + * involving FSM instances, it is recommended to call osmo_fsm_log_timeouts(true). + * + * \param[in] log_timeouts Pass true to log timeouts on state transitions, false to omit timeouts. + */ +void osmo_fsm_log_timeouts(bool log_timeouts) +{ + fsm_log_timeouts = log_timeouts; +} + +/*! Enable safer way to deallocate cascades of terminating FSM instances. + * + * Note, using osmo_fsm_set_dealloc_ctx() is a more general solution to this same problem. + * Particularly, in a program using osmo_select_main_ctx(), the simplest solution to avoid most use-after-free problems + * from FSM instance deallocation is using osmo_fsm_set_dealloc_ctx(OTC_SELECT). + * + * When enabled, an FSM instance termination detects whether another FSM instance is already terminating, and instead of + * deallocating immediately, collects all terminating FSM instances in a talloc context, to be bulk deallocated once all + * event handling and termination cascades are done. + * + * For example, if an FSM's cleanup() sends an event to some "other" FSM, which in turn causes the FSM's parent to + * deallocate, then the parent would talloc_free() the child's memory, causing a use-after-free. There are infinite + * constellations like this, which all are trivially solved with this feature enabled. + * + * For illustration, see fsm_dealloc_test.c. + * + * When enabled, this feature changes the order of logging, which may break legacy unit test expectations, and changes + * the order of deallocation to after the parent term event is dispatched. + * + * \param[in] term_safely Pass true to switch to safer FSM instance termination behavior. + */ +void osmo_fsm_term_safely(bool term_safely) +{ + fsm_term_safely_enabled = term_safely; +} + +/*! Instead of deallocating FSM instances, move them to the given talloc context. + * + * It is the caller's responsibility to clear this context to actually free the memory of terminated FSM instances. + * Make sure to not talloc_free(ctx) itself before setting a different osmo_fsm_set_dealloc_ctx(). To clear a ctx + * without the need to call osmo_fsm_set_dealloc_ctx() again, rather use talloc_free_children(ctx). + * + * For example, to defer deallocation to the next osmo_select_main_ctx() iteration, set this to OTC_SELECT. + * + * Deferring deallocation is the simplest solution to avoid most use-after-free problems from FSM instance deallocation. + * This is a simpler and more general solution than osmo_fsm_term_safely(). + * + * To disable the feature again, pass NULL as ctx. + * + * Both osmo_fsm_term_safely() and osmo_fsm_set_dealloc_ctx() can be enabled at the same time, which will result in + * first collecting deallocated FSM instances in fsm_term_safely.collect_ctx, and finally reparenting that to the ctx + * passed here. However, in practice, it does not really make sense to enable both at the same time. + * + * \param ctx[in] Instead of talloc_free()int, talloc_steal() all future deallocated osmo_fsm_inst instances to this + * ctx. If NULL, go back to talloc_free() as usual. + */ +void osmo_fsm_set_dealloc_ctx(void *ctx) +{ + fsm_term_safely.fsm_dealloc_ctx = ctx; +} + +/*! talloc_free() the given object immediately, or once ongoing FSM terminations are done. + * + * If an FSM deallocation cascade is ongoing, talloc_steal() the given talloc_object into the talloc context that is + * freed once the cascade is done. If no FSM deallocation cascade is ongoing, or if osmo_fsm_term_safely() is disabled, + * immediately talloc_free the object. + * + * This can be useful if some higher order talloc object, which is the talloc parent for FSM instances or their priv + * objects, is not itself tied to an FSM instance. This function allows safely freeing it without affecting ongoing FSM + * termination cascades. + * + * Once passed to this function, the talloc_object should be considered as already freed. Only FSM instance pre_term() + * and cleanup() functions as well as event handling caused by these may safely assume that it is still valid memory. + * + * The talloc_object should not have multiple parents. + * + * (This function may some day move to public API, which might be redundant if we introduce a select-loop volatile + * context mechanism to defer deallocation instead.) + * + * \param[in] talloc_object Object pointer to free. + */ +static void osmo_fsm_defer_free(void *talloc_object) +{ + if (!fsm_term_safely.depth) { + fsm_free_or_steal(talloc_object); + return; + } + + if (!fsm_term_safely.collect_ctx) { + /* This is actually the first other object / FSM instance besides the root terminating inst. Create the + * ctx to collect this and possibly more objects to free. Avoid talloc parent loops: don't make this ctx + * the child of the root inst or anything like that. */ + fsm_term_safely.collect_ctx = talloc_named_const(NULL, 0, "fsm_term_safely.collect_ctx"); + OSMO_ASSERT(fsm_term_safely.collect_ctx); + } + talloc_steal(fsm_term_safely.collect_ctx, talloc_object); +} + +struct osmo_fsm *osmo_fsm_find_by_name(const char *name) +{ + struct osmo_fsm *fsm; + llist_for_each_entry(fsm, &osmo_g_fsms, list) { + if (!strcmp(name, fsm->name)) + return fsm; + } + return NULL; +} + +struct osmo_fsm_inst *osmo_fsm_inst_find_by_name(const struct osmo_fsm *fsm, + const char *name) +{ + struct osmo_fsm_inst *fi; + + if (!name) + return NULL; + + llist_for_each_entry(fi, &fsm->instances, list) { + if (!fi->name) + continue; + if (!strcmp(name, fi->name)) + return fi; + } + return NULL; +} + +struct osmo_fsm_inst *osmo_fsm_inst_find_by_id(const struct osmo_fsm *fsm, + const char *id) +{ + struct osmo_fsm_inst *fi; + + llist_for_each_entry(fi, &fsm->instances, list) { + if (!strcmp(id, fi->id)) + return fi; + } + return NULL; +} + +/*! register a FSM with the core + * + * A FSM descriptor needs to be registered with the core before any + * instances can be created for it. + * + * \param[in] fsm Descriptor of Finite State Machine to be registered + * \returns 0 on success; negative on error + */ +int osmo_fsm_register(struct osmo_fsm *fsm) +{ + if (!osmo_identifier_valid(fsm->name)) { + LOGP(DLGLOBAL, LOGL_ERROR, "Attempting to register FSM with illegal identifier '%s'\n", fsm->name); + return -EINVAL; + } + if (osmo_fsm_find_by_name(fsm->name)) + return -EEXIST; + if (fsm->event_names == NULL) + LOGP(DLGLOBAL, LOGL_ERROR, "FSM '%s' has no event names! Please fix!\n", fsm->name); + llist_add_tail(&fsm->list, &osmo_g_fsms); + INIT_LLIST_HEAD(&fsm->instances); + + return 0; +} + +/*! unregister a FSM from the core + * + * Once the FSM descriptor is unregistered, active instances can still + * use it, but no new instances may be created for it. + * + * \param[in] fsm Descriptor of Finite State Machine to be removed + */ +void osmo_fsm_unregister(struct osmo_fsm *fsm) +{ + llist_del(&fsm->list); +} + +/* small wrapper function around timer expiration (for logging) */ +static void fsm_tmr_cb(void *data) +{ + struct osmo_fsm_inst *fi = data; + struct osmo_fsm *fsm = fi->fsm; + int32_t T = fi->T; + + LOGPFSM(fi, "Timeout of " OSMO_T_FMT "\n", OSMO_T_FMT_ARGS(fi->T)); + + if (fsm->timer_cb) { + int rc = fsm->timer_cb(fi); + if (rc != 1) + /* We don't actually know whether fi exists anymore. + * Make sure to not access it and return right away. */ + return; + /* The timer_cb told us to terminate, so we can safely assume + * that fi still exists. */ + LOGPFSM(fi, "timer_cb requested termination\n"); + } else + LOGPFSM(fi, "No timer_cb, automatic termination\n"); + + /* if timer_cb returns 1 or there is no timer_cb */ + osmo_fsm_inst_term(fi, OSMO_FSM_TERM_TIMEOUT, &T); +} + +/*! Change id of the FSM instance + * \param[in] fi FSM instance + * \param[in] id new ID + * \returns 0 if the ID was updated, otherwise -EINVAL + */ +int osmo_fsm_inst_update_id(struct osmo_fsm_inst *fi, const char *id) +{ + if (!id) + return osmo_fsm_inst_update_id_f(fi, NULL); + else + return osmo_fsm_inst_update_id_f(fi, "%s", id); +} + +static void update_name(struct osmo_fsm_inst *fi) +{ + if (fi->name) + talloc_free((char*)fi->name); + + if (!fsm_log_addr) { + if (fi->id) + fi->name = talloc_asprintf(fi, "%s(%s)", fi->fsm->name, fi->id); + else + fi->name = talloc_asprintf(fi, "%s", fi->fsm->name); + } else { + if (fi->id) + fi->name = talloc_asprintf(fi, "%s(%s)[%p]", fi->fsm->name, fi->id, fi); + else + fi->name = talloc_asprintf(fi, "%s[%p]", fi->fsm->name, fi); + } +} + +/*! Change id of the FSM instance using a string format. + * \param[in] fi FSM instance. + * \param[in] fmt format string to compose new ID. + * \param[in] ... variable argument list for format string. + * \returns 0 if the ID was updated, otherwise -EINVAL. + */ +int osmo_fsm_inst_update_id_f(struct osmo_fsm_inst *fi, const char *fmt, ...) +{ + char *id = NULL; + + if (fmt) { + va_list ap; + + va_start(ap, fmt); + id = talloc_vasprintf(fi, fmt, ap); + va_end(ap); + + if (!osmo_identifier_valid(id)) { + LOGP(DLGLOBAL, LOGL_ERROR, + "Attempting to set illegal id for FSM instance of type '%s': %s\n", + fi->fsm->name, osmo_quote_str(id, -1)); + talloc_free(id); + return -EINVAL; + } + } + + if (fi->id) + talloc_free((char*)fi->id); + fi->id = id; + + update_name(fi); + return 0; +} + +/*! Change id of the FSM instance using a string format, and ensuring a valid id. + * Replace any characters that are not permitted as FSM identifier with replace_with. + * \param[in] fi FSM instance. + * \param[in] replace_with Character to use instead of non-permitted FSM id characters. + * Make sure to choose a legal character, e.g. '-'. + * \param[in] fmt format string to compose new ID. + * \param[in] ... variable argument list for format string. + * \returns 0 if the ID was updated, otherwise -EINVAL. + */ +int osmo_fsm_inst_update_id_f_sanitize(struct osmo_fsm_inst *fi, char replace_with, const char *fmt, ...) +{ + char *id = NULL; + va_list ap; + int rc; + + if (!fmt) + return osmo_fsm_inst_update_id(fi, NULL); + + va_start(ap, fmt); + id = talloc_vasprintf(fi, fmt, ap); + va_end(ap); + + osmo_identifier_sanitize_buf(id, NULL, replace_with); + + rc = osmo_fsm_inst_update_id(fi, id); + talloc_free(id); + return rc; +} + +/*! allocate a new instance of a specified FSM + * \param[in] fsm Descriptor of the FSM + * \param[in] ctx talloc context from which to allocate memory + * \param[in] priv private data reference store in fsm instance + * \param[in] log_level The log level for events of this FSM + * \param[in] id The name/ID of the FSM instance + * \returns newly-allocated, initialized and registered FSM instance + */ +struct osmo_fsm_inst *osmo_fsm_inst_alloc(struct osmo_fsm *fsm, void *ctx, void *priv, + int log_level, const char *id) +{ + struct osmo_fsm_inst *fi = talloc_zero(ctx, struct osmo_fsm_inst); + + fi->fsm = fsm; + fi->priv = priv; + fi->log_level = log_level; + 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; + } + + INIT_LLIST_HEAD(&fi->proc.children); + INIT_LLIST_HEAD(&fi->proc.child); + llist_add(&fi->list, &fsm->instances); + + LOGPFSM(fi, "Allocated\n"); + + return fi; +} + +/*! allocate a new instance of a specified FSM as child of + * other FSM instance + * + * This is like \ref osmo_fsm_inst_alloc but using the parent FSM as + * talloc context, and inheriting the log level of the parent. + * + * \param[in] fsm Descriptor of the to-be-allocated FSM + * \param[in] parent Parent FSM instance + * \param[in] parent_term_event Event to be sent to parent when terminating + * \returns newly-allocated, initialized and registered FSM instance + */ +struct osmo_fsm_inst *osmo_fsm_inst_alloc_child(struct osmo_fsm *fsm, + struct osmo_fsm_inst *parent, + uint32_t parent_term_event) +{ + struct osmo_fsm_inst *fi; + + fi = osmo_fsm_inst_alloc(fsm, parent, NULL, parent->log_level, + parent->id); + if (!fi) { + /* indicate immediate termination to caller */ + osmo_fsm_inst_dispatch(parent, parent_term_event, NULL); + return NULL; + } + + LOGPFSM(fi, "is child of %s\n", osmo_fsm_inst_name(parent)); + + osmo_fsm_inst_change_parent(fi, parent, parent_term_event); + + return fi; +} + +/*! unlink child FSM from its parent FSM. + * \param[in] fi Descriptor of the child FSM to unlink. + * \param[in] ctx New talloc context + * + * Never call this function from the cleanup callback, because at that time + * the child FSMs will already be terminated. If unlinking should be performed + * on FSM termination, use the grace callback instead. */ +void osmo_fsm_inst_unlink_parent(struct osmo_fsm_inst *fi, void *ctx) +{ + if (fi->proc.parent) { + talloc_steal(ctx, fi); + fi->proc.parent = NULL; + fi->proc.parent_term_event = 0; + llist_del(&fi->proc.child); + } +} + +/*! change parent instance of an FSM. + * \param[in] fi Descriptor of the to-be-allocated FSM. + * \param[in] new_parent New parent FSM instance. + * \param[in] new_parent_term_event Event to be sent to parent when terminating. + * + * Never call this function from the cleanup callback! + * (see also osmo_fsm_inst_unlink_parent()).*/ +void osmo_fsm_inst_change_parent(struct osmo_fsm_inst *fi, + struct osmo_fsm_inst *new_parent, + uint32_t new_parent_term_event) +{ + /* Make sure a possibly existing old parent is unlinked first + * (new_parent can be NULL) */ + osmo_fsm_inst_unlink_parent(fi, new_parent); + + /* Add new parent */ + if (new_parent) { + fi->proc.parent = new_parent; + fi->proc.parent_term_event = new_parent_term_event; + llist_add(&fi->proc.child, &new_parent->proc.children); + } +} + +/*! delete a given instance of a FSM + * \param[in] fi FSM instance to be un-registered and deleted + */ +void osmo_fsm_inst_free(struct osmo_fsm_inst *fi) +{ + osmo_timer_del(&fi->timer); + llist_del(&fi->list); + + if (fsm_term_safely.depth) { + /* Another FSM instance has caused this one to free and is still busy with its termination. Don't free + * yet, until the other FSM instance is done. */ + osmo_fsm_defer_free(fi); + /* The root_fi can't go missing really, but to be safe... */ + if (fsm_term_safely.root_fi) + LOGPFSM(fi, "Deferring: will deallocate with %s\n", fsm_term_safely.root_fi->name); + else + LOGPFSM(fi, "Deferring deallocation\n"); + + /* Don't free anything yet. Exit. */ + return; + } + + /* fsm_term_safely.depth == 0. + * - If fsm_term_safely is enabled, this is the original FSM instance that started terminating first. Free this + * and along with it all other collected terminated FSM instances. + * - If fsm_term_safely is disabled, this is just any FSM instance deallocating. */ + + if (fsm_term_safely.collect_ctx) { + /* The fi may be a child of any other FSM instances or objects collected in the collect_ctx. Don't + * deallocate separately to avoid use-after-free errors, put it in there and deallocate all at once. */ + LOGPFSM(fi, "Deallocated, including all deferred deallocations\n"); + osmo_fsm_defer_free(fi); + fsm_free_or_steal(fsm_term_safely.collect_ctx); + fsm_term_safely.collect_ctx = NULL; + } else { + LOGPFSM(fi, "Deallocated\n"); + fsm_free_or_steal(fi); + } + fsm_term_safely.root_fi = NULL; +} + +/*! get human-readable name of FSM event + * \param[in] fsm FSM descriptor of event + * \param[in] event Event integer value + * \returns string rendering of the event + */ +const char *osmo_fsm_event_name(const struct osmo_fsm *fsm, uint32_t event) +{ + static __thread char buf[32]; + if (!fsm->event_names) { + snprintf(buf, sizeof(buf), "%"PRIu32, event); + return buf; + } else + return get_value_string(fsm->event_names, event); +} + +/*! get human-readable name of FSM instance + * \param[in] fi FSM instance + * \returns string rendering of the FSM identity + */ +const char *osmo_fsm_inst_name(const struct osmo_fsm_inst *fi) +{ + if (!fi) + return "NULL"; + + if (fi->name) + return fi->name; + else + return fi->fsm->name; +} + +/*! get human-readable name of FSM state + * \param[in] fsm FSM descriptor + * \param[in] state FSM state number + * \returns string rendering of the FSM 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) { + snprintf(buf, sizeof(buf), "unknown %"PRIu32, state); + return buf; + } else + return fsm->states[state].name; +} + +static int state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, + bool keep_timer, unsigned long timeout_ms, int T, + const char *file, int line) +{ + struct osmo_fsm *fsm = fi->fsm; + uint32_t old_state = fi->state; + const struct osmo_fsm_state *st = &fsm->states[fi->state]; + struct timeval remaining; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, + "FSM instance already terminating, not changing state to %s\n", + osmo_fsm_state_name(fsm, new_state)); + return -EINVAL; + } + + /* validate if new_state is a valid state */ + if (!(st->out_state_mask & (1 << new_state))) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "transition to state %s not permitted!\n", + osmo_fsm_state_name(fsm, new_state)); + return -EPERM; + } + + if (!keep_timer) { + /* delete the old timer */ + osmo_timer_del(&fi->timer); + } + + if (st->onleave) + st->onleave(fi, new_state); + + if (fsm_log_timeouts) { + char trailer[64]; + trailer[0] = '\0'; + if (keep_timer && fi->timer.active) { + /* This should always give us a timeout, but just in case the return value indicates error, omit + * logging the remaining time. */ + if (osmo_timer_remaining(&fi->timer, NULL, &remaining)) + snprintf(trailer, sizeof(trailer), "(keeping " OSMO_T_FMT ")", + OSMO_T_FMT_ARGS(fi->T)); + else + snprintf(trailer, sizeof(trailer), "(keeping " OSMO_T_FMT + ", %ld.%03lds remaining)", OSMO_T_FMT_ARGS(fi->T), + (long) remaining.tv_sec, remaining.tv_usec / 1000); + } else if (timeout_ms) { + if (timeout_ms % 1000 == 0) + /* keep log output legacy compatible to avoid autotest failures */ + snprintf(trailer, sizeof(trailer), "(" OSMO_T_FMT ", %lus)", + OSMO_T_FMT_ARGS(T), timeout_ms/1000); + else + snprintf(trailer, sizeof(trailer), "(" OSMO_T_FMT ", %lums)", + OSMO_T_FMT_ARGS(T), timeout_ms); + } else + snprintf(trailer, sizeof(trailer), "(no timeout)"); + + LOGPFSMSRC(fi, file, line, "State change to %s %s\n", + osmo_fsm_state_name(fsm, new_state), trailer); + } else { + LOGPFSMSRC(fi, file, line, "state_chg to %s\n", + osmo_fsm_state_name(fsm, new_state)); + } + + fi->state = new_state; + st = &fsm->states[new_state]; + + if (!keep_timer + || (keep_timer && !osmo_timer_pending(&fi->timer))) { + fi->T = T; + 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 */ + if (st->onenter) + st->onenter(fi, old_state); + + return 0; +} + +/*! perform a state change of the given FSM instance + * + * Best invoke via the osmo_fsm_inst_state_chg() macro which logs the source + * file where the state change was effected. Alternatively, you may pass \a + * file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * If timeout_secs is 0, stay in the new state indefinitely, without a timeout + * (stop the FSM instance's timer if it was runnning). + * + * If timeout_secs > 0, start or reset the FSM instance's timer with this + * timeout. On expiry, invoke the FSM instance's timer_cb -- if no timer_cb is + * set, an expired timer immediately terminates the FSM instance with + * OSMO_FSM_TERM_TIMEOUT. + * + * The value of T is stored in fi->T and is then available for query in + * timer_cb. If passing timeout_secs == 0, it is recommended to also pass T == + * 0, so that fi->T is reset to 0 when no timeout is invoked. + * + * Positive values for T are considered to be 3GPP spec compliant and appear in + * logging and VTY as "T1234", while negative values are considered to be + * Osmocom specific timers, represented in logging and VTY as "X1234". + * + * See also osmo_tdef_fsm_inst_state_chg() from the osmo_tdef API, which + * provides a unified way to configure and apply GSM style Tnnnn timers to FSM + * state transitions. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] timeout_secs Timeout in seconds (if !=0), maximum-clamped to 2147483647 seconds. + * \param[in] T Timer number, where positive numbers are considered to be 3GPP spec compliant timer numbers and are + * logged as "T1234", while negative numbers are considered Osmocom specific timer numbers logged as + * "X1234". + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, false, timeout_secs*1000, T, file, line); +} +int _osmo_fsm_inst_state_chg_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, false, timeout_ms, T, file, line); +} + +/*! perform a state change while keeping the current timer running. + * + * This is useful to keep a timeout across several states (without having to round the + * remaining time to seconds). + * + * Best invoke via the osmo_fsm_inst_state_chg_keep_timer() macro which logs the source + * file where the state change was effected. Alternatively, you may pass \a + * file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg_keep_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + const char *file, int line) +{ + return state_chg(fi, new_state, true, 0, 0, file, line); +} + +/*! perform a state change while keeping the current timer if running, or starting a timer otherwise. + * + * This is useful to keep a timeout across several states, but to make sure that some timeout is actually running. + * + * Best invoke via the osmo_fsm_inst_state_chg_keep_or_start_timer() macro which logs the source file where the state + * change was effected. Alternatively, you may pass file as NULL to use the normal file/line indication instead. + * + * All changes to the FSM instance state must be made via an osmo_fsm_inst_state_chg_* + * function. It verifies that the existing state actually permits a + * transition to new_state. + * + * \param[in] fi FSM instance whose state is to change + * \param[in] new_state The new state into which we should change + * \param[in] timeout_secs If no timer is running yet, set this timeout in seconds (if !=0), maximum-clamped to + * 2147483647 seconds. + * \param[in] T Timer number, where positive numbers are considered to be 3GPP spec compliant timer numbers and are + * logged as "T1234", while negative numbers are considered Osmocom specific timer numbers logged as + * "X1234". + * \param[in] file Calling source file (from osmo_fsm_inst_state_chg macro) + * \param[in] line Calling source line (from osmo_fsm_inst_state_chg macro) + * \returns 0 on success; negative on error + */ +int _osmo_fsm_inst_state_chg_keep_or_start_timer(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_secs, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, true, timeout_secs*1000, T, file, line); +} +int _osmo_fsm_inst_state_chg_keep_or_start_timer_ms(struct osmo_fsm_inst *fi, uint32_t new_state, + unsigned long timeout_ms, int T, + const char *file, int line) +{ + return state_chg(fi, new_state, true, timeout_ms, T, file, line); +} + + +/*! dispatch an event to an osmocom finite state machine instance + * + * Best invoke via the osmo_fsm_inst_dispatch() macro which logs the source + * file where the event was effected. Alternatively, you may pass \a file as + * NULL to use the normal file/line indication instead. + * + * Any incoming events to \ref osmo_fsm instances must be dispatched to + * them via this function. It verifies, whether the event is permitted + * based on the current state of the FSM. If not, -1 is returned. + * + * \param[in] fi FSM instance + * \param[in] event Event to send to 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) + * \returns 0 in case of success; negative on error + */ +int _osmo_fsm_inst_dispatch(struct osmo_fsm_inst *fi, uint32_t event, void *data, + const char *file, int line) +{ + struct osmo_fsm *fsm; + const struct osmo_fsm_state *fs; + + if (!fi) { + LOGPSRC(DLGLOBAL, LOGL_ERROR, file, line, + "Trying to dispatch event %"PRIu32" to non-existent" + " FSM instance!\n", event); + osmo_log_backtrace(DLGLOBAL, LOGL_ERROR); + return -ENODEV; + } + + fsm = fi->fsm; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, + "FSM instance already terminating, not dispatching event %s\n", + osmo_fsm_event_name(fsm, event)); + return -EINVAL; + } + + OSMO_ASSERT(fi->state < fsm->num_states); + fs = &fi->fsm->states[fi->state]; + + LOGPFSMSRC(fi, file, line, + "Received Event %s\n", osmo_fsm_event_name(fsm, event)); + + if (((1 << event) & fsm->allstate_event_mask) && fsm->allstate_action) { + fsm->allstate_action(fi, event, data); + return 0; + } + + if (!((1 << event) & fs->in_event_mask)) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "Event %s not permitted\n", + osmo_fsm_event_name(fsm, event)); + return -1; + } + + if (fs->action) + fs->action(fi, event, data); + + return 0; +} + +/*! Terminate FSM instance with given cause + * + * This safely terminates the given FSM instance by first iterating + * over all children and sending them a termination event. Next, it + * calls the FSM descriptors cleanup function (if any), followed by + * releasing any memory associated with the FSM instance. + * + * Finally, the parent FSM instance (if any) is notified using the + * parent termination event configured at time of FSM instance start. + * + * \param[in] fi FSM instance to be terminated + * \param[in] cause Cause / reason for termination + * \param[in] data Opaque event data to be passed with the parent term event + * \param[in] file Calling source file (from osmo_fsm_inst_term macro) + * \param[in] line Calling source line (from osmo_fsm_inst_term macro) + */ +void _osmo_fsm_inst_term(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *parent; + uint32_t parent_term_event = fi->proc.parent_term_event; + + if (fi->proc.terminating) { + LOGPFSMSRC(fi, file, line, "Ignoring trigger to terminate: already terminating\n"); + return; + } + fi->proc.terminating = true; + + /* Start termination cascade handling only if the feature is enabled. Also check the current depth: though + * unlikely, theoretically the fsm_term_safely_enabled flag could be toggled in the middle of a cascaded + * termination, so make sure to continue if it already started. */ + if (fsm_term_safely_enabled || fsm_term_safely.depth) { + fsm_term_safely.depth++; + /* root_fi is just for logging, so no need to be extra careful about it. */ + if (!fsm_term_safely.root_fi) + fsm_term_safely.root_fi = fi; + } + + if (fsm_term_safely.depth > 1) { + /* fsm_term_safely is enabled and this is a secondary FSM instance terminated, caused by the root_fi. */ + LOGPFSMSRC(fi, file, line, "Terminating in cascade, depth %d (cause = %s, caused by: %s)\n", + fsm_term_safely.depth, osmo_fsm_term_cause_name(cause), + fsm_term_safely.root_fi ? fsm_term_safely.root_fi->name : "unknown"); + /* The root_fi can't go missing really, but to be safe, log "unknown" in that case. */ + } else { + /* fsm_term_safely is disabled, or this is the root_fi. */ + LOGPFSMSRC(fi, file, line, "Terminating (cause = %s)\n", osmo_fsm_term_cause_name(cause)); + } + + /* graceful exit (optional) */ + if (fi->fsm->pre_term) + fi->fsm->pre_term(fi, cause); + + _osmo_fsm_inst_term_children(fi, OSMO_FSM_TERM_PARENT, NULL, + file, line); + + /* delete ourselves from the parent */ + parent = fi->proc.parent; + if (parent) { + LOGPFSMSRC(fi, file, line, "Removing from parent %s\n", + osmo_fsm_inst_name(parent)); + llist_del(&fi->proc.child); + } + + /* call destructor / clean-up function */ + if (fi->fsm->cleanup) + fi->fsm->cleanup(fi, cause); + + /* Fetch parent again in case it has changed. */ + parent = fi->proc.parent; + + /* Legacy behavior if fsm_term_safely is disabled: free before dispatching parent event. (If fsm_term_safely is + * enabled, depth will *always* be > 0 here.) Pivot on depth instead of the enabled flag in case the enabled + * flag is toggled in the middle of an FSM term. */ + if (!fsm_term_safely.depth) { + LOGPFSMSRC(fi, file, line, "Freeing instance\n"); + osmo_fsm_inst_free(fi); + } + + /* indicate our termination to the parent */ + if (parent && cause != OSMO_FSM_TERM_PARENT) + _osmo_fsm_inst_dispatch(parent, parent_term_event, data, + file, line); + + /* Newer, safe deallocation: free only after the parent_term_event was dispatched, to catch all termination + * cascades, and free all FSM instances at once. (If fsm_term_safely is enabled, depth will *always* be > 0 + * here.) osmo_fsm_inst_free() will do the defer magic depending on the fsm_term_safely.depth. */ + if (fsm_term_safely.depth) { + fsm_term_safely.depth--; + osmo_fsm_inst_free(fi); + } +} + +/*! Terminate all child FSM instances of an FSM instance. + * + * Iterate over all children and send them a termination event, with the given + * cause. Pass OSMO_FSM_TERM_PARENT to avoid dispatching events from the + * terminated child FSMs. + * + * \param[in] fi FSM instance that should be cleared of child FSMs + * \param[in] cause Cause / reason for termination (OSMO_FSM_TERM_PARENT) + * \param[in] data Opaque event data to be passed with the parent term events + * \param[in] file Calling source file (from osmo_fsm_inst_term_children macro) + * \param[in] line Calling source line (from osmo_fsm_inst_term_children macro) + */ +void _osmo_fsm_inst_term_children(struct osmo_fsm_inst *fi, + enum osmo_fsm_term_cause cause, + void *data, + const char *file, int line) +{ + struct osmo_fsm_inst *first_child, *last_seen_first_child; + + /* iterate over all children, starting from the beginning every time: + * terminating an FSM may emit events that cause other FSMs to also + * terminate and remove themselves from this list. */ + last_seen_first_child = NULL; + while (!llist_empty(&fi->proc.children)) { + first_child = llist_entry(fi->proc.children.next, + typeof(*first_child), + proc.child); + + /* paranoia: do not loop forever */ + if (first_child == last_seen_first_child) { + LOGPFSMLSRC(fi, LOGL_ERROR, file, line, + "Internal error while terminating child" + " FSMs: a child FSM is stuck\n"); + break; + } + last_seen_first_child = first_child; + + /* terminate child */ + _osmo_fsm_inst_term(first_child, cause, data, + file, line); + } +} + +/*! 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), + OSMO_VALUE_STRING(OSMO_FSM_TERM_REGULAR), + OSMO_VALUE_STRING(OSMO_FSM_TERM_ERROR), + OSMO_VALUE_STRING(OSMO_FSM_TERM_TIMEOUT), + { 0, NULL } +}; + +/*! @} */ diff --git a/src/core/gsmtap_util.c b/src/core/gsmtap_util.c new file mode 100644 index 00000000..dcbd3049 --- /dev/null +++ b/src/core/gsmtap_util.c @@ -0,0 +1,627 @@ +/*! \file gsmtap_util.c + * GSMTAP support code in libosmocore. */ +/* + * (C) 2010-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" + +#include <osmocom/core/gsmtap_util.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/gsmtap.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/select.h> +#include <osmocom/core/socket.h> +#include <osmocom/core/byteswap.h> +#include <osmocom/gsm/protocol/gsm_04_08.h> +#include <osmocom/gsm/rsl.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <string.h> +#include <errno.h> + +/*! \addtogroup gsmtap + * @{ + * GSMTAP utility routines. Encapsulates GSM messages over UDP. + * + * \file gsmtap_util.c */ + +/*! one gsmtap instance */ +struct gsmtap_inst { + int ofd_wq_mode; /*!< wait queue mode? 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_fd sink_ofd; /*!< file descriptor */ +}; + +/*! 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_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: + 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: + 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; + break; + case RSL_CHAN_SDCCH8_ACCH: + ret = GSMTAP_CHANNEL_SDCCH8; + break; + case RSL_CHAN_BCCH: + ret = GSMTAP_CHANNEL_BCCH; + break; + case RSL_CHAN_RACH: + ret = GSMTAP_CHANNEL_RACH; + break; + case RSL_CHAN_PCH_AGCH: + /* it could also be AGCH... */ + ret = GSMTAP_CHANNEL_PCH; + break; + 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) + ret |= GSMTAP_CHANNEL_ACCH; + + 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 + * \param[out] link_id RSL link identifier + */ +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_FACCH_F: + case GSMTAP_CHANNEL_VOICE_F: // TCH/F + *rsl_chantype = RSL_CHAN_Bm_ACCHs; + break; + 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 + *rsl_chantype = RSL_CHAN_SDCCH4_ACCH; + break; + case GSMTAP_CHANNEL_SDCCH8: // SDCCH/8 + *rsl_chantype = RSL_CHAN_SDCCH8_ACCH; + break; + case GSMTAP_CHANNEL_BCCH: // BCCH + *rsl_chantype = RSL_CHAN_BCCH; + break; + case GSMTAP_CHANNEL_RACH: // RACH + *rsl_chantype = RSL_CHAN_RACH; + break; + case GSMTAP_CHANNEL_PCH: // PCH + case GSMTAP_CHANNEL_AGCH: // AGCH + *rsl_chantype = RSL_CHAN_PCH_AGCH; + break; + case GSMTAP_CHANNEL_PDCH: + *rsl_chantype = RSL_CHAN_OSMO_PDCH; + break; + } + + *link_id = gsmtap_chantype & GSMTAP_CHANNEL_ACCH ? 0x40 : 0x00; +} + +/*! create an arbitrary type GSMTAP message + * \param[in] type The GSMTAP_TYPE_xxx constant of the message to create + * \param[in] arfcn GSM ARFCN (Channel Number) + * \param[in] ts GSM time slot + * \param[in] chan_type Channel Type + * \param[in] ss Sub-slot + * \param[in] fn GSM Frame Number + * \param[in] signal_dbm Signal Strength (dBm) + * \param[in] snr Signal/Noise Ratio (SNR) + * \param[in] data Pointer to data buffer + * \param[in] len Length of \ref data + * \return dynamically allocated message buffer containing data + * + * This function will allocate a new msgb and fill it with a GSMTAP + * header containing the information + */ +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, + int8_t snr, const uint8_t *data, unsigned int len) +{ + struct msgb *msg; + struct gsmtap_hdr *gh; + uint8_t *dst; + + msg = msgb_alloc(sizeof(*gh) + len, "gsmtap_tx"); + if (!msg) + return NULL; + + gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh)); + + gh->version = GSMTAP_VERSION; + gh->hdr_len = sizeof(*gh)/4; + gh->type = type; + gh->timeslot = ts; + gh->sub_slot = ss; + gh->arfcn = osmo_htons(arfcn); + gh->snr_db = snr; + gh->signal_dbm = signal_dbm; + gh->frame_number = osmo_htonl(fn); + gh->sub_type = chan_type; + gh->antenna_nr = 0; + + dst = msgb_put(msg, len); + memcpy(dst, data, len); + + return msg; +} + +/*! create L1/L2 data and put it into GSMTAP + * \param[in] arfcn GSM ARFCN (Channel Number) + * \param[in] ts GSM time slot + * \param[in] chan_type Channel Type + * \param[in] ss Sub-slot + * \param[in] fn GSM Frame Number + * \param[in] signal_dbm Signal Strength (dBm) + * \param[in] snr Signal/Noise Ratio (SNR) + * \param[in] data Pointer to data buffer + * \param[in] len Length of \ref data + * \return message buffer or NULL in case of error + * + * This function will allocate a new msgb and fill it with a GSMTAP + * header containing the information + */ +struct msgb *gsmtap_makemsg(uint16_t arfcn, uint8_t ts, uint8_t chan_type, + uint8_t ss, uint32_t fn, int8_t signal_dbm, + 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); +} + +#ifdef HAVE_SYS_SOCKET_H + +#include <sys/socket.h> +#include <netinet/in.h> + +/*! 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, 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. + * */ +int gsmtap_source_init_fd(const char *host, uint16_t port) +{ + if (port == 0) + port = GSMTAP_UDP_PORT; + if (host == NULL) + host = "localhost"; + + return osmo_sock_init(AF_UNSPEC, SOCK_DGRAM, IPPROTO_UDP, host, 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 + * + * In case the GSMTAP socket is connected to a local destination + * IP/port, this function creates a corresponding receiving socket + * bound to that destination IP + port. + * + * In case the gsmtap socket is not connected to a local IP/port, or + * creation of the receiving socket fails, a negative error code is + * returned. + */ +int gsmtap_source_add_sink_fd(int gsmtap_fd) +{ + struct sockaddr_storage ss; + socklen_t ss_len = sizeof(ss); + int rc; + + rc = getpeername(gsmtap_fd, (struct sockaddr *)&ss, &ss_len); + if (rc < 0) + return rc; + + if (osmo_sockaddr_is_local((struct sockaddr *)&ss, ss_len) == 1) { + rc = osmo_sock_init_sa((struct sockaddr *)&ss, SOCK_DGRAM, + IPPROTO_UDP, + OSMO_SOCK_F_BIND | + OSMO_SOCK_F_UDP_REUSEADDR); + if (rc >= 0) + return rc; + } + + return -ENODEV; +} + +/*! Send a \ref msgb through a GSMTAP source + * \param[in] gti GSMTAP instance + * \param[in] msg message buffer + * \return 0 in case of success; negative in case of error + * NOTE: in case of nonzero return value, the *caller* must free the msg! + * (This enables the caller to attempt re-sending the message.) + * If 0 is returned, the msgb was freed by this function. + */ +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); + else { + /* try immediate send and return error if any */ + int rc; + + rc = write(gsmtap_inst_fd2(gti), msg->data, msg->len); + if (rc < 0) { + return rc; + } else if (rc >= msg->len) { + msgb_free(msg); + return 0; + } else { + /* short write */ + return -EIO; + } + } +} + +/*! 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, int8_t snr, const uint8_t *data, + unsigned int len) +{ + struct msgb *msg; + int rc; + + if (!gti) + return -ENODEV; + + msg = gsmtap_makemsg_ex(type, arfcn, ts, chan_type, ss, fn, signal_dbm, + snr, data, len); + if (!msg) + return -ENOMEM; + + rc = gsmtap_sendmsg(gti, msg); + if (rc) + msgb_free(msg); + return rc; +} + +/*! send a message from L1/L2 through GSMTAP. + * See \ref gsmtap_makemsg for arguments + */ +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, 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 + * + * In case the GSMTAP socket is connected to a local destination + * IP/port, this function creates a corresponding receiving socket + * bound to that destination IP + port. + * + * In case the gsmtap socket is not connected to a local IP/port, or + * creation of the receiving socket fails, a negative error code is + * returned. + * + * The file descriptor of the receiving socket is automatically added + * to the libosmocore select() handling. + */ +int gsmtap_source_add_sink(struct gsmtap_inst *gti) +{ + int fd, rc; + + fd = gsmtap_source_add_sink_fd(gsmtap_inst_fd2(gti)); + if (fd < 0) + return fd; + + if (gti->ofd_wq_mode) { + struct osmo_fd *sink_ofd; + + sink_ofd = >i->sink_ofd; + sink_ofd->fd = fd; + sink_ofd->when = OSMO_FD_READ; + sink_ofd->cb = gsmtap_sink_fd_cb; + + rc = osmo_fd_register(sink_ofd); + if (rc < 0) { + close(fd); + return rc; + } + } + + return fd; +} + + +/*! 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; + + 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->ofd_wq_mode = ofd_wq_mode; + gti->wq.bfd.fd = fd; + gti->sink_ofd.fd = -1; + + if (ofd_wq_mode) { + osmo_wqueue_init(>i->wq, 64); + gti->wq.write_cb = &gsmtap_wq_w_cb; + + rc = osmo_fd_register(>i->wq.bfd); + if (rc < 0) { + talloc_free(gti); + close(fd); + return NULL; + } + } + + return gti; +} + +/*! Open GSMTAP source socket, connect and register osmo_fd + * \param[in] host host name or IP address in string format + * \param[in] 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 host/port, + * allocate 'struct gsmtap_inst' and optionally osmo_fd/osmo_wqueue + * registration. + */ +struct gsmtap_inst *gsmtap_source_init(const char *host, uint16_t port, + int ofd_wq_mode) +{ + return gsmtap_source_init2(NULL, 0, host, port, ofd_wq_mode); +} + +void gsmtap_source_free(struct gsmtap_inst *gti) +{ + if (!gti) + return; + + if (gti->ofd_wq_mode) { + osmo_fd_unregister(>i->wq.bfd); + osmo_wqueue_clear(>i->wq); + + if (gti->sink_ofd.fd != -1) { + osmo_fd_unregister(>i->sink_ofd); + close(gti->sink_ofd.fd); + } + } + + close(gti->wq.bfd.fd); + talloc_free(gti); +} + +#endif /* HAVE_SYS_SOCKET_H */ + +const struct value_string gsmtap_gsm_channel_names[] = { + { GSMTAP_CHANNEL_UNKNOWN, "UNKNOWN" }, + { GSMTAP_CHANNEL_BCCH, "BCCH" }, + { GSMTAP_CHANNEL_CCCH, "CCCH" }, + { GSMTAP_CHANNEL_RACH, "RACH" }, + { GSMTAP_CHANNEL_AGCH, "AGCH" }, + { GSMTAP_CHANNEL_PCH, "PCH" }, + { GSMTAP_CHANNEL_SDCCH, "SDCCH" }, + { GSMTAP_CHANNEL_SDCCH4, "SDCCH/4" }, + { GSMTAP_CHANNEL_SDCCH8, "SDCCH/8" }, + { GSMTAP_CHANNEL_FACCH_F, "FACCH/F" }, + { GSMTAP_CHANNEL_FACCH_H, "FACCH/H" }, + { GSMTAP_CHANNEL_PACCH, "PACCH" }, + { GSMTAP_CHANNEL_CBCH52, "CBCH" }, + { GSMTAP_CHANNEL_PDCH, "PDCH" } , + { GSMTAP_CHANNEL_PTCCH, "PTTCH" }, + { GSMTAP_CHANNEL_CBCH51, "CBCH" }, + { 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_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 } +}; + +/* for debugging */ +const struct value_string gsmtap_type_names[] = { + { GSMTAP_TYPE_UM, "GSM Um (MS<->BTS)" }, + { GSMTAP_TYPE_ABIS, "GSM Abis (BTS<->BSC)" }, + { GSMTAP_TYPE_UM_BURST, "GSM Um burst (MS<->BTS)" }, + { GSMTAP_TYPE_SIM, "SIM Card" }, + { 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" }, + { GSMTAP_TYPE_LTE_RRC, "LTE RRC" }, + { GSMTAP_TYPE_LTE_MAC, "LTE MAC" }, + { 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/core/isdnhdlc.c b/src/core/isdnhdlc.c new file mode 100644 index 00000000..4ced5afd --- /dev/null +++ b/src/core/isdnhdlc.c @@ -0,0 +1,612 @@ +/* + * isdnhdlc.c -- General purpose ISDN HDLC decoder. + * + * Copyright (C) + * 2009 Karsten Keil <keil@b1-systems.de> + * 2002 Wolfgang Mües <wolfgang@iksw-muees.de> + * 2001 Frode Isaksen <fisaksen@bewan.com> + * 2001 Kai Germaschewski <kai.germaschewski@gmx.de> + * + * slightly adapted for use in userspace / osmocom envrionment by Harald Welte + * + * 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/crc16.h> +#include <osmocom/core/bits.h> +#include <osmocom/core/isdnhdlc.h> + +enum { + HDLC_FAST_IDLE, HDLC_GET_FLAG_B0, HDLC_GETFLAG_B1A6, HDLC_GETFLAG_B7, + HDLC_GET_DATA, HDLC_FAST_FLAG +}; + +enum { + HDLC_SEND_DATA, HDLC_SEND_CRC1, HDLC_SEND_FAST_FLAG, + HDLC_SEND_FIRST_FLAG, HDLC_SEND_CRC2, HDLC_SEND_CLOSING_FLAG, + HDLC_SEND_IDLE1, HDLC_SEND_FAST_IDLE, HDLC_SENDFLAG_B0, + HDLC_SENDFLAG_B1A6, HDLC_SENDFLAG_B7, STOPPED, HDLC_SENDFLAG_ONE +}; + +#define crc_ccitt_byte osmo_crc16_ccitt_byte + +void osmo_isdnhdlc_rcv_init(struct osmo_isdnhdlc_vars *hdlc, uint32_t features) +{ + memset(hdlc, 0, sizeof(*hdlc)); + hdlc->state = HDLC_GET_DATA; + if (features & OSMO_HDLC_F_56KBIT) + hdlc->do_adapt56 = 1; + if (features & OSMO_HDLC_F_BITREVERSE) + hdlc->do_bitreverse = 1; +} + +void osmo_isdnhdlc_out_init(struct osmo_isdnhdlc_vars *hdlc, uint32_t features) +{ + memset(hdlc, 0, sizeof(*hdlc)); + if (features & OSMO_HDLC_F_DCHANNEL) { + hdlc->dchannel = 1; + hdlc->state = HDLC_SEND_FIRST_FLAG; + } else { + hdlc->dchannel = 0; + hdlc->state = HDLC_SEND_FAST_FLAG; + hdlc->ffvalue = 0x7e; + } + hdlc->cbin = 0x7e; + if (features & OSMO_HDLC_F_56KBIT) { + hdlc->do_adapt56 = 1; + hdlc->state = HDLC_SENDFLAG_B0; + } else + hdlc->data_bits = 8; + if (features & OSMO_HDLC_F_BITREVERSE) + hdlc->do_bitreverse = 1; +} + +static int +check_frame(struct osmo_isdnhdlc_vars *hdlc) +{ + int status; + + if (hdlc->dstpos < 2) /* too small - framing error */ + status = -OSMO_HDLC_FRAMING_ERROR; + else if (hdlc->crc != 0xf0b8) /* crc error */ + status = -OSMO_HDLC_CRC_ERROR; + else { + /* remove CRC */ + hdlc->dstpos -= 2; + /* good frame */ + status = hdlc->dstpos; + } + return status; +} + +/*! decodes HDLC frames from a transparent bit stream. + + The source buffer is scanned for valid HDLC frames looking for + flags (01111110) to indicate the start of a frame. If the start of + the frame is found, the bit stuffing is removed (0 after 5 1's). + 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. + 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 -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. + + \param[in] src source buffer + \param[in] slen source buffer length + \param[out] count number of bytes removed (decoded) from the source buffer + \param[out] dst destination buffer + \param[in] dsize destination buffer size + \returns number of decoded bytes in the destination buffer and status flag. +*/ +int osmo_isdnhdlc_decode(struct osmo_isdnhdlc_vars *hdlc, const uint8_t *src, int slen, + int *count, uint8_t *dst, int dsize) +{ + int status = 0; + + static const unsigned char fast_flag[] = { + 0x00, 0x00, 0x00, 0x20, 0x30, 0x38, 0x3c, 0x3e, 0x3f + }; + + static const unsigned char fast_flag_value[] = { + 0x00, 0x7e, 0xfc, 0xf9, 0xf3, 0xe7, 0xcf, 0x9f, 0x3f + }; + + static const unsigned char fast_abort[] = { + 0x00, 0x00, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff + }; + +#define handle_fast_flag(h) \ + do { \ + if (h->cbin == fast_flag[h->bit_shift]) { \ + h->ffvalue = fast_flag_value[h->bit_shift]; \ + h->state = HDLC_FAST_FLAG; \ + h->ffbit_shift = h->bit_shift; \ + h->bit_shift = 1; \ + } else { \ + h->state = HDLC_GET_DATA; \ + h->data_received = 0; \ + } \ + } while (0) + +#define handle_abort(h) \ + do { \ + h->shift_reg = fast_abort[h->ffbit_shift - 1]; \ + h->hdlc_bits1 = h->ffbit_shift - 2; \ + if (h->hdlc_bits1 < 0) \ + h->hdlc_bits1 = 0; \ + h->data_bits = h->ffbit_shift - 1; \ + h->state = HDLC_GET_DATA; \ + h->data_received = 0; \ + } while (0) + + *count = slen; + + while (slen > 0) { + if (hdlc->bit_shift == 0) { + /* the code is for bitreverse streams */ + if (hdlc->do_bitreverse == 0) + hdlc->cbin = osmo_revbytebits_8(*src++); + else + hdlc->cbin = *src++; + slen--; + hdlc->bit_shift = 8; + if (hdlc->do_adapt56) + hdlc->bit_shift--; + } + + switch (hdlc->state) { + case STOPPED: + return 0; + case HDLC_FAST_IDLE: + if (hdlc->cbin == 0xff) { + hdlc->bit_shift = 0; + break; + } + hdlc->state = HDLC_GET_FLAG_B0; + hdlc->hdlc_bits1 = 0; + hdlc->bit_shift = 8; + break; + case HDLC_GET_FLAG_B0: + if (!(hdlc->cbin & 0x80)) { + hdlc->state = HDLC_GETFLAG_B1A6; + hdlc->hdlc_bits1 = 0; + } else { + if ((!hdlc->do_adapt56) && + (++hdlc->hdlc_bits1 >= 8) && + (hdlc->bit_shift == 1)) + hdlc->state = HDLC_FAST_IDLE; + } + hdlc->cbin <<= 1; + hdlc->bit_shift--; + break; + case HDLC_GETFLAG_B1A6: + if (hdlc->cbin & 0x80) { + hdlc->hdlc_bits1++; + if (hdlc->hdlc_bits1 == 6) + hdlc->state = HDLC_GETFLAG_B7; + } else + hdlc->hdlc_bits1 = 0; + hdlc->cbin <<= 1; + hdlc->bit_shift--; + break; + case HDLC_GETFLAG_B7: + if (hdlc->cbin & 0x80) { + hdlc->state = HDLC_GET_FLAG_B0; + } else { + hdlc->state = HDLC_GET_DATA; + hdlc->crc = 0xffff; + hdlc->shift_reg = 0; + hdlc->hdlc_bits1 = 0; + hdlc->data_bits = 0; + hdlc->data_received = 0; + } + hdlc->cbin <<= 1; + hdlc->bit_shift--; + break; + case HDLC_GET_DATA: + if (hdlc->cbin & 0x80) { + hdlc->hdlc_bits1++; + switch (hdlc->hdlc_bits1) { + case 6: + break; + case 7: + if (hdlc->data_received) + /* bad frame */ + status = -OSMO_HDLC_FRAMING_ERROR; + if (!hdlc->do_adapt56) { + if (hdlc->cbin == fast_abort + [hdlc->bit_shift + 1]) { + hdlc->state = + HDLC_FAST_IDLE; + hdlc->bit_shift = 1; + break; + } + } else + hdlc->state = HDLC_GET_FLAG_B0; + break; + default: + hdlc->shift_reg >>= 1; + hdlc->shift_reg |= 0x80; + hdlc->data_bits++; + break; + } + } else { + switch (hdlc->hdlc_bits1) { + case 5: + break; + case 6: + if (hdlc->data_received) + status = check_frame(hdlc); + hdlc->crc = 0xffff; + hdlc->shift_reg = 0; + hdlc->data_bits = 0; + if (!hdlc->do_adapt56) + handle_fast_flag(hdlc); + else { + hdlc->state = HDLC_GET_DATA; + hdlc->data_received = 0; + } + break; + default: + hdlc->shift_reg >>= 1; + hdlc->data_bits++; + break; + } + hdlc->hdlc_bits1 = 0; + } + if (status) { + hdlc->dstpos = 0; + *count -= slen; + hdlc->cbin <<= 1; + hdlc->bit_shift--; + return status; + } + if (hdlc->data_bits == 8) { + hdlc->data_bits = 0; + hdlc->data_received = 1; + hdlc->crc = crc_ccitt_byte(hdlc->crc, + hdlc->shift_reg); + + /* good byte received */ + if (hdlc->dstpos < dsize) + dst[hdlc->dstpos++] = hdlc->shift_reg; + else { + /* frame too long */ + status = -OSMO_HDLC_LENGTH_ERROR; + hdlc->dstpos = 0; + } + } + hdlc->cbin <<= 1; + hdlc->bit_shift--; + break; + case HDLC_FAST_FLAG: + if (hdlc->cbin == hdlc->ffvalue) { + hdlc->bit_shift = 0; + break; + } else { + if (hdlc->cbin == 0xff) { + hdlc->state = HDLC_FAST_IDLE; + hdlc->bit_shift = 0; + } else if (hdlc->ffbit_shift == 8) { + hdlc->state = HDLC_GETFLAG_B7; + break; + } else + handle_abort(hdlc); + } + break; + default: + break; + } + } + *count -= slen; + return 0; +} +/*! encodes HDLC frames to a transparent bit stream. + + The bit stream starts with a beginning flag (01111110). After + that each byte is added to the bit stream with bit stuffing added + (0 after 5 1's). + When the last byte has been removed from the source buffer, the + CRC (2 bytes is added) and the frame terminates with the ending flag. + For the dchannel, the idle character (all 1's) is also added at the end. + If this function is called with empty source buffer (slen=0), flags or + idle character will be generated. + + \param[in] src source buffer + \param[in] slen source buffer length + \param[out] count number of bytes removed (encoded) from source buffer + \param[out] dst destination buffer + \param[in] dsize destination buffer size + \returns - number of encoded bytes in the destination buffer +*/ +int osmo_isdnhdlc_encode(struct osmo_isdnhdlc_vars *hdlc, const uint8_t *src, uint16_t slen, + int *count, uint8_t *dst, int dsize) +{ + static const unsigned char xfast_flag_value[] = { + 0x7e, 0x3f, 0x9f, 0xcf, 0xe7, 0xf3, 0xf9, 0xfc, 0x7e + }; + + int len = 0; + + *count = slen; + + /* special handling for one byte frames */ + if ((slen == 1) && (hdlc->state == HDLC_SEND_FAST_FLAG)) + hdlc->state = HDLC_SENDFLAG_ONE; + while (dsize > 0) { + if (hdlc->bit_shift == 0) { + if (slen && !hdlc->do_closing) { + hdlc->shift_reg = *src++; + slen--; + if (slen == 0) + /* closing sequence, CRC + flag(s) */ + hdlc->do_closing = 1; + hdlc->bit_shift = 8; + } else { + if (hdlc->state == HDLC_SEND_DATA) { + if (hdlc->data_received) { + hdlc->state = HDLC_SEND_CRC1; + hdlc->crc ^= 0xffff; + hdlc->bit_shift = 8; + hdlc->shift_reg = + hdlc->crc & 0xff; + } else if (!hdlc->do_adapt56) + hdlc->state = + HDLC_SEND_FAST_FLAG; + else + hdlc->state = + HDLC_SENDFLAG_B0; + } + + } + } + + switch (hdlc->state) { + case STOPPED: + while (dsize--) + *dst++ = 0xff; + return dsize; + case HDLC_SEND_FAST_FLAG: + hdlc->do_closing = 0; + if (slen == 0) { + /* the code is for bitreverse streams */ + if (hdlc->do_bitreverse == 0) + *dst++ = osmo_revbytebits_8(hdlc->ffvalue); + else + *dst++ = hdlc->ffvalue; + len++; + dsize--; + break; + } + /* fall through */ + case HDLC_SENDFLAG_ONE: + if (hdlc->bit_shift == 8) { + hdlc->cbin = hdlc->ffvalue >> + (8 - hdlc->data_bits); + hdlc->state = HDLC_SEND_DATA; + hdlc->crc = 0xffff; + hdlc->hdlc_bits1 = 0; + hdlc->data_received = 1; + } + break; + case HDLC_SENDFLAG_B0: + hdlc->do_closing = 0; + hdlc->cbin <<= 1; + hdlc->data_bits++; + hdlc->hdlc_bits1 = 0; + hdlc->state = HDLC_SENDFLAG_B1A6; + break; + case HDLC_SENDFLAG_B1A6: + hdlc->cbin <<= 1; + hdlc->data_bits++; + hdlc->cbin++; + if (++hdlc->hdlc_bits1 == 6) + hdlc->state = HDLC_SENDFLAG_B7; + break; + case HDLC_SENDFLAG_B7: + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (slen == 0) { + hdlc->state = HDLC_SENDFLAG_B0; + break; + } + if (hdlc->bit_shift == 8) { + hdlc->state = HDLC_SEND_DATA; + hdlc->crc = 0xffff; + hdlc->hdlc_bits1 = 0; + hdlc->data_received = 1; + } + break; + case HDLC_SEND_FIRST_FLAG: + hdlc->data_received = 1; + if (hdlc->data_bits == 8) { + hdlc->state = HDLC_SEND_DATA; + hdlc->crc = 0xffff; + hdlc->hdlc_bits1 = 0; + break; + } + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (hdlc->shift_reg & 0x01) + hdlc->cbin++; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + if (hdlc->bit_shift == 0) { + hdlc->state = HDLC_SEND_DATA; + hdlc->crc = 0xffff; + hdlc->hdlc_bits1 = 0; + } + break; + case HDLC_SEND_DATA: + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (hdlc->hdlc_bits1 == 5) { + hdlc->hdlc_bits1 = 0; + break; + } + if (hdlc->bit_shift == 8) + hdlc->crc = crc_ccitt_byte(hdlc->crc, + hdlc->shift_reg); + if (hdlc->shift_reg & 0x01) { + hdlc->hdlc_bits1++; + hdlc->cbin++; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } else { + hdlc->hdlc_bits1 = 0; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } + break; + case HDLC_SEND_CRC1: + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (hdlc->hdlc_bits1 == 5) { + hdlc->hdlc_bits1 = 0; + break; + } + if (hdlc->shift_reg & 0x01) { + hdlc->hdlc_bits1++; + hdlc->cbin++; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } else { + hdlc->hdlc_bits1 = 0; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } + if (hdlc->bit_shift == 0) { + hdlc->shift_reg = (hdlc->crc >> 8); + hdlc->state = HDLC_SEND_CRC2; + hdlc->bit_shift = 8; + } + break; + case HDLC_SEND_CRC2: + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (hdlc->hdlc_bits1 == 5) { + hdlc->hdlc_bits1 = 0; + break; + } + if (hdlc->shift_reg & 0x01) { + hdlc->hdlc_bits1++; + hdlc->cbin++; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } else { + hdlc->hdlc_bits1 = 0; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + } + if (hdlc->bit_shift == 0) { + hdlc->shift_reg = 0x7e; + hdlc->state = HDLC_SEND_CLOSING_FLAG; + hdlc->bit_shift = 8; + } + break; + case HDLC_SEND_CLOSING_FLAG: + hdlc->cbin <<= 1; + hdlc->data_bits++; + if (hdlc->hdlc_bits1 == 5) { + hdlc->hdlc_bits1 = 0; + break; + } + if (hdlc->shift_reg & 0x01) + hdlc->cbin++; + hdlc->shift_reg >>= 1; + hdlc->bit_shift--; + if (hdlc->bit_shift == 0) { + hdlc->ffvalue = + xfast_flag_value[hdlc->data_bits]; + if (hdlc->dchannel) { + hdlc->ffvalue = 0x7e; + hdlc->state = HDLC_SEND_IDLE1; + hdlc->bit_shift = 8-hdlc->data_bits; + if (hdlc->bit_shift == 0) + hdlc->state = + HDLC_SEND_FAST_IDLE; + } else { + if (!hdlc->do_adapt56) { + hdlc->state = + HDLC_SEND_FAST_FLAG; + hdlc->data_received = 0; + } else { + hdlc->state = HDLC_SENDFLAG_B0; + hdlc->data_received = 0; + } + /* Finished this frame, send flags */ + if (dsize > 1) + dsize = 1; + } + } + break; + case HDLC_SEND_IDLE1: + hdlc->do_closing = 0; + hdlc->cbin <<= 1; + hdlc->cbin++; + hdlc->data_bits++; + hdlc->bit_shift--; + if (hdlc->bit_shift == 0) { + hdlc->state = HDLC_SEND_FAST_IDLE; + hdlc->bit_shift = 0; + } + break; + case HDLC_SEND_FAST_IDLE: + hdlc->do_closing = 0; + hdlc->cbin = 0xff; + hdlc->data_bits = 8; + if (hdlc->bit_shift == 8) { + hdlc->cbin = 0x7e; + hdlc->state = HDLC_SEND_FIRST_FLAG; + } else { + /* the code is for bitreverse streams */ + if (hdlc->do_bitreverse == 0) + *dst++ = osmo_revbytebits_8(hdlc->cbin); + else + *dst++ = hdlc->cbin; + hdlc->bit_shift = 0; + hdlc->data_bits = 0; + len++; + dsize = 0; + } + break; + default: + break; + } + if (hdlc->do_adapt56) { + if (hdlc->data_bits == 7) { + hdlc->cbin <<= 1; + hdlc->cbin++; + hdlc->data_bits++; + } + } + if (hdlc->data_bits == 8) { + /* the code is for bitreverse streams */ + if (hdlc->do_bitreverse == 0) + *dst++ = osmo_revbytebits_8(hdlc->cbin); + else + *dst++ = hdlc->cbin; + hdlc->data_bits = 0; + len++; + dsize--; + } + } + *count -= slen; + + return len; +} diff --git a/src/core/it_q.c b/src/core/it_q.c new file mode 100644 index 00000000..a3ff420c --- /dev/null +++ b/src/core/it_q.c @@ -0,0 +1,272 @@ +/*! \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 dequeued message buffer; 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); + + if (llist_empty(&queue->list)) + l = NULL; + l = queue->list.next; + OSMO_ASSERT(l); + llist_del(l); + 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..e5f8bd8c --- /dev/null +++ b/src/core/libosmocore.map @@ -0,0 +1,601 @@ +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_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_fd; +osmo_iofd_get_name; +osmo_iofd_set_name; +osmo_iofd_get_priv_nr; +osmo_iofd_init; +osmo_iofd_ops; +osmo_iofd_register; +osmo_iofd_sendto_msgb; +osmo_iofd_set_alloc_info; +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_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_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_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_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_set_dscp; +osmo_sock_set_priority; +osmo_sock_unix_init; +osmo_sock_unix_init_ofd; +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_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/core/logging.c b/src/core/logging.c new file mode 100644 index 00000000..c6774f57 --- /dev/null +++ b/src/core/logging.c @@ -0,0 +1,1539 @@ +/*! \file logging.c + * Debugging/Logging support code. */ +/* + * (C) 2008-2010 by Harald Welte <laforge@gnumonks.org> + * (C) 2008 by Holger Hans Peter Freyther <zecke@selfish.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 logging + * @{ + * libosmocore Logging sub-system + * + * \file logging.c */ + +#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> + +#include <osmocom/core/talloc.h> +#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), + enum_logging_filters_fit_in_log_target_filter_data); +osmo_static_assert(_LOG_FLT_COUNT <= 8*sizeof(((struct log_target*)NULL)->filter_map), + enum_logging_filters_fit_in_log_target_filter_map); + +struct log_info *osmo_log_info; + +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) +/*! 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 + thread is writing to it */ +static pthread_mutex_t osmo_log_tgt_mutex; +static bool osmo_log_tgt_mutex_on = false; + +/*! Enable multithread support (mutex) in libosmocore logging system. + * Must be called by processes willing to use logging subsystem from several + * threads. Once enabled, it's not possible to disable it again. + */ +void log_enable_multithread(void) { + if (osmo_log_tgt_mutex_on) + return; + pthread_mutex_init(&osmo_log_tgt_mutex, NULL); + osmo_log_tgt_mutex_on = true; +} + +/*! Acquire the osmo_log_tgt_mutex. Don't use this function directly, always use + * macro log_tgt_mutex_lock() instead. + */ +void log_tgt_mutex_lock_impl(void) { + /* These lines are useful to debug scenarios where there's only 1 thread + and a double lock appears, for instance during startup and some + unlock() missing somewhere: + if (osmo_log_tgt_mutex_on && pthread_mutex_trylock(&osmo_log_tgt_mutex) != 0) + osmo_panic("acquiring already locked mutex!\n"); + return; + */ + + if (osmo_log_tgt_mutex_on) + pthread_mutex_lock(&osmo_log_tgt_mutex); +} + +/*! Release the osmo_log_tgt_mutex. Don't use this function directly, always use + * macro log_tgt_mutex_unlock() instead. + */ +void log_tgt_mutex_unlock_impl(void) { + if (osmo_log_tgt_mutex_on) + pthread_mutex_unlock(&osmo_log_tgt_mutex); +} + +#else /* if (!EMBEDDED) */ +#pragma message ("logging multithread support disabled in embedded build") +void log_enable_multithread(void) {} +void log_tgt_mutex_lock_impl(void) {} +void log_tgt_mutex_unlock_impl(void) {} +#endif /* if (!EMBEDDED) */ + +const struct value_string loglevel_strs[] = { + { LOGL_DEBUG, "DEBUG" }, + { LOGL_INFO, "INFO" }, + { LOGL_NOTICE, "NOTICE" }, + { LOGL_ERROR, "ERROR" }, + { LOGL_FATAL, "FATAL" }, + { 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 */ + .name = "DLGLOBAL", + .description = "Library-internal global log family", + .loglevel = LOGL_NOTICE, + .enabled = 1, + }, + [INT2IDX(DLLAPD)] = { /* -2 becomes 1 */ + .name = "DLLAPD", + .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[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", + }, +}; + +void assert_loginfo(const char *src) +{ + if (!osmo_log_info) { + fprintf(stderr, "ERROR: osmo_log_info == NULL! " + "You must call log_init() before using logging in %s()!\n", src); + OSMO_ASSERT(osmo_log_info); + } +} + +/* special magic for negative (library-internal) log subsystem numbers */ +static int subsys_lib2index(int subsys) +{ + return (subsys * -1) + (osmo_log_info->num_cat_user-1); +} + +/*! Parse a human-readable log level into a numeric value + * \param[in] lvl zero-terminated string containing log level name + * \returns numeric log level + */ +int log_parse_level(const char *lvl) +{ + return get_string_value(loglevel_strs, lvl); +} + +/*! convert a numeric log level into human-readable string + * \param[in] lvl numeric log level + * \returns zero-terminated string (log level name) + */ +const char *log_level_str(unsigned int lvl) +{ + return get_value_string(loglevel_strs, lvl); +} + +/*! parse a human-readable log category into numeric form + * \param[in] category human-readable log category name + * \returns numeric category value, or -EINVAL otherwise + */ +int log_parse_category(const char *category) +{ + int i; + + assert_loginfo(__func__); + + for (i = 0; i < osmo_log_info->num_cat; ++i) { + if (osmo_log_info->cat[i].name == NULL) + continue; + if (!strcasecmp(osmo_log_info->cat[i].name+1, category)) + return i; + } + + return -EINVAL; +} + +/*! parse the log category mask + * \param[in] target log target to be configured + * \param[in] _mask log category mask string + * + * The format can be this: category1:category2:category3 + * or category1,2:category2,3:... + */ +void log_parse_category_mask(struct log_target* target, const char *_mask) +{ + int i = 0; + char *mask = strdup(_mask); + char *category_token = NULL; + + assert_loginfo(__func__); + + /* Disable everything to enable it afterwards */ + for (i = 0; i < osmo_log_info->num_cat; ++i) + target->categories[i].enabled = 0; + + category_token = strtok(mask, ":"); + OSMO_ASSERT(category_token); + do { + for (i = 0; i < osmo_log_info->num_cat; ++i) { + size_t length, cat_length; + char* colon = strstr(category_token, ","); + + if (!osmo_log_info->cat[i].name) + continue; + + length = strlen(category_token); + cat_length = strlen(osmo_log_info->cat[i].name); + + /* Use longest length not to match subocurrences. */ + if (cat_length > length) + length = cat_length; + + if (colon) + length = colon - category_token; + + if (strncasecmp(osmo_log_info->cat[i].name, + category_token, length) == 0) { + int level = 0; + + if (colon) + level = atoi(colon+1); + + target->categories[i].enabled = 1; + target->categories[i].loglevel = level; + } + } + } while ((category_token = strtok(NULL, ":"))); + + free(mask); +} + +static const char* color(int subsys) +{ + if (subsys < osmo_log_info->num_cat) + return osmo_log_info->cat[subsys].color; + + return NULL; +} + +static const struct value_string level_colors[] = { + { 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 } +}; + +static const char *level_color(int level) +{ + const char *c = get_value_string_or_null(level_colors, level); + if (!c) + return get_value_string(level_colors, LOGL_FATAL); + return c; +} + +const char* log_category_name(int subsys) +{ + if (subsys < osmo_log_info->num_cat) + return osmo_log_info->cat[subsys].name; + + return NULL; +} + +static const char *const_basename(const char *path) +{ + const char *bn = strrchr(path, '/'); + if (!bn || !bn[1]) + return path; + return bn + 1; +} + +/*! 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) +{ + int ret, len = 0, offset = 0, rem = buf_len; + const char *c_subsys = NULL; + + /* 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 (!cont) { + if (target->print_ext_timestamp) { +#ifdef HAVE_LOCALTIME_R + struct tm tm; + 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); +#endif + } else if (target->print_timestamp) { + time_t tm; + if ((tm = time(NULL)) == (time_t) -1) + 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)) + goto err; + ret = strlen(buf + offset); + if (ret <= 0) + goto err; + /* 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_tid) { + if (logging_tid == 0) + logging_tid = (long int)osmo_gettid(); + ret = snprintf(buf + offset, rem, "%ld ", logging_tid); + if (ret < 0) + goto err; + 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 ? OSMO_LOGCOLOR_END : "", + 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 ? OSMO_LOGCOLOR_END : "", + c_subsys ? c_subsys : ""); + if (ret < 0) + goto err; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + } + 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_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); + 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); + break; + } + } + } + ret = vsnprintf(buf + offset, rem, format, ap); + if (ret < 0) + goto err; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + + /* 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') { + switch (target->print_filename2) { + case LOG_FILENAME_NONE: + break; + case LOG_FILENAME_PATH: + offset--; + len--; + ret = snprintf(buf + offset, rem, " (%s:%d)\n", file, line); + if (ret < 0) + goto err; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + break; + case LOG_FILENAME_BASENAME: + offset--; + len--; + ret = snprintf(buf + offset, rem, " (%s:%d)\n", const_basename(file), line); + if (ret < 0) + goto err; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + break; + } + } + + if (target->use_color && c_subsys) { + ret = snprintf(buf + offset, rem, OSMO_LOGCOLOR_END); + if (ret < 0) + goto err; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + } +err: + len = OSMO_MIN(buf_len - 1, len); + buf[len] = '\0'; + return len; +} + +/* 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. + * For internal categories, the ID is negative starting with -1; and internal + * logging categories are added behind the user categories. For out-of-bounds + * indexes, return the index of DLGLOBAL. The returned category index is + * guaranteed to exist in osmo_log_info, otherwise the program would abort, + * which should never happen unless even the DLGLOBAL category is missing. */ +static inline int map_subsys(int subsys) +{ + /* Note: comparing signed and unsigned integers */ + + if (subsys > 0 && ((unsigned int)subsys) >= osmo_log_info->num_cat_user) + subsys = DLGLOBAL; + + if (subsys < 0) + subsys = subsys_lib2index(subsys); + + if (subsys < 0 || subsys >= osmo_log_info->num_cat) + subsys = subsys_lib2index(DLGLOBAL); + + OSMO_ASSERT(!(subsys < 0 || subsys >= osmo_log_info->num_cat)); + + return subsys; +} + +static inline bool should_log_to_target(struct log_target *tar, int subsys, + int level) +{ + struct log_category *category; + + category = &tar->categories[subsys]; + + /* subsystem is not supposed to be logged */ + if (!category->enabled) + return false; + + /* Check the global log level */ + if (tar->loglevel != 0 && level < tar->loglevel) + return false; + + /* Check the category log level */ + if (tar->loglevel == 0 && category->loglevel != 0 && + level < category->loglevel) + return false; + + /* Apply filters here... if that becomes messy we will + * need to put filters in a list and each filter will + * say stop, continue, output */ + if ((tar->filter_map & (1 << LOG_FLT_ALL)) != 0) + return true; + + if (osmo_log_info->filter_fn) + return osmo_log_info->filter_fn(&log_context, tar); + + /* TODO: Check the filter/selector too? */ + return true; +} + +/*! vararg version of logging function + * \param[in] subsys Logging sub-system + * \param[in] level Log level + * \param[in] file name of source code file + * \param[in] cont continuation (1) or new line (0) + * \param[in] format format string + * \param[in] ap vararg-list containing format string arguments + */ +void osmo_vlogp(int subsys, int level, const char *file, int line, + int cont, const char *format, va_list ap) +{ + struct log_target *tar; + + subsys = map_subsys(subsys); + + log_tgt_mutex_lock(); + + llist_for_each_entry(tar, &osmo_log_target_list, entry) { + va_list bp; + + if (!should_log_to_target(tar, subsys, level)) + continue; + + /* According to the manpage, vsnprintf leaves the value of ap + * in undefined state. Since _output uses vsnprintf and it may + * be called several times, we have to pass a copy of ap. */ + va_copy(bp, ap); + if (tar->raw_output) + tar->raw_output(tar, subsys, level, file, line, cont, format, bp); + else + _output(tar, subsys, level, file, line, cont, format, bp); + va_end(bp); + } + + log_tgt_mutex_unlock(); +} + +/*! logging function used by DEBUGP() macro + * \param[in] subsys Logging sub-system + * \param[in] file name of source code file + * \param[in] cont continuation (1) or new line (0) + * \param[in] format format string + */ +void logp(int subsys, const char *file, int line, int cont, + const char *format, ...) +{ + va_list ap; + + va_start(ap, format); + osmo_vlogp(subsys, LOGL_DEBUG, file, line, cont, format, ap); + va_end(ap); +} + +/*! logging function used by LOGP() macro + * \param[in] subsys Logging sub-system + * \param[in] level Log level + * \param[in] file name of source code file + * \param[in] cont continuation (1) or new line (0) + * \param[in] format format string + */ +void logp2(int subsys, unsigned int level, const char *file, int line, int cont, const char *format, ...) +{ + 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 + * \param[in] target Log target to be registered + */ +void log_add_target(struct log_target *target) +{ + llist_add_tail(&target->entry, &osmo_log_target_list); +} + +/*! Unregister a log target from the logging core + * \param[in] target Log target to be unregistered + */ +void log_del_target(struct log_target *target) +{ + llist_del(&target->entry); +} + +/*! Reset (clear) the logging context */ +void log_reset_context(void) +{ + memset(&log_context, 0, sizeof(log_context)); +} + +/*! Set the logging context + * \param[in] ctx_nr logging context number + * \param[in] value value to which the context is to be set + * \returns 0 in case of success; negative otherwise + * + * A logging context is something like the subscriber identity to which + * the currently processed message relates, or the BTS through which it + * was received. As soon as this data is known, it can be set using + * this function. The main use of context information is for logging + * filters. + */ +int log_set_context(uint8_t ctx_nr, void *value) +{ + if (ctx_nr > LOG_MAX_CTX) + return -EINVAL; + + log_context.ctx[ctx_nr] = value; + + return 0; +} + +/*! Enable the \ref LOG_FLT_ALL log filter + * \param[in] target Log target to be affected + * \param[in] all enable (1) or disable (0) the ALL filter + * + * When the \ref LOG_FLT_ALL filter is enabled, all log messages will be + * printed. It acts as a wildcard. Setting it to \a 1 means there is no + * filtering. + */ +void log_set_all_filter(struct log_target *target, int all) +{ + if (all) + target->filter_map |= (1 << LOG_FLT_ALL); + else + target->filter_map &= ~(1 << LOG_FLT_ALL); +} + +/*! Enable or disable the use of colored output + * \param[in] target Log target to be affected + * \param[in] use_color Use color (1) or don't use color (0) + */ +void log_set_use_color(struct log_target *target, int use_color) +{ + target->use_color = use_color; +} + +/*! Enable or disable printing of timestamps while logging + * \param[in] target Log target to be affected + * \param[in] print_timestamp Enable (1) or disable (0) timestamps + */ +void log_set_print_timestamp(struct log_target *target, int print_timestamp) +{ + target->print_timestamp = print_timestamp; +} + +/*! Enable or disable printing of extended timestamps while logging + * \param[in] target Log target to be affected + * \param[in] print_timestamp Enable (1) or disable (0) timestamps + * + * When both timestamp and extended timestamp is enabled then only + * the extended timestamp will be used. The format of the timestamp + * is YYYYMMDDhhmmssnnn. + */ +void log_set_print_extended_timestamp(struct log_target *target, int print_timestamp) +{ + 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 + * behavior, which combined the category in hex with the filename. For example, if the category-hex + * output were no longer affected by log_set_print_filename(), many unit tests (in libosmocore as well as + * dependent projects) would fail since they expect the category to disappear along with the filename. + * \param[in] target Log target to be affected + * \param[in] print_filename Enable (1) or disable (0) filenames + */ +void log_set_print_filename(struct log_target *target, int print_filename) +{ + log_set_print_filename2(target, print_filename ? LOG_FILENAME_PATH : LOG_FILENAME_NONE); + log_set_print_category_hex(target, print_filename); +} + +/*! Enable or disable printing of the filename while logging. + * \param[in] target Log target to be affected. + * \param[in] lft An LOG_FILENAME_* enum value. + * LOG_FILENAME_NONE omits the source file and line information from logs. + * LOG_FILENAME_PATH prints the entire source file path as passed to LOGP macros. + */ +void log_set_print_filename2(struct log_target *target, enum log_filename_type lft) +{ + target->print_filename2 = lft; +} + +/*! Set the position where on a log line the source file info should be logged. + * \param[in] target Log target to be affected. + * \param[in] pos A LOG_FILENAME_POS_* enum value. + * LOG_FILENAME_POS_DEFAULT logs just before the caller supplied log message. + * LOG_FILENAME_POS_LAST logs only at the end of a log line, where the caller issued an '\n' to end the + */ +void log_set_print_filename_pos(struct log_target *target, enum log_filename_pos pos) +{ + target->print_filename_pos = pos; +} + +/*! Enable or disable printing of the category name + * \param[in] target Log target to be affected + * \param[in] print_category Enable (1) or disable (0) filenames + * + * Print the category/subsys name in front of every log message. + */ +void log_set_print_category(struct log_target *target, int print_category) +{ + target->print_category = print_category; +} + +/*! Enable or disable printing of the category number in hex ('<000b>'). + * \param[in] target Log target to be affected. + * \param[in] print_category_hex Enable (1) or disable (0) hex category. + */ +void log_set_print_category_hex(struct log_target *target, int print_category_hex) +{ + target->print_category_hex = print_category_hex; +} + +/*! Enable or disable printing of the log level name. + * \param[in] target Log target to be affected + * \param[in] print_level Enable (1) or disable (0) log level name + * + * Print the log level name in front of every log message. + */ +void log_set_print_level(struct log_target *target, int print_level) +{ + target->print_level = (bool)print_level; +} + +/*! Set the global log level for a given log target + * \param[in] target Log target to be affected + * \param[in] log_level New global log level + */ +void log_set_log_level(struct log_target *target, int log_level) +{ + target->loglevel = log_level; +} + +/*! Set a category filter on a given log target + * \param[in] target Log target to be affected + * \param[in] category Log category to be affected + * \param[in] enable whether to enable or disable the filter + * \param[in] level Log level of the filter + */ +void log_set_category_filter(struct log_target *target, int category, + int enable, int level) +{ + if (!target) + return; + category = map_subsys(category); + target->categories[category].enabled = !!enable; + target->categories[category].loglevel = level; +} + +#if (!EMBEDDED) +/* 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) +{ + 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 + * \returns dynamically-allocated log target + * This funcition allocates a \ref log_target and initializes it + * with some default values. The newly created target is not + * registered yet. + */ +struct log_target *log_target_create(void) +{ + struct log_target *target; + unsigned int i; + + assert_loginfo(__func__); + + target = talloc_zero(tall_log_ctx, struct log_target); + if (!target) + return NULL; + + target->categories = talloc_zero_array(target, + struct log_category, + osmo_log_info->num_cat); + if (!target->categories) { + talloc_free(target); + return NULL; + } + + INIT_LLIST_HEAD(&target->entry); + + /* initialize the per-category enabled/loglevel from defaults */ + for (i = 0; i < osmo_log_info->num_cat; i++) { + struct log_category *cat = &target->categories[i]; + cat->enabled = osmo_log_info->cat[i].enabled; + cat->loglevel = osmo_log_info->cat[i].loglevel; + } + + /* 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; + return target; +} + +/*! Create the STDERR log target + * \returns dynamically-allocated \ref log_target for STDERR */ +struct log_target *log_target_create_stderr(void) +{ +/* since C89/C99 says stderr is a macro, we can safely do this! */ +#if !EMBEDDED && defined(stderr) + struct log_target *target; + + target = log_target_create(); + if (!target) + return NULL; + + target->type = LOG_TGT_TYPE_STDERR; + target->tgt_file.out = stderr; + target->output = _file_output_stream; + return target; +#else + return NULL; +#endif /* stderr */ +} + +#if (!EMBEDDED) +/*! 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_stream(const char *fname) +{ + struct log_target *target; + + target = log_target_create(); + if (!target) + return NULL; + + target->type = LOG_TGT_TYPE_FILE; + target->tgt_file.out = fopen(fname, "a"); + 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; +} + +/*! 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; +} +#endif + +/*! Find a registered log target + * \param[in] type Log target type + * \param[in] fname File name + * \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(enum log_target_type type, const char *fname) +{ + struct log_target *tgt; + + llist_for_each_entry(tgt, &osmo_log_target_list, entry) { + if (tgt->type != type) + continue; + switch (tgt->type) { + case LOG_TGT_TYPE_FILE: + if (!strcmp(fname, tgt->tgt_file.fname)) + return tgt; + break; + case LOG_TGT_TYPE_GSMTAP: + if (!strcmp(fname, tgt->tgt_gsmtap.hostname)) + return tgt; + break; + default: + return tgt; + } + } + return NULL; +} + +/*! Unregister, close and delete a log target + * \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) + 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 + + talloc_free(target); +} + +/*! close and re-open a log file (for log file rotation) + * \param[in] target log target to re-open + * \returns 0 in case of success; negative otherwise */ +int log_target_file_reopen(struct log_target *target) +{ + 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; + } + + 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; +} + +/*! close and re-open all log files (for log file rotation) + * \returns 0 in case of success; negative otherwise */ +int log_targets_reopen(void) +{ + struct log_target *tar; + int rc = 0; + + log_tgt_mutex_lock(); + + llist_for_each_entry(tar, &osmo_log_target_list, entry) { + switch (tar->type) { + case LOG_TGT_TYPE_FILE: + if (log_target_file_reopen(tar) < 0) + rc = -1; + break; + default: + break; + } + } + + log_tgt_mutex_unlock(); + + return rc; +} + +/*! Initialize the Osmocom logging core + * \param[in] inf Information regarding logging categories, could be NULL + * \param[in] ctx talloc context for logging allocations + * \returns 0 in case of success, negative in case of error + * + * If inf is NULL then only library-internal categories are initialized. + */ +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) + return -ENOMEM; + + osmo_log_info = talloc_zero(tall_log_ctx, struct log_info); + if (!osmo_log_info) + return -ENOMEM; + + osmo_log_info->num_cat = ARRAY_SIZE(internal_cat); + + if (inf) { + osmo_log_info->filter_fn = inf->filter_fn; + osmo_log_info->num_cat_user = inf->num_cat; + osmo_log_info->num_cat += inf->num_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; + } + + /* copy over the user part and sanitize loglevel */ + if (inf) { + for (i = 0; i < inf->num_cat; i++) { + 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(&cat_ptr[cn], &internal_cat[i], sizeof(struct log_info_cat)); + } + + osmo_log_info->cat = cat_ptr; + + return 0; +} + +/* De-initialize the Osmocom logging core + * This function destroys all targets and releases associated memory */ +void log_fini(void) +{ + struct log_target *tar, *tar2; + + log_tgt_mutex_lock(); + + llist_for_each_entry_safe(tar, tar2, &osmo_log_target_list, entry) + log_target_destroy(tar); + + talloc_free(osmo_log_info); + osmo_log_info = NULL; + talloc_free(tall_log_ctx); + tall_log_ctx = NULL; + + log_tgt_mutex_unlock(); +} + +/*! Check whether a log entry will be generated. + * \returns != 0 if a log entry might get generated by at least one target */ +int log_check_level(int subsys, unsigned int level) +{ + struct log_target *tar; + + assert_loginfo(__func__); + + subsys = map_subsys(subsys); + + /* TODO: The following could/should be cached (update on config) */ + + log_tgt_mutex_lock(); + + llist_for_each_entry(tar, &osmo_log_target_list, entry) { + if (!should_log_to_target(tar, subsys, level)) + continue; + + /* This might get logged (ignoring filters) */ + log_tgt_mutex_unlock(); + return 1; + } + + /* We are sure, that this will not be logged. */ + log_tgt_mutex_unlock(); + return 0; +} + +/*! @} */ diff --git a/src/core/logging_gsmtap.c b/src/core/logging_gsmtap.c new file mode 100644 index 00000000..dfd059b7 --- /dev/null +++ b/src/core/logging_gsmtap.c @@ -0,0 +1,161 @@ +/*! \file logging_gsmtap.c + * libosmocore log output encapsulated in GSMTAP. + * + * Encapsulating the log output inside GSMTAP frames allows us to + * observer protocol traces (of Um, Abis, A or any other interface in + * the Osmocom world) with synchronous interspersed log messages. + */ +/* + * (C) 2016 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 logging + * @{ + * \file logging_gsmtap.c */ + +#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> +#endif + +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/gsmtap.h> +#include <osmocom/core/gsmtap_util.h> +#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, + va_list ap) +{ + struct msgb *msg; + struct gsmtap_hdr *gh; + struct gsmtap_osmocore_log_hdr *golh; + const char *subsys_name = log_category_name(subsys); + struct timeval tv; + int rc; + const char *file_basename; + + /* get timestamp ASAP */ + osmo_gettimeofday(&tv, NULL); + + msg = msgb_alloc(sizeof(*gh)+sizeof(*golh)+GSMTAP_LOG_MAX_SIZE, + "GSMTAP logging"); + + /* GSMTAP header */ + gh = (struct gsmtap_hdr *) msgb_put(msg, sizeof(*gh)); + memset(gh, 0, sizeof(*gh)); + gh->version = GSMTAP_VERSION; + gh->hdr_len = sizeof(*gh)/4; + gh->type = GSMTAP_TYPE_OSMOCORE_LOG; + + /* 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 + golh->subsys[0] = '\0'; + + /* strip all leading path elements from file, if any. */ + file_basename = strrchr(file, '/'); + file = (file_basename && file_basename[1])? file_basename + 1 : file; + OSMO_STRLCPY_ARRAY(golh->src_file.name, file); + golh->src_file.line_nr = osmo_htonl(line); + golh->level = level; + /* we always store the timestamp in the message, irrespective + * of hat prrint_[ext_]timestamp say */ + golh->ts.sec = osmo_htonl(tv.tv_sec); + golh->ts.usec = osmo_htonl(tv.tv_usec); + + rc = vsnprintf((char *) msg->tail, msgb_tailroom(msg), format, ap); + if (rc < 0) { + msgb_free(msg); + return; + } else if (rc >= msgb_tailroom(msg)) { + /* If the output was truncated, vsnprintf() returns the + * number of characters which would have been written + * if enough space had been available (excluding '\0'). */ + rc = msgb_tailroom(msg); + msg->tail[rc - 1] = '\0'; + } + msgb_put(msg, rc); + + rc = gsmtap_sendmsg(target->tgt_gsmtap.gsmtap_inst, msg); + if (rc) + msgb_free(msg); +} + +/*! Create a new logging target for GSMTAP logging + * \param[in] host remote host to send the logs to + * \param[in] port remote port to send the logs to + * \param[in] ident string identifier + * \param[in] ofd_wq_mode register osmo_wqueue (1) or not (0) + * \param[in] add_sink add GSMTAP sink or not + * \returns Log target in case of success, NULL in case of error + */ +struct log_target *log_target_create_gsmtap(const char *host, uint16_t port, + const char *ident, + bool ofd_wq_mode, + bool add_sink) +{ + struct log_target *target; + struct gsmtap_inst *gti; + + target = log_target_create(); + if (!target) + return NULL; + + gti = gsmtap_source_init(host, port, ofd_wq_mode); + if (!gti) { + log_target_destroy(target); + return NULL; + } + + if (add_sink) + gsmtap_source_add_sink(gti); + + target->tgt_gsmtap.gsmtap_inst = gti; + target->tgt_gsmtap.ident = talloc_strdup(target, ident); + target->tgt_gsmtap.hostname = talloc_strdup(target, host); + + target->type = LOG_TGT_TYPE_GSMTAP; + target->raw_output = _gsmtap_raw_output; + + return target; +} + +/* @} */ diff --git a/src/core/logging_syslog.c b/src/core/logging_syslog.c new file mode 100644 index 00000000..1153bdf4 --- /dev/null +++ b/src/core/logging_syslog.c @@ -0,0 +1,89 @@ +/*! \file logging_syslog.c + * Syslog logging support code. */ +/* + * (C) 2011 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 logging + * @{ + * \file logging_syslog.c */ + +#include "config.h" + +#ifdef HAVE_SYSLOG_H + +#include <stdarg.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <syslog.h> + +#ifdef HAVE_STRINGS_H +#include <strings.h> +#endif + +#include <osmocom/core/talloc.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> + +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 _syslog_output(struct log_target *target, + unsigned int level, const char *log) +{ + syslog(logp2syslog_level(level), "%s", log); +} + +/*! Create a new logging target for syslog logging + * \param[in] ident syslog string identifier + * \param[in] option syslog options + * \param[in] facility syslog facility + * \returns Log target in case of success, NULL in case of error + */ +struct log_target *log_target_create_syslog(const char *ident, int option, + int facility) +{ + struct log_target *target; + + target = log_target_create(); + if (!target) + return NULL; + + target->tgt_syslog.facility = facility; + target->type = LOG_TGT_TYPE_SYSLOG; + target->output = _syslog_output; + + openlog(ident, option, facility); + + return target; +} + +#endif /* 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/core/loggingrb.c b/src/core/loggingrb.c new file mode 100644 index 00000000..2bf7b665 --- /dev/null +++ b/src/core/loggingrb.c @@ -0,0 +1,102 @@ +/*! \file loggingrb.c + * Ringbuffer-backed logging support code. */ +/* + * (C) 2012-2013 by Katerina Barone-Adesi + * 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 loggingrb + * @{ + * This adds a log which consist of an in-memory ring buffer. The idea + * is that the user can configure his logging in a way that critical + * messages get stored in the ring buffer, and that the last few + * critical messages can then always obtained by dumping the ring + * buffer. It can hence be used as a more generic version of the + * "show me the last N alarms" functionality. + * + * \file loggingrb.c */ + +#include <osmocom/core/strrb.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/loggingrb.h> + +static void _rb_output(struct log_target *target, + unsigned int level, const char *log) +{ + osmo_strrb_add(target->tgt_rb.rb, log); +} + +/*! Return the number of log strings in the osmo_strrb-backed target. + * \param[in] target The target to search. + * + * \return The number of log strings in the osmo_strrb-backed target. + */ +size_t log_target_rb_used_size(struct log_target const *target) +{ + return osmo_strrb_elements(target->tgt_rb.rb); +} + +/*! Return the capacity of the osmo_strrb-backed target. + * \param[in] target The target to search. + * + * Note that this is the capacity (aka max number of messages). + * It is not the number of unused message slots. + * \return The number of log strings in the osmo_strrb-backed target. + */ +size_t log_target_rb_avail_size(struct log_target const *target) +{ + struct osmo_strrb *rb = target->tgt_rb.rb; + return rb->size - 1; +} + +/*! Return the nth log entry in a target. + * \param[in] target The target to search. + * \param[in] logindex The index of the log entry/error message. + * + * \return A pointer to the nth message, or NULL if logindex is invalid. + */ +const char *log_target_rb_get(struct log_target const *target, size_t logindex) +{ + return osmo_strrb_get_nth(target->tgt_rb.rb, logindex); +} + +/*! Create a new logging target for ringbuffer-backed logging. + * \param[in] size The capacity (number of messages) of the logging target. + * \returns A log target in case of success, NULL in case of error. + */ +struct log_target *log_target_create_rb(size_t size) +{ + struct log_target *target; + struct osmo_strrb *rb; + + target = log_target_create(); + if (!target) + return NULL; + + rb = osmo_strrb_create(target, size + 1); + if (!rb) { + log_target_destroy(target); + return NULL; + } + + target->tgt_rb.rb = rb; + target->type = LOG_TGT_TYPE_STRRB; + target->output = _rb_output; + + return target; +} + +/* @} */ diff --git a/src/core/macaddr.c b/src/core/macaddr.c new file mode 100644 index 00000000..3b231fb8 --- /dev/null +++ b/src/core/macaddr.c @@ -0,0 +1,149 @@ +/*! \file macaddr.c + * MAC address utility routines. */ +/* + * (C) 2013-2014 by Harald Welte <laforge@gnumonks.org> + * (C) 2014 by Holger Hans Peter Freyther + * + * 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 utils + * @{ + * \file macaddr.c */ + +#include "config.h" + +#include <stdint.h> +#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 + * hex/colon notation (00:00:00:00:00:00) and generates the binary + * representation from it. + * \param[out] out pointer to caller-allocated buffer of 6 bytes + * \param[in] in pointer to input data as string with hex/colon notation + */ +int osmo_macaddr_parse(uint8_t *out, const char *in) +{ + /* 00:00:00:00:00:00 */ + char tmp[18]; + char *tok; + unsigned int i = 0; + + if (strlen(in) < 17) + return -1; + + strncpy(tmp, in, sizeof(tmp)-1); + tmp[sizeof(tmp)-1] = '\0'; + + for (tok = strtok(tmp, ":"); tok && (i < 6); tok = strtok(NULL, ":")) { + unsigned long ul = strtoul(tok, NULL, 16); + out[i++] = ul & 0xff; + } + + return 0; +} + +#if defined(__FreeBSD__) || defined(__APPLE__) +#include <sys/socket.h> +#include <sys/types.h> +#include <ifaddrs.h> +#include <net/if_dl.h> +#include <net/if_types.h> + +/*! Obtain the MAC address of a given network device + * \param[out] mac_out pointer to caller-allocated buffer of 6 bytes + * \param[in] dev_name string name of the network device + * \returns 0 in case of success; negative otherwise + */ +int osmo_get_macaddr(uint8_t *mac_out, const char *dev_name) +{ + struct ifaddrs *ifa, *ifaddr; + int rc = -ENODEV; + + if (getifaddrs(&ifaddr) != 0) + return -errno; + + for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { + struct sockaddr_dl *sdl; + + sdl = (struct sockaddr_dl *) ifa->ifa_addr; + if (!sdl) + continue; + if (sdl->sdl_family != AF_LINK) + continue; + if (sdl->sdl_type != IFT_ETHER) + continue; + if (strcmp(ifa->ifa_name, dev_name) != 0) + continue; + + memcpy(mac_out, LLADDR(sdl), 6); + rc = 0; + break; + } + + freeifaddrs(ifaddr); + return rc; +} + +#else + +#if (!EMBEDDED) + +#include <sys/ioctl.h> +#include <net/if.h> +#include <netinet/in.h> +#include <netinet/ip.h> +#include <errno.h> + +/*! Obtain the MAC address of a given network device + * \param[out] mac_out pointer to caller-allocated buffer of 6 bytes + * \param[in] dev_name string name of the network device + * \returns 0 in case of success; negative otherwise + */ +int osmo_get_macaddr(uint8_t *mac_out, const char *dev_name) +{ + int fd, rc, dev_len; + struct ifreq ifr; + + dev_len = strlen(dev_name); + if (dev_len >= sizeof(ifr.ifr_name)) + return -EINVAL; + + fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP); + if (fd < 0) + return fd; + + memset(&ifr, 0, sizeof(ifr)); + memcpy(&ifr.ifr_name, dev_name, dev_len + 1); + rc = ioctl(fd, SIOCGIFHWADDR, &ifr); + close(fd); + + if (rc < 0) + return rc; + + memcpy(mac_out, ifr.ifr_hwaddr.sa_data, 6); + + return 0; +} +#endif /* !EMBEDDED */ + +#endif + +/*! @} */ 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/core/msgb.c b/src/core/msgb.c new file mode 100644 index 00000000..713510c6 --- /dev/null +++ b/src/core/msgb.c @@ -0,0 +1,607 @@ +/* (C) 2008 by Harald Welte <laforge@gnumonks.org> + * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.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 msgb + * @{ + * + * libosmocore message buffers, inspired by Linux kernel skbuff + * + * Inspired by the 'struct skbuff' of the Linux kernel, we implement a + * 'struct msgb' which we use for handling network + * packets aka messages aka PDUs. + * + * A msgb consists of + * * a header with some metadata, such as + * * a linked list header for message queues or the like + * * pointers to the headers of various protocol layers inside + * the packet + * * a data section consisting of + * * headroom, i.e. space in front of the message, to allow + * for additional headers being pushed in front of the current + * data + * * the currently occupied data for the message + * * tailroom, i.e. space at the end of the message, to + * allow more data to be added after the end of the current + * data + * + * We have plenty of utility functions around the \ref msgb: + * * allocation / release + * * enqueue / dequeue from/to message queues + * * prepending (pushing) and appending (putting) data + * * copying / resizing + * * hex-dumping to a string for debug purposes + * + * \file msgb.c + */ + +#include <unistd.h> +#include <string.h> +#include <stdlib.h> +#include <inttypes.h> +#include <stdarg.h> +#include <errno.h> + +#include <osmocom/core/msgb.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/logging.h> + +/*! 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 + * \returns dynamically-allocated \ref msgb + * + * This function allocates a 'struct msgb' as well as the underlying + * memory buffer for the actual message data (size specified by \a size) + * using the talloc memory context previously set by \ref msgb_set_talloc_ctx + */ +struct msgb *msgb_alloc_c(const void *ctx, uint16_t size, const char *name) +{ + struct msgb *msg; + + msg = talloc_named_const(ctx, sizeof(*msg) + size, name); + if (!msg) { + LOGP(DLGLOBAL, LOGL_FATAL, "Unable to allocate a msgb: " + "name='%s', size=%u\n", name, size); + return NULL; + } + + /* Manually zero-initialize allocated memory */ + memset(msg, 0x00, sizeof(*msg) + size); + + msg->data_len = size; + msg->len = 0; + msg->data = msg->_data; + msg->head = msg->_data; + msg->tail = msg->_data; + + return msg; +} + +/* default msgb allocation context for msgb_alloc() */ +void *tall_msgb_ctx = NULL; + +/*! Allocate a new message buffer from tall_msgb_ctx + * \param[in] size Length in octets, including headroom + * \param[in] name Human-readable name to be associated with msgb + * \returns dynamically-allocated \ref msgb + * + * This function allocates a 'struct msgb' as well as the underlying + * memory buffer for the actual message data (size specified by \a size) + * using the talloc memory context previously set by \ref msgb_set_talloc_ctx + */ +struct msgb *msgb_alloc(uint16_t size, const char *name) +{ + return msgb_alloc_c(tall_msgb_ctx, size, name); +} + + +/*! Release given message buffer + * \param[in] m Message buffer to be freed + */ +void msgb_free(struct msgb *m) +{ + talloc_free(m); +} + +/*! Enqueue message buffer to tail of a queue + * \param[in] queue linked list header of queue + * \param[in] msg message buffer to be added to the queue + * + * The function will append the specified message buffer \a msg to the + * queue implemented by \ref llist_head \a queue + */ +void msgb_enqueue(struct llist_head *queue, struct msgb *msg) +{ + llist_add_tail(&msg->list, queue); +} + +/*! Dequeue message buffer from head of queue + * \param[in] queue linked list header of queue + * \returns message buffer (if any) or NULL if queue empty + * + * The function will remove the first message buffer from the queue + * implemented by \ref llist_head \a queue. + */ +struct msgb *msgb_dequeue(struct llist_head *queue) +{ + struct llist_head *lh; + + if (llist_empty(queue)) + return NULL; + + lh = queue->next; + + if (lh) { + llist_del(lh); + return llist_entry(lh, struct msgb, list); + } else + return NULL; +} + +/*! Re-set all message buffer pointers + * \param[in] msg message buffer that is to be resetted + * + * This will re-set the various internal pointers into the underlying + * message buffer, i.e. remove all headroom and treat the msgb as + * completely empty. It also initializes the control buffer to zero. + */ +void msgb_reset(struct msgb *msg) +{ + msg->len = 0; + msg->data = msg->_data; + msg->head = msg->_data; + msg->tail = msg->_data; + + msg->trx = NULL; + msg->lchan = NULL; + msg->l2h = NULL; + msg->l3h = NULL; + msg->l4h = NULL; + + memset(&msg->cb, 0, sizeof(msg->cb)); +} + +/*! get pointer to data section of message buffer + * \param[in] msg message buffer + * \returns pointer to data section of message buffer + */ +uint8_t *msgb_data(const struct msgb *msg) +{ + return msg->data; +} + +/*! Compare and print: check data in msgb against given data and print errors if any + * \param[in] file text prefix, usually __FILE__, ignored if print == false + * \param[in] line numeric prefix, usually __LINE__, ignored if print == false + * \param[in] func text prefix, usually __func__, ignored if print == false + * \param[in] level while layer (L1, L2 etc) data should be compared against + * \param[in] msg message buffer + * \param[in] data expected data + * \param[in] len length of data + * \param[in] print boolean indicating whether we should print anything to stdout + * \returns boolean indicating whether msgb content is equal to a given data + * + * This function is not intended to be called directly but rather used through corresponding macro wrappers. + */ +bool _msgb_eq(const char *file, size_t line, const char *func, uint8_t level, + const struct msgb *msg, const uint8_t *data, size_t len, bool print) +{ + const char *m_dump; + unsigned int m_len, i; + uint8_t *m_data; + + if (!msg) { + if (print) + LOGPSRC(DLGLOBAL, LOGL_FATAL, file, line, "%s() NULL msg comparison\n", func); + return false; + } + + if (!data) { + if (print) + LOGPSRC(DLGLOBAL, LOGL_FATAL, file, line, "%s() NULL comparison data\n", func); + return false; + } + + switch (level) { + case 0: + m_len = msgb_length(msg); + m_data = msgb_data(msg); + m_dump = print ? msgb_hexdump(msg) : NULL; + break; + case 1: + m_len = msgb_l1len(msg); + m_data = msgb_l1(msg); + m_dump = print ? msgb_hexdump_l1(msg) : NULL; + break; + case 2: + m_len = msgb_l2len(msg); + m_data = msgb_l2(msg); + m_dump = print ? msgb_hexdump_l2(msg) : NULL; + break; + case 3: + m_len = msgb_l3len(msg); + m_data = msgb_l3(msg); + m_dump = print ? msgb_hexdump_l3(msg) : NULL; + break; + case 4: + m_len = msgb_l4len(msg); + m_data = msgb_l4(msg); + m_dump = print ? msgb_hexdump_l4(msg) : NULL; + break; + default: + LOGPSRC(DLGLOBAL, LOGL_FATAL, file, line, + "%s() FIXME: unexpected comparison level %u\n", func, level); + return false; + } + + if (m_len != len) { + if (print) + LOGPSRC(DLGLOBAL, LOGL_FATAL, file, line, + "%s() Length mismatch: %d != %zu, %s\n", func, m_len, len, m_dump); + return false; + } + + if (memcmp(m_data, data, len) == 0) + return true; + + if (!print) + return false; + + LOGPSRC(DLGLOBAL, LOGL_FATAL, file, line, + "%s() L%u data mismatch:\nexpected %s\n ", func, level, osmo_hexdump(data, len)); + + for(i = 0; i < len; i++) + if (data[i] != m_data[i]) { + LOGPC(DLGLOBAL, LOGL_FATAL, "!!\n"); + break; + } else + LOGPC(DLGLOBAL, LOGL_FATAL, ".. "); + + LOGPC(DLGLOBAL, LOGL_FATAL, " msgb %s\n", osmo_hexdump(m_data, len)); + + return false; +} + +/*! get length of message buffer + * \param[in] msg message buffer + * \returns length of data section in message buffer + */ +uint16_t msgb_length(const struct msgb *msg) +{ + return msg->len; +} + +/*! Set the talloc context for \ref msgb_alloc + * Deprecated, use msgb_talloc_ctx_init() instead. + * \param[in] ctx talloc context to be used as root for msgb allocations + */ +void msgb_set_talloc_ctx(void *ctx) +{ + tall_msgb_ctx = ctx; +} + +/*! Initialize a msgb talloc context for \ref msgb_alloc. + * Create a talloc context called "msgb". If \a pool_size is 0, create a named + * const as msgb talloc context. If \a pool_size is nonzero, create a talloc + * pool, possibly for faster msgb allocations (see talloc_pool()). + * \param[in] root_ctx talloc context used as parent for the new "msgb" ctx. + * \param[in] pool_size if nonzero, create a talloc pool of this size. + * \returns the new msgb talloc context, e.g. for reporting + */ +void *msgb_talloc_ctx_init(void *root_ctx, unsigned int pool_size) +{ + if (!pool_size) + tall_msgb_ctx = talloc_size(root_ctx, 0); + else + tall_msgb_ctx = talloc_pool(root_ctx, pool_size); + talloc_set_name_const(tall_msgb_ctx, "msgb"); + return tall_msgb_ctx; +} + +/*! 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] ctx talloc context on which allocation happens + * \param[in] msg The old msgb object + * \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_resize_c(const void *ctx, const struct msgb *msg, uint16_t new_len, const char *name) +{ + struct msgb *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; + } + + new_msg = msgb_alloc_c(ctx, new_len, name); + if (!new_msg) + return NULL; + + /* copy header */ + new_msg->len = msg->len; + new_msg->data += msg->data - msg->_data; + 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) + new_msg->l2h = new_msg->_data + (msg->l2h - msg->_data); + if (msg->l3h) + new_msg->l3h = new_msg->_data + (msg->l3h - msg->_data); + if (msg->l4h) + new_msg->l4h = new_msg->_data + (msg->l4h - msg->_data); + + 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, + * 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 msgb + */ +struct msgb *msgb_copy(const struct msgb *msg, const char *name) +{ + return msgb_copy_c(tall_msgb_ctx, msg, name); +} + +/*! Resize an area within an msgb + * + * This resizes a sub area of the msgb data and adjusts the pointers (incl + * l1h-l4h) accordingly. The cb part is not updated. If the area is extended, + * the contents of the extension is undefined. The complete sub area must be a + * part of [data,tail]. + * + * \param[inout] msg The msgb object + * \param[in] area A pointer to the sub-area + * \param[in] old_size The old size of the sub-area + * \param[in] new_size The new size of the sub-area + * \returns 0 on success, -1 if there is not enough space to extend the area + */ +int msgb_resize_area(struct msgb *msg, uint8_t *area, + int old_size, int new_size) +{ + int rc; + uint8_t *post_start = area + old_size; + int pre_len = area - msg->data; + int post_len = msg->len - old_size - pre_len; + int delta_size = new_size - old_size; + + if (old_size < 0 || new_size < 0) + MSGB_ABORT(msg, "Negative sizes are not allowed\n"); + if (area < msg->data || post_start > msg->tail) + MSGB_ABORT(msg, "Sub area is not fully contained in the msg data\n"); + + if (delta_size == 0) + return 0; + + if (delta_size > 0) { + rc = msgb_trim(msg, msg->len + delta_size); + if (rc < 0) + return rc; + } + + memmove(area + new_size, area + old_size, post_len); + + if (msg->l1h >= post_start) + msg->l1h += delta_size; + if (msg->l2h >= post_start) + msg->l2h += delta_size; + if (msg->l3h >= post_start) + msg->l3h += delta_size; + if (msg->l4h >= post_start) + msg->l4h += delta_size; + + if (delta_size < 0) + msgb_trim(msg, msg->len + delta_size); + + return 0; +} + + +/*! fill user-provided buffer with hexdump of the msg. + * \param[out] buf caller-allocated buffer for output string + * \param[in] buf_len length of buf + * \param[in] msg message buffer to be dumped + * \returns buf + */ +char *msgb_hexdump_buf(char *buf, size_t buf_len, const struct msgb *msg) +{ + unsigned int buf_offs = 0; + int nchars; + const unsigned char *start = msg->data; + const unsigned char *lxhs[4]; + unsigned int i; + + lxhs[0] = msg->l1h; + lxhs[1] = msg->l2h; + lxhs[2] = msg->l3h; + lxhs[3] = msg->l4h; + + for (i = 0; i < ARRAY_SIZE(lxhs); i++) { + if (!lxhs[i]) + continue; + + if (lxhs[i] < msg->head) + continue; + if (lxhs[i] > msg->head + msg->data_len) + continue; + if (lxhs[i] > msg->tail) + continue; + if (lxhs[i] < msg->data || lxhs[i] > msg->tail) { + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "(L%d=data%+" PRIdPTR ") ", + i+1, lxhs[i] - msg->data); + buf_offs += nchars; + continue; + } + if (lxhs[i] < start) { + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "(L%d%+" PRIdPTR ") ", i+1, + start - lxhs[i]); + buf_offs += nchars; + continue; + } + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "%s[L%d]> ", + osmo_hexdump(start, lxhs[i] - start), + i+1); + if (nchars < 0 || nchars + buf_offs >= buf_len) + return "ERROR"; + + buf_offs += nchars; + start = lxhs[i]; + } + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "%s", osmo_hexdump(start, msg->tail - start)); + if (nchars < 0 || nchars + buf_offs >= buf_len) + return "ERROR"; + + buf_offs += nchars; + + for (i = 0; i < ARRAY_SIZE(lxhs); i++) { + if (!lxhs[i]) + continue; + + if (lxhs[i] < msg->head || lxhs[i] > msg->head + msg->data_len) { + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "(L%d out of range) ", i+1); + } else if (lxhs[i] <= msg->data + msg->data_len && + lxhs[i] > msg->tail) { + nchars = snprintf(buf + buf_offs, buf_len - buf_offs, + "(L%d=tail%+" PRIdPTR ") ", + i+1, lxhs[i] - msg->tail); + } else + continue; + + if (nchars < 0 || nchars + buf_offs >= buf_len) + return "ERROR"; + buf_offs += nchars; + } + + return buf; +} + +/*! Return a (static) buffer containing a hexdump of the msg. + * \param[in] msg message buffer + * \returns a pointer to a static char array + */ +const char *msgb_hexdump(const struct msgb *msg) +{ + static __thread char buf[4100]; + return msgb_hexdump_buf(buf, sizeof(buf), msg); +} + +/*! Return a dynamically allocated buffer containing a hexdump of the msg + * \param[in] ctx talloc context from where to allocate the output string + * \param[in] msg message buffer + * \returns a pointer to a static char array + */ +char *msgb_hexdump_c(const void *ctx, const struct msgb *msg) +{ + size_t buf_len = msgb_length(msg) * 3 + 100; + char *buf = talloc_size(ctx, buf_len); + if (!buf) + return NULL; + return msgb_hexdump_buf(buf, buf_len, msg); +} + +/*! Print a string to the end of message buffer. + * \param[in] msgb message buffer. + * \param[in] format format string. + * \returns 0 on success, -EINVAL on error. + * + * The resulting string is printed to the msgb without a trailing nul + * character. A nul following the data tail may be written as an implementation + * detail, but a trailing nul is never part of the msgb data in terms of + * msgb_length(). + * + * Note: the tailroom must always be one byte longer than the string to be + * written. The msgb is filled only up to tailroom=1. This is an implementation + * detail that allows leaving a nul character behind the valid data. + * + * In case of error, the msgb remains unchanged, though data may have been + * written to the (unused) memory after the tail pointer. + */ +int msgb_printf(struct msgb *msgb, const char *format, ...) +{ + va_list args; + int str_len; + int rc = 0; + + OSMO_ASSERT(msgb); + OSMO_ASSERT(format); + + /* Regardless of what we plan to add to the buffer, we must at least + * be able to store a string terminator (nullstring) */ + if (msgb_tailroom(msgb) < 1) + return -EINVAL; + + va_start(args, format); + + str_len = + vsnprintf((char *)msgb->tail, msgb_tailroom(msgb), format, args); + + if (str_len >= msgb_tailroom(msgb) || str_len < 0) { + rc = -EINVAL; + } else + msgb_put(msgb, str_len); + + va_end(args); + return rc; +} + +/*! @} */ diff --git a/src/core/msgfile.c b/src/core/msgfile.c new file mode 100644 index 00000000..abb4e7cf --- /dev/null +++ b/src/core/msgfile.c @@ -0,0 +1,125 @@ +/*! \file msgfile.c + * Parse a simple file with messages, e.g used for USSD messages. */ +/* + * (C) 2010 by Holger Hans Peter Freyther + * (C) 2010 by On-Waves + * 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. + * + */ + +#define _WITH_GETLINE + +#include <osmocom/core/msgfile.h> +#include <osmocom/core/talloc.h> + +#include <sys/stat.h> +#include <fcntl.h> +#include <unistd.h> +#include <string.h> +#include <stdio.h> + +static struct osmo_config_entry * +alloc_entry(struct osmo_config_list *entries, + const char *mcc, const char *mnc, + const char *option, const char *text) +{ + struct osmo_config_entry *entry = + talloc_zero(entries, struct osmo_config_entry); + if (!entry) + return NULL; + + entry->mcc = talloc_strdup(entry, mcc); + entry->mnc = talloc_strdup(entry, mnc); + entry->option = talloc_strdup(entry, option); + entry->text = talloc_strdup(entry, text); + + llist_add_tail(&entry->list, &entries->entry); + return entry; +} + +static struct osmo_config_list *alloc_entries(void *ctx) +{ + struct osmo_config_list *entries; + + entries = talloc_zero(ctx, struct osmo_config_list); + if (!entries) + return NULL; + + INIT_LLIST_HEAD(&entries->entry); + return entries; +} + +/* + * split a line like 'foo:Text'. + */ +static void handle_line(struct osmo_config_list *entries, char *line) +{ + int i; + const int len = strlen(line); + + char *items[3]; + int last_item = 0; + + /* Skip comments from the file */ + if (line[0] == '#') + return; + + for (i = 0; i < len; ++i) { + if (line[i] == '\n' || line[i] == '\r') + line[i] = '\0'; + else if (line[i] == ':' && last_item < 3) { + line[i] = '\0'; + + items[last_item++] = &line[i + 1]; + } + } + + if (last_item == 3) { + alloc_entry(entries, &line[0] , items[0], items[1], items[2]); + return; + } + + /* nothing found */ +} + +struct osmo_config_list *osmo_config_list_parse(void *ctx, const char *filename) +{ + struct osmo_config_list *entries; + size_t n; + char *line; + FILE *file; + + file = fopen(filename, "r"); + if (!file) + return NULL; + + entries = alloc_entries(ctx); + if (!entries) { + fclose(file); + return NULL; + } + + n = 2342; + line = NULL; + while (getline(&line, &n, file) != -1) { + handle_line(entries, line); + } + /* The returned getline() buffer needs to be freed even if it failed. It can simply re-use the + * buffer that was allocated on the first call. */ + free(line); + + fclose(file); + return entries; +} 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..2b2b7ddb --- /dev/null +++ b/src/core/osmo_io.c @@ -0,0 +1,702 @@ +/*! \file osmo_io.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" +#if defined(__linux__) + +#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" + +/*! 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 } +}; + +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_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 + * \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) +{ + 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(iofd, struct iofd_msghdr); + 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, + iofd->name ? : "iofd_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; + 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_cb) { + *pending_out = NULL; + return IOFD_SEG_ACT_HANDLE_ONE; + } + + int expected_len = iofd->io_ops.segmentation_cb(msg); + 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; + + 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; +} + +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_SCTP_RECVMSG_SENDMSG: + /* TODO Implement */ + OSMO_ASSERT(false); + break; + } +} + +/* Public functions */ + +/*! Send a message through a connected socket. + * + * Appends the message to the internal transmit queue. + * 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 file descriptor to write to + * \param[in] msg message buffer 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(!iofd->io_ops.write_cb)) { + LOGPIO(iofd, LOGL_ERROR, "write_cb not set, Rejecting msgb\n"); + return -EINVAL; + } + + struct iofd_msghdr *msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_WRITE, msg); + 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. + * + * Appends the message to the internal transmit queue. + * 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 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; + + OSMO_ASSERT(iofd->mode == OSMO_IO_FD_MODE_RECVFROM_SENDTO); + if (OSMO_UNLIKELY(!iofd->io_ops.sendto_cb)) { + LOGPIO(iofd, LOGL_ERROR, "sendto_cb not set, Rejecting msgb\n"); + return -EINVAL; + } + + struct iofd_msghdr *msghdr = iofd_msghdr_alloc(iofd, IOFD_ACT_SENDTO, msg); + 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; +} + +/*! Allocate and setup a new iofd. + * \param[in] ctx the parent context from which to allocate + * \param[in] fd the underlying system file descriptor + * \param[in] name the name of the iofd + * \param[in] mode the mode of the iofd, whether it should use read()/write(), sendto()/recvfrom() + * \param[in] ioops structure with read/write/send/recv callbacks + * \param[in] data 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 = 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; +} + +/*! Register the fd with the underlying backend. + * + * \param[in] iofd the iofd file descriptor + * \param[in] fd the system fd number that will be registeres. If negative will use the one already set. + * \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; + + if (osmo_iofd_ops.register_fd) + rc = osmo_iofd_ops.register_fd(iofd); + if (rc) + return rc; + + IOFD_FLAG_UNSET(iofd, IOFD_FLAG_CLOSED); + if (iofd->io_ops.read_cb) + osmo_iofd_ops.read_enable(iofd); + + if (iofd->tx_queue.current_length > 0) + osmo_iofd_ops.write_enable(iofd); + + return rc; +} + +/*! Unregister the fd from the underlying backend. + * + * \param[in] iofd the file descriptor + * \returns zero on success, a negative value on error + */ +int osmo_iofd_unregister(struct osmo_io_fd *iofd) +{ + if (osmo_iofd_ops.unregister_fd) + return osmo_iofd_ops.unregister_fd(iofd); + IOFD_FLAG_SET(iofd, IOFD_FLAG_CLOSED); + + return 0; +} + +/*! Get the number of messages in the tx 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 the iofd. + * + * 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 iofd. + * + * This function is safe to use in the read/write callbacks and will defer freeing it until safe to do so. + * The iofd will be closed before. + * \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 iofd. + * + * This function closes the underlying fd and clears any messages in the tx queue + * The iofd 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; + + if (osmo_iofd_ops.close) + 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; +} + +/*! Get the associated user-data from an iofd. + * \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 iofd. + * \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; +} + +/*! Get the private number from an iofd. + * \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 from an iofd. + * \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; +} + +/*! Get the underlying file descriptor from an iofd. + * \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; +} + +/*! Get the name of the file descriptor. + * \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 name of the file descriptor. + * \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 for an iofd. + * \param[in] iofd Target iofd file descriptor + * \param[in] ioops osmo_io_ops structure to be set */ +void osmo_iofd_set_ioops(struct osmo_io_fd *iofd, const struct osmo_io_ops *ioops) +{ + 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_SCTP_RECVMSG_SENDMSG: + default: + OSMO_ASSERT(0); + } +} + +/*! Notify the user if/when the socket is connected. + * When the socket is connected the write_cb will be called. + * \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); + osmo_iofd_ops.write_enable(iofd); +} + + +#endif /* defined(__linux__) */ diff --git a/src/core/osmo_io_internal.h b/src/core/osmo_io_internal.h new file mode 100644 index 00000000..5b7ab908 --- /dev/null +++ b/src/core/osmo_io_internal.h @@ -0,0 +1,145 @@ +/*! \file osmo_io_internal.h */ + +#pragma once + +#include <unistd.h> +#include <stdbool.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); +}; + +#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_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; + + 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? */ + } uring; + } u; +}; + +enum iofd_msg_action { + IOFD_ACT_READ, + IOFD_ACT_WRITE, + IOFD_ACT_RECVFROM, + IOFD_ACT_SENDTO, + // TODO: SCTP_* +}; + + +/* serialized version of 'struct msghdr' employed by sendmsg/recvmsg */ +struct iofd_msghdr { + struct llist_head list; + enum iofd_msg_action action; + struct msghdr hdr; + struct osmo_sockaddr osa; + struct iovec iov[1]; + int flags; + + struct msgb *msg; + struct osmo_io_fd *iofd; +}; + +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); +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_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..a9aaea4e --- /dev/null +++ b/src/core/osmo_io_poll.c @@ -0,0 +1,188 @@ +/*! \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" +#if defined(__linux__) + +#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), + }; + + rc = recvmsg(ofd->fd, &hdr.hdr, flags); + if (rc > 0) + msgb_put(msg, rc); + + iofd_handle_recv(iofd, msg, 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) { + msg = msghdr->msg; + + rc = sendmsg(ofd->fd, &msghdr->hdr, msghdr->flags); + if (rc > 0 && rc < msgb_length(msg)) { + msgb_pull(msg, rc); + iofd_txqueue_enqueue_front(iofd, msghdr); + return; + } + if (rc == -EAGAIN) { + iofd_txqueue_enqueue_front(iofd, msghdr); + return; + } + + switch (iofd->mode) { + case OSMO_IO_FD_MODE_READ_WRITE: + iofd->io_ops.write_cb(iofd, rc, msg); + break; + case OSMO_IO_FD_MODE_RECVFROM_SENDTO: + iofd->io_ops.sendto_cb(iofd, rc, msg, &msghdr->osa); + break; + case OSMO_IO_FD_MODE_SCTP_RECVMSG_SENDMSG: + OSMO_ASSERT(false); + break; + } + + talloc_free(msghdr); + msgb_free(msg); + } else { + if (iofd->mode == OSMO_IO_FD_MODE_READ_WRITE) + /* Socket is writable, but we have no data to send. A non-blocking/async + connect() is signalled this way. */ + iofd->io_ops.write_cb(iofd, 0, NULL); + 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; +} + +int iofd_poll_register(struct osmo_io_fd *iofd) +{ + struct osmo_fd *ofd = &iofd->u.poll.ofd; + osmo_fd_setup(ofd, iofd->fd, 0, &iofd_poll_ofd_cb_dispatch, iofd, 0); + return osmo_fd_register(ofd); +} + +int iofd_poll_unregister(struct osmo_io_fd *iofd) +{ + struct osmo_fd *ofd = &iofd->u.poll.ofd; + osmo_fd_unregister(ofd); + + return 0; +} + +int iofd_poll_close(struct osmo_io_fd *iofd) +{ + osmo_fd_close(&iofd->u.poll.ofd); + + return 0; +} + +void iofd_poll_read_enable(struct osmo_io_fd *iofd) +{ + osmo_fd_read_enable(&iofd->u.poll.ofd); +} + +void iofd_poll_read_disable(struct osmo_io_fd *iofd) +{ + osmo_fd_read_disable(&iofd->u.poll.ofd); +} + +void iofd_poll_write_enable(struct osmo_io_fd *iofd) +{ + osmo_fd_write_enable(&iofd->u.poll.ofd); +} + +void iofd_poll_write_disable(struct osmo_io_fd *iofd) +{ + osmo_fd_write_disable(&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, +}; + +#endif /* defined(__linux__) */ diff --git a/src/core/osmo_io_uring.c b/src/core/osmo_io_uring.c new file mode 100644 index 00000000..a6395fea --- /dev/null +++ b/src/core/osmo_io_uring.c @@ -0,0 +1,428 @@ +/*! \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> + * + * 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 <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); +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; + rc = io_uring_queue_init(IOFD_URING_ENTRIES, &g_ring.ring, 0); + if (rc < 0) + OSMO_ASSERT(0); + + rc = eventfd(0, 0); + if (rc < 0) { + io_uring_queue_exit(&g_ring.ring); + OSMO_ASSERT(0); + } + + osmo_fd_setup(&g_ring.event_ofd, rc, OSMO_FD_READ, iofd_uring_poll_cb, &g_ring.ring, 0); + osmo_fd_register(&g_ring.event_ofd); + io_uring_register_eventfd(&g_ring.ring, 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); + 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_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_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; +} + +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); + +static void iofd_uring_handle_tx(struct iofd_msghdr *msghdr, int rc) +{ + struct osmo_io_fd *iofd = msghdr->iofd; + struct msgb *msg = msghdr->msg; + + if (IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + goto out_free; + + /* Error during write */ + if (rc < 0) { + if (msghdr->action == IOFD_ACT_WRITE) + iofd->io_ops.write_cb(iofd, rc, msg); + else if (msghdr->action == IOFD_ACT_SENDTO) + iofd->io_ops.sendto_cb(iofd, rc, msg, &msghdr->osa); + else + OSMO_ASSERT(0); + goto out_free; + } + + /* Incomplete write */ + if (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); + goto out; + } + + if (msghdr->action == IOFD_ACT_WRITE) + iofd->io_ops.write_cb(iofd, rc, msg); + else if (msghdr->action == IOFD_ACT_SENDTO) + iofd->io_ops.sendto_cb(iofd, rc, msg, &msghdr->osa); + else + OSMO_ASSERT(0); + +out_free: + msgb_free(msghdr->msg); + iofd_msghdr_free(msghdr); + +out: + iofd->u.uring.write_msghdr = NULL; + if (iofd->u.uring.write_enabled && !IOFD_FLAG_ISSET(iofd, IOFD_FLAG_CLOSED)) + iofd_uring_submit_tx(iofd); +} + +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: + iofd_uring_handle_recv(msghdr, res); + break; + case IOFD_ACT_WRITE: + case IOFD_ACT_SENDTO: + iofd_uring_handle_tx(msghdr, res); + break; + default: + OSMO_ASSERT(0) + } + + if (!iofd->u.uring.read_msghdr && !iofd->u.uring.write_msghdr) + 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); +} + +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; + } + + rc = cqe->res; + /* Hand the entry back to the kernel before */ + io_uring_cqe_seen(ring, cqe); + + iofd_uring_handle_completion(msghdr, rc); + + } +} + +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: + 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; + if (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"); + io_uring_prep_cancel(sqe, iofd->u.uring.read_msghdr, 0); + } + + if (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"); + io_uring_prep_cancel(sqe, iofd->u.uring.write_msghdr, 0); + } + io_uring_submit(&g_ring.ring); + + 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; + + 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); + 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_tailroom(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); + } + // Prep msgb/iov + 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; + + 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; + 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); +} + +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, +}; + +#endif /* defined(__linux__) */ diff --git a/src/core/panic.c b/src/core/panic.c new file mode 100644 index 00000000..bbf6d081 --- /dev/null +++ b/src/core/panic.c @@ -0,0 +1,103 @@ +/*! \file panic.c + * Routines for panic handling. */ +/* + * (C) 2010 by Sylvain Munaut <tnt@246tNt.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 utils + * @{ + * \file panic.c */ + +#include <unistd.h> +#include <osmocom/core/panic.h> +#include <osmocom/core/backtrace.h> + +#include "config.h" + + +static osmo_panic_handler_t osmo_panic_handler = (void*)0; + + +#ifndef PANIC_INFLOOP + +#include <stdio.h> +#include <stdlib.h> + +static void osmo_panic_default(const char *fmt, va_list args) +{ + vfprintf(stderr, fmt, args); + osmo_generate_backtrace(); + abort(); +} + +#else + +static void osmo_panic_default(const char *fmt, va_list args) +{ + while (1); +} + +#endif + + +/*! Terminate the current program with a panic + * + * You can call this function in case some severely unexpected situation + * is detected and the program is supposed to terminate in a way that + * reports the fact that it terminates. + * + * The application can register a panic handler function using \ref + * osmo_set_panic_handler. If it doesn't, a default panic handler + * function is called automatically. + * + * The default function on most systems will generate a backtrace and + * then abort() the process. + */ +void osmo_panic(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + + if (osmo_panic_handler) + osmo_panic_handler(fmt, args); + else + osmo_panic_default(fmt, args); + + va_end(args); + + /* not reached, but make compiler believe we really never return */ +#ifndef PANIC_INFLOOP + exit(2342); +#else + while (1) ; +#endif +} + +/*! Set the panic handler + * \param[in] h New panic handler function + * + * This changes the panic handling function from the currently active + * function to a new call-back function supplied by the caller. + */ +void osmo_set_panic_handler(osmo_panic_handler_t h) +{ + osmo_panic_handler = h; +} + +/*! @} */ diff --git a/src/core/plugin.c b/src/core/plugin.c new file mode 100644 index 00000000..687ad406 --- /dev/null +++ b/src/core/plugin.c @@ -0,0 +1,71 @@ +/*! \file plugin.c + * Routines for loading and managing shared library plug-ins. */ +/* + * (C) 2010 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 utils + * @{ + * \file plugin.c */ + +#include "config.h" + +#if HAVE_DLFCN_H + +#include <dirent.h> +#include <dlfcn.h> +#include <stdio.h> +#include <errno.h> +#include <limits.h> + +#include <osmocom/core/plugin.h> + +/*! Load all plugins available in given directory + * \param[in] directory full path name of directory containing plug-ins + * \returns number of plugins loaded in case of success, negative in case of error + */ +int osmo_plugin_load_all(const char *directory) +{ + unsigned int num = 0; + char fname[PATH_MAX]; + DIR *dir; + struct dirent *entry; + + dir = opendir(directory); + if (!dir) + return -errno; + + while ((entry = readdir(dir))) { + snprintf(fname, sizeof(fname), "%s/%s", directory, + entry->d_name); + if (dlopen(fname, RTLD_NOW)) + num++; + } + + closedir(dir); + + return num; +} +#else +int osmo_plugin_load_all(const char *directory) +{ + return 0; +} +#endif /* HAVE_DLFCN_H */ + +/*! @} */ diff --git a/src/core/prbs.c b/src/core/prbs.c new file mode 100644 index 00000000..8fa04bb7 --- /dev/null +++ b/src/core/prbs.c @@ -0,0 +1,78 @@ +/* Osmocom implementation of pseudo-random bit sequence generation */ +/* (C) 2017 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * */ + +#include <stdint.h> +#include <string.h> +#include <osmocom/core/bits.h> +#include <osmocom/core/prbs.h> + +/*! \brief PRBS-7 according ITU-T O.150 */ +const struct osmo_prbs osmo_prbs7 = { + /* x^7 + x^6 + 1 */ + .name = "PRBS-7", + .len = 7, + .coeff = (1<<6) | (1<<5), +}; + +/*! \brief PRBS-9 according ITU-T O.150 */ +const struct osmo_prbs osmo_prbs9 = { + /* x^9 + x^5 + 1 */ + .name = "PRBS-9", + .len = 9, + .coeff = (1<<8) | (1<<4), +}; + +/*! \brief PRBS-11 according ITU-T O.150 */ +const struct osmo_prbs osmo_prbs11 = { + /* x^11 + x^9 + 1 */ + .name = "PRBS-11", + .len = 11, + .coeff = (1<<10) | (1<<8), +}; + +/*! \brief PRBS-15 according ITU-T O.150 */ +const struct osmo_prbs osmo_prbs15 = { + /* x^15 + x^14+ 1 */ + .name = "PRBS-15", + .len = 15, + .coeff = (1<<14) | (1<<13), +}; + +/*! \brief Initialize the given caller-allocated PRBS state */ +void osmo_prbs_state_init(struct osmo_prbs_state *st, const struct osmo_prbs *prbs) +{ + memset(st, 0, sizeof(*st)); + st->prbs = prbs; + st->state = 1; +} + +static void osmo_prbs_process_bit(struct osmo_prbs_state *state, ubit_t bit) +{ + state->state >>= 1; + if (bit) + state->state ^= state->prbs->coeff; +} + +/*! \brief Get the next bit out of given PRBS instance */ +ubit_t osmo_prbs_get_ubit(struct osmo_prbs_state *state) +{ + ubit_t result = state->state & 0x1; + osmo_prbs_process_bit(state, result); + + return result; +} + +/*! \brief Fill buffer of unpacked bits with next bits out of given PRBS instance */ +int osmo_prbs_get_ubits(ubit_t *out, unsigned int out_len, struct osmo_prbs_state *state) +{ + unsigned int i; + + for (i = 0; i < out_len; i++) + out[i] = osmo_prbs_get_ubit(state); + + return i; +} diff --git a/src/core/prim.c b/src/core/prim.c new file mode 100644 index 00000000..3c8a7f12 --- /dev/null +++ b/src/core/prim.c @@ -0,0 +1,42 @@ +/*! + * (C) 2015-2017 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * \addtogroup prim + * @{ + * \file prim.c */ + +#include <osmocom/core/utils.h> +#include <osmocom/core/prim.h> + +/*! human-readable string mapping for + * \ref osmo_prim_operation */ +const struct value_string osmo_prim_op_names[5] = { + { PRIM_OP_REQUEST, "request" }, + { PRIM_OP_RESPONSE, "response" }, + { PRIM_OP_INDICATION, "indication" }, + { PRIM_OP_CONFIRM, "confirm" }, + { 0, NULL } +}; + +/*! resolve the (fsm) event for a given primitive using a map + * \param[in] oph primitive header used as key for match + * \param[in] maps list of mappings from primitive to event + * \returns event determined by map; \ref OSMO_NO_EVENT if no match */ +uint32_t osmo_event_for_prim(const struct osmo_prim_hdr *oph, + const struct osmo_prim_event_map *maps) +{ + const struct osmo_prim_event_map *map; + + for (map = maps; map->event != OSMO_NO_EVENT; map++) { + if (map->sap == oph->sap && + map->primitive == oph->primitive && + map->operation == oph->operation) + return map->event; + } + return OSMO_NO_EVENT; +} + +/*! @} */ 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/core/rate_ctr.c b/src/core/rate_ctr.c new file mode 100644 index 00000000..44e26585 --- /dev/null +++ b/src/core/rate_ctr.c @@ -0,0 +1,499 @@ +/* (C) 2009-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. + * + */ + +/*! \addtogroup rate_ctr + * @{ + * Counters about events and their event rates. + * + * As \ref osmo_counter and \ref osmo_stat_item are concerned only with + * a single given value that may be increased/decreased, or the difference + * to one given previous value, this module adds some support for keeping + * long term information about a given event rate. + * + * A \ref rate_ctr keeps information on the amount of events per second, + * per minute, per hour and per day. + * + * \ref rate_ctr come in groups: An application describes a group of counters + * with their names and identities once in a (typically const) \ref + * rate_ctr_group_desc. + * + * As objects (such as e.g. a subscriber or a PDP context) are + * allocated dynamically at runtime, the application calls \ref + * rate_ctr_group_alloc with a refernce to the \ref + * rate_ctr_group_desc, which causes the library to allocate one set of + * \ref rate_ctr: One for each in the group. + * + * The application then uses functions like \ref rate_ctr_add or \ref + * rate_ctr_inc to increment the value as certain events (e.g. location + * update) happens. + * + * The library internally keeps a timer once per second which iterates + * over all registered counters and which updates the per-second, + * per-minute, per-hour and per-day averages based on the current + * value. + * + * The counters can be reported using \ref stats or by VTY + * introspection, as well as by any application-specific code accessing + * the \ref rate_ctr.intv array directly. + * + * \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/select.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/logging.h> + +static LLIST_HEAD(rate_ctr_groups); + +static void *tall_rate_ctr_ctx; + + +static bool rate_ctrl_group_desc_validate(const struct rate_ctr_group_desc *desc) +{ + unsigned int i; + const struct rate_ctr_desc *ctr_desc; + + if (!desc) { + LOGP(DLGLOBAL, LOGL_ERROR, "NULL is not a valid counter group descriptor\n"); + return false; + } + ctr_desc = desc->ctr_desc; + + DEBUGP(DLGLOBAL, "validating counter group %p(%s) with %u counters\n", desc, + desc->group_name_prefix, desc->num_ctr); + + if (!osmo_identifier_valid(desc->group_name_prefix)) { + LOGP(DLGLOBAL, LOGL_ERROR, "'%s' is not a valid counter group identifier\n", + desc->group_name_prefix); + return false; + } + + for (i = 0; i < desc->num_ctr; i++) { + if (!osmo_identifier_valid(ctr_desc[i].name)) { + LOGP(DLGLOBAL, LOGL_ERROR, "'%s' is not a valid counter identifier\n", + ctr_desc[i].name); + return false; + } + } + + return true; +} + +/* return 'in' if it doesn't contain any '.'; otherwise allocate a copy and + * replace all '.' with ':' */ +static char *mangle_identifier_ifneeded(const void *ctx, const char *in) +{ + char *out; + unsigned int i; + bool modified = false; + + if (!in) + return NULL; + + if (!strchr(in, '.')) + return (char *)in; + + out = talloc_strdup(ctx, in); + OSMO_ASSERT(out); + + for (i = 0; i < strlen(out); i++) { + if (out[i] == '.') { + out[i] = ':'; + modified = true; + } + } + + if (modified) + LOGP(DLGLOBAL, LOGL_NOTICE, "counter group name mangled: '%s' -> '%s'\n", + in, out); + + return out; +} + +/* "mangle" a rate counter group descriptor, i.e. replace any '.' with ':' */ +static struct rate_ctr_group_desc * +rate_ctr_group_desc_mangle(void *ctx, const struct rate_ctr_group_desc *desc) +{ + struct rate_ctr_group_desc *desc_new = talloc_zero(ctx, struct rate_ctr_group_desc); + int i; + + OSMO_ASSERT(desc_new); + + LOGP(DLGLOBAL, LOGL_INFO, "Needed to mangle counter group '%s' names: it is still using '.' as " + "separator, which is not allowed. please consider updating the application\n", + desc->group_name_prefix); + + /* mangle the name_prefix but copy/keep the rest */ + desc_new->group_name_prefix = mangle_identifier_ifneeded(desc_new, desc->group_name_prefix); + desc_new->group_description = desc->group_description; + desc_new->class_id = desc->class_id; + desc_new->num_ctr = desc->num_ctr; + desc_new->ctr_desc = talloc_array(desc_new, struct rate_ctr_desc, desc_new->num_ctr); + OSMO_ASSERT(desc_new->ctr_desc); + + for (i = 0; i < desc->num_ctr; i++) { + struct rate_ctr_desc *ctrd_new = (struct rate_ctr_desc *) desc_new->ctr_desc; + const struct rate_ctr_desc *ctrd = desc->ctr_desc; + + if (!ctrd[i].name) { + LOGP(DLGLOBAL, LOGL_ERROR, "counter group '%s'[%d] == NULL, aborting\n", + desc->group_name_prefix, i); + goto err_free; + } + + ctrd_new[i].name = mangle_identifier_ifneeded(desc_new->ctr_desc, ctrd[i].name); + ctrd_new[i].description = ctrd[i].description; + } + + if (!rate_ctrl_group_desc_validate(desc_new)) { + /* simple mangling of identifiers ('.' -> ':') was not sufficient to render a valid + * descriptor, we have to bail out */ + LOGP(DLGLOBAL, LOGL_ERROR, "counter group '%s' still invalid after mangling\n", + desc->group_name_prefix); + goto err_free; + } + + return desc_new; +err_free: + talloc_free(desc_new); + return NULL; +} + +/*! Find an unused index for this rate counter group. + * \param[in] name Name of the counter group + * \returns the largest used index number + 1, or 0 if none exist yet. */ +static unsigned int rate_ctr_get_unused_name_idx(const char *name) +{ + unsigned int idx = 0; + struct rate_ctr_group *ctrg; + + llist_for_each_entry(ctrg, &rate_ctr_groups, list) { + if (!ctrg->desc) + continue; + + if (strcmp(ctrg->desc->group_name_prefix, name)) + continue; + + if (idx <= ctrg->idx) + idx = ctrg->idx + 1; + } + return idx; +} + +/*! Allocate a new group of counters according to description + * \param[in] ctx parent talloc context + * \param[in] desc Rate counter group description + * \param[in] idx Index of new counter group + */ +struct rate_ctr_group *rate_ctr_group_alloc(void *ctx, + const struct rate_ctr_group_desc *desc, + unsigned int idx) +{ + unsigned int size; + struct rate_ctr_group *group; + + if (rate_ctr_get_group_by_name_idx(desc->group_name_prefix, idx)) { + unsigned int new_idx = rate_ctr_get_unused_name_idx(desc->group_name_prefix); + LOGP(DLGLOBAL, LOGL_ERROR, "counter group '%s' already exists for index %u," + " instead using index %u. This is a software bug that needs fixing.\n", + desc->group_name_prefix, idx, new_idx); + idx = new_idx; + } + + size = sizeof(struct rate_ctr_group) + + desc->num_ctr * sizeof(struct rate_ctr); + + if (!ctx) + ctx = tall_rate_ctr_ctx; + + group = talloc_zero_size(ctx, size); + if (!group) + return NULL; + + /* attempt to mangle all '.' in identifiers to ':' for backwards compat */ + if (!rate_ctrl_group_desc_validate(desc)) { + desc = rate_ctr_group_desc_mangle(group, desc); + if (!desc) { + talloc_free(group); + return NULL; + } + } + + group->desc = desc; + group->idx = idx; + + llist_add(&group->list, &rate_ctr_groups); + + return group; +} + +/*! Free the memory for the specified group of counters */ +void rate_ctr_group_free(struct rate_ctr_group *grp) +{ + if (!grp) + return; + + if (!llist_empty(&grp->list)) + llist_del(&grp->list); + 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) +{ + ctr->current += inc; +} + +/*! Return the counter difference since the last call to this function */ +int64_t rate_ctr_difference(struct rate_ctr *ctr) +{ + int64_t result = ctr->current - ctr->previous; + ctr->previous = ctr->current; + + return result; +} + +/* TODO: support update intervals > 1s */ +/* TODO: implement this as a special stats reporter */ + +static void interval_expired(struct rate_ctr *ctr, enum rate_ctr_intv intv) +{ + /* calculate rate over last interval */ + ctr->intv[intv].rate = ctr->current - ctr->intv[intv].last; + /* save current counter for next interval */ + ctr->intv[intv].last = ctr->current; +} + +static struct osmo_fd rate_ctr_timer = { .fd = -1 }; +static uint64_t timer_ticks; + +/* The one-second interval has expired */ +static void rate_ctr_group_intv(struct rate_ctr_group *grp) +{ + unsigned int i; + + for (i = 0; i < grp->desc->num_ctr; i++) { + struct rate_ctr *ctr = &grp->ctr[i]; + + interval_expired(ctr, RATE_CTR_INTV_SEC); + if ((timer_ticks % 60) == 0) + interval_expired(ctr, RATE_CTR_INTV_MIN); + if ((timer_ticks % (60*60)) == 0) + interval_expired(ctr, RATE_CTR_INTV_HOUR); + if ((timer_ticks % (24*60*60)) == 0) + interval_expired(ctr, RATE_CTR_INTV_DAY); + } +} + +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)); + + if (expire_count > 1) + LOGP(DLGLOBAL, LOGL_NOTICE, "Stats timer expire_count=%" PRIu64 ": We missed %" PRIu64 " timers\n", + expire_count, expire_count - 1); + + 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); + + return 0; +} + +/*! Initialize the counter module. Call this once from your application. + * \param[in] tall_ctx Talloc context from which rate_ctr_group will be allocated + * \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; + + 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; +} + +/*! Search for counter group based on group name and index + * \param[in] name Name of the counter group you're looking for + * \param[in] idx Index inside the counter group + * \returns \ref rate_ctr_group or NULL in case of error */ +struct rate_ctr_group *rate_ctr_get_group_by_name_idx(const char *name, const unsigned int idx) +{ + struct rate_ctr_group *ctrg; + + llist_for_each_entry(ctrg, &rate_ctr_groups, list) { + if (!ctrg->desc) + continue; + + if (!strcmp(ctrg->desc->group_name_prefix, name) && + ctrg->idx == idx) { + return ctrg; + } + } + return NULL; +} + +/*! Search for counter based on group + name + * \param[in] ctrg pointer to \ref rate_ctr_group + * \param[in] name name of counter inside group + * \returns \ref rate_ctr or NULL in case of error + */ +const struct rate_ctr *rate_ctr_get_by_name(const struct rate_ctr_group *ctrg, const char *name) +{ + int i; + const struct rate_ctr_desc *ctr_desc; + + if (!ctrg->desc) + return NULL; + + for (i = 0; i < ctrg->desc->num_ctr; i++) { + ctr_desc = &ctrg->desc->ctr_desc[i]; + + if (!strcmp(ctr_desc->name, name)) { + return &ctrg->ctr[i]; + } + } + return NULL; +} + +/*! Iterate over each counter in group and call function + * \param[in] ctrg counter group over which to iterate + * \param[in] handle_counter function pointer + * \param[in] data Data to hand transparently to handle_counter() + * \returns 0 on success; negative otherwise + */ +int rate_ctr_for_each_counter(struct rate_ctr_group *ctrg, + rate_ctr_handler_t handle_counter, void *data) +{ + int rc = 0; + int i; + + for (i = 0; i < ctrg->desc->num_ctr; i++) { + struct rate_ctr *ctr = &ctrg->ctr[i]; + rc = handle_counter(ctrg, + ctr, &ctrg->desc->ctr_desc[i], data); + if (rc < 0) + return rc; + } + + return rc; +} + +/*! Iterate over all counter groups + * \param[in] handle_group function pointer of callback function + * \param[in] data Data to hand transparently to handle_group() + * \returns 0 on success; negative otherwise + */ +int rate_ctr_for_each_group(rate_ctr_group_handler_t handle_group, void *data) +{ + struct rate_ctr_group *statg; + int rc = 0; + + llist_for_each_entry(statg, &rate_ctr_groups, list) { + rc = handle_group(statg, data); + if (rc < 0) + return rc; + } + + 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/core/rbtree.c b/src/core/rbtree.c new file mode 100644 index 00000000..f4dc219b --- /dev/null +++ b/src/core/rbtree.c @@ -0,0 +1,381 @@ +/* + Red Black Trees + (C) 1999 Andrea Arcangeli <andrea@suse.de> + (C) 2002 David Woodhouse <dwmw2@infradead.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. + + linux/lib/rbtree.c +*/ + +#include <osmocom/core/linuxrbtree.h> + +static void __rb_rotate_left(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *right = node->rb_right; + struct rb_node *parent = rb_parent(node); + + if ((node->rb_right = right->rb_left)) + rb_set_parent(right->rb_left, node); + right->rb_left = node; + + rb_set_parent(right, parent); + + if (parent) + { + if (node == parent->rb_left) + parent->rb_left = right; + else + parent->rb_right = right; + } + else + root->rb_node = right; + rb_set_parent(node, right); +} + +static void __rb_rotate_right(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *left = node->rb_left; + struct rb_node *parent = rb_parent(node); + + if ((node->rb_left = left->rb_right)) + rb_set_parent(left->rb_right, node); + left->rb_right = node; + + rb_set_parent(left, parent); + + if (parent) + { + if (node == parent->rb_right) + parent->rb_right = left; + else + parent->rb_left = left; + } + else + root->rb_node = left; + rb_set_parent(node, left); +} + +void rb_insert_color(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *parent, *gparent; + + while ((parent = rb_parent(node)) && rb_is_red(parent)) + { + gparent = rb_parent(parent); + + if (parent == gparent->rb_left) + { + { + register struct rb_node *uncle = gparent->rb_right; + if (uncle && rb_is_red(uncle)) + { + rb_set_black(uncle); + rb_set_black(parent); + rb_set_red(gparent); + node = gparent; + continue; + } + } + + if (parent->rb_right == node) + { + register struct rb_node *tmp; + __rb_rotate_left(parent, root); + tmp = parent; + parent = node; + node = tmp; + } + + rb_set_black(parent); + rb_set_red(gparent); + __rb_rotate_right(gparent, root); + } else { + { + register struct rb_node *uncle = gparent->rb_left; + if (uncle && rb_is_red(uncle)) + { + rb_set_black(uncle); + rb_set_black(parent); + rb_set_red(gparent); + node = gparent; + continue; + } + } + + if (parent->rb_left == node) + { + register struct rb_node *tmp; + __rb_rotate_right(parent, root); + tmp = parent; + parent = node; + node = tmp; + } + + rb_set_black(parent); + rb_set_red(gparent); + __rb_rotate_left(gparent, root); + } + } + + rb_set_black(root->rb_node); +} + +static void __rb_erase_color(struct rb_node *node, struct rb_node *parent, + struct rb_root *root) +{ + struct rb_node *other; + + while ((!node || rb_is_black(node)) && node != root->rb_node) + { + if (parent->rb_left == node) + { + other = parent->rb_right; + if (rb_is_red(other)) + { + rb_set_black(other); + rb_set_red(parent); + __rb_rotate_left(parent, root); + other = parent->rb_right; + } + if ((!other->rb_left || rb_is_black(other->rb_left)) && + (!other->rb_right || rb_is_black(other->rb_right))) + { + rb_set_red(other); + node = parent; + parent = rb_parent(node); + } + else + { + if (!other->rb_right || rb_is_black(other->rb_right)) + { + rb_set_black(other->rb_left); + rb_set_red(other); + __rb_rotate_right(other, root); + other = parent->rb_right; + } + rb_set_color(other, rb_color(parent)); + rb_set_black(parent); + rb_set_black(other->rb_right); + __rb_rotate_left(parent, root); + node = root->rb_node; + break; + } + } + else + { + other = parent->rb_left; + if (rb_is_red(other)) + { + rb_set_black(other); + rb_set_red(parent); + __rb_rotate_right(parent, root); + other = parent->rb_left; + } + if ((!other->rb_left || rb_is_black(other->rb_left)) && + (!other->rb_right || rb_is_black(other->rb_right))) + { + rb_set_red(other); + node = parent; + parent = rb_parent(node); + } + else + { + if (!other->rb_left || rb_is_black(other->rb_left)) + { + rb_set_black(other->rb_right); + rb_set_red(other); + __rb_rotate_left(other, root); + other = parent->rb_left; + } + rb_set_color(other, rb_color(parent)); + rb_set_black(parent); + rb_set_black(other->rb_left); + __rb_rotate_right(parent, root); + node = root->rb_node; + break; + } + } + } + if (node) + rb_set_black(node); +} + +void rb_erase(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *child, *parent; + int color; + + if (!node->rb_left) + child = node->rb_right; + else if (!node->rb_right) + child = node->rb_left; + else + { + struct rb_node *old = node, *left; + + node = node->rb_right; + while ((left = node->rb_left) != NULL) + node = left; + + if (rb_parent(old)) { + if (rb_parent(old)->rb_left == old) + rb_parent(old)->rb_left = node; + else + rb_parent(old)->rb_right = node; + } else + root->rb_node = node; + + child = node->rb_right; + parent = rb_parent(node); + color = rb_color(node); + + if (parent == old) { + parent = node; + } else { + if (child) + rb_set_parent(child, parent); + parent->rb_left = child; + + node->rb_right = old->rb_right; + rb_set_parent(old->rb_right, node); + } + + node->rb_parent_color = old->rb_parent_color; + node->rb_left = old->rb_left; + rb_set_parent(old->rb_left, node); + + goto color; + } + + parent = rb_parent(node); + color = rb_color(node); + + if (child) + rb_set_parent(child, parent); + if (parent) + { + if (parent->rb_left == node) + parent->rb_left = child; + else + parent->rb_right = child; + } + else + root->rb_node = child; + + color: + if (color == RB_BLACK) + __rb_erase_color(child, parent, root); +} + +/* + * This function returns the first node (in sort order) of the tree. + */ +struct rb_node *rb_first(const struct rb_root *root) +{ + struct rb_node *n; + + n = root->rb_node; + if (!n) + return NULL; + while (n->rb_left) + n = n->rb_left; + return n; +} + +struct rb_node *rb_last(const struct rb_root *root) +{ + struct rb_node *n; + + n = root->rb_node; + if (!n) + return NULL; + while (n->rb_right) + n = n->rb_right; + return n; +} + +struct rb_node *rb_next(const struct rb_node *node) +{ + struct rb_node *parent; + + if (rb_parent(node) == node) + return NULL; + + /* If we have a right-hand child, go down and then left as far + as we can. */ + if (node->rb_right) { + node = node->rb_right; + while (node->rb_left) + node=node->rb_left; + return (struct rb_node *)node; + } + + /* No right-hand children. Everything down and left is + smaller than us, so any 'next' node must be in the general + direction of our parent. Go up the tree; any time the + ancestor is a right-hand child of its parent, keep going + up. First time it's a left-hand child of its parent, said + parent is our 'next' node. */ + while ((parent = rb_parent(node)) && node == parent->rb_right) + node = parent; + + return parent; +} + +struct rb_node *rb_prev(const struct rb_node *node) +{ + struct rb_node *parent; + + if (rb_parent(node) == node) + return NULL; + + /* If we have a left-hand child, go down and then right as far + as we can. */ + if (node->rb_left) { + node = node->rb_left; + while (node->rb_right) + node=node->rb_right; + return (struct rb_node *)node; + } + + /* No left-hand children. Go up till we find an ancestor which + is a right-hand child of its parent */ + while ((parent = rb_parent(node)) && node == parent->rb_left) + node = parent; + + return parent; +} + +void rb_replace_node(struct rb_node *victim, struct rb_node *new, + struct rb_root *root) +{ + struct rb_node *parent = rb_parent(victim); + + /* Set the surrounding nodes to point to the replacement */ + if (parent) { + if (victim == parent->rb_left) + parent->rb_left = new; + else + parent->rb_right = new; + } else { + root->rb_node = new; + } + if (victim->rb_left) + rb_set_parent(victim->rb_left, new); + if (victim->rb_right) + rb_set_parent(victim->rb_right, new); + + /* Copy the pointers/colour from the victim to the replacement */ + *new = *victim; +} 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/core/sercomm.c b/src/core/sercomm.c new file mode 100644 index 00000000..1798acec --- /dev/null +++ b/src/core/sercomm.c @@ -0,0 +1,337 @@ +/* (C) 2010,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. + * + */ + +/*! \addtogroup sercomm + * @{ + * Serial communications layer, based on HDLC. + * + * \file sercomm.c */ + +#include "config.h" + +#include <stdint.h> +#include <stdio.h> +#include <errno.h> + +#include <osmocom/core/msgb.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/sercomm.h> +#include <osmocom/core/linuxlist.h> + +#ifndef EMBEDDED +# define DEFAULT_RX_MSG_SIZE 2048 +/*! Protect against IRQ context */ +void sercomm_drv_lock(unsigned long __attribute__((unused)) *flags) {} +/*! Release protection against IRQ context */ +void sercomm_drv_unlock(unsigned long __attribute__((unused)) *flags) {} +#else +# define DEFAULT_RX_MSG_SIZE 256 +#endif /* EMBEDDED */ + +/* weak symbols to be overridden by application */ +__attribute__((weak)) void sercomm_drv_start_tx(struct osmo_sercomm_inst *sercomm) {}; +__attribute__((weak)) int sercomm_drv_baudrate_chg(struct osmo_sercomm_inst *sercomm, uint32_t bdrt) +{ + return -1; +} + +#define HDLC_FLAG 0x7E +#define HDLC_ESCAPE 0x7D + +#define HDLC_C_UI 0x03 +#define HDLC_C_P_BIT (1 << 4) +#define HDLC_C_F_BIT (1 << 4) + +enum rx_state { + RX_ST_WAIT_START, + RX_ST_ADDR, + RX_ST_CTRL, + RX_ST_DATA, + RX_ST_ESCAPE, +}; + +/*! Initialize an Osmocom sercomm instance + * \param sercomm Caller-allocated sercomm instance to be initialized + * + * This function initializes the sercomm instance, including the + * registration of the ECHO service at the ECHO DLCI + */ +void osmo_sercomm_init(struct osmo_sercomm_inst *sercomm) +{ + unsigned int i; + for (i = 0; i < ARRAY_SIZE(sercomm->tx.dlci_queues); i++) + INIT_LLIST_HEAD(&sercomm->tx.dlci_queues[i]); + + sercomm->rx.msg = NULL; + if (!sercomm->rx.msg_size) + sercomm->rx.msg_size = DEFAULT_RX_MSG_SIZE; + sercomm->initialized = 1; + + /* set up the echo dlci */ + osmo_sercomm_register_rx_cb(sercomm, SC_DLCI_ECHO, &osmo_sercomm_sendmsg); +} + +/*! Determine if a given Osmocom sercomm instance has been initialized + * \param[in] sercomm Osmocom sercomm instance to be checked + * \returns 1 in case \a sercomm was previously initialized; 0 otherwise */ +int osmo_sercomm_initialized(struct osmo_sercomm_inst *sercomm) +{ + return sercomm->initialized; +} + +/*! User interface for transmitting messages for a given DLCI + * \param[in] sercomm Osmocom sercomm instance through which to transmit + * \param[in] dlci DLCI through whcih to transmit \a msg + * \param[in] msg Message buffer to be transmitted via \a dlci on \a * sercomm + **/ +void osmo_sercomm_sendmsg(struct osmo_sercomm_inst *sercomm, uint8_t dlci, struct msgb *msg) +{ + unsigned long flags; + uint8_t *hdr; + + /* prepend address + control octet */ + hdr = msgb_push(msg, 2); + hdr[0] = dlci; + hdr[1] = HDLC_C_UI; + + /* This functiion can be called from any context: FIQ, IRQ + * and supervisor context. Proper locking is important! */ + sercomm_drv_lock(&flags); + msgb_enqueue(&sercomm->tx.dlci_queues[dlci], msg); + sercomm_drv_unlock(&flags); + + /* tell UART that we have something to send */ + sercomm_drv_start_tx(sercomm); +} + +/*! How deep is the Tx queue for a given DLCI? + * \param[n] sercomm Osmocom sercomm instance on which to operate + * \param[in] dlci DLCI whose queue depthy is to be determined + * \returns number of elements in the per-DLCI transmit queue */ +unsigned int osmo_sercomm_tx_queue_depth(struct osmo_sercomm_inst *sercomm, uint8_t dlci) +{ + struct llist_head *le; + unsigned int num = 0; + + llist_for_each(le, &sercomm->tx.dlci_queues[dlci]) { + num++; + } + + return num; +} + +/*! wait until everything has been transmitted, then grab the lock and + * change the baud rate as requested + * \param[in] sercomm Osmocom sercomm instance + * \param[in] bdrt New UART Baud Rate + * \returns result of the operation as provided by sercomm_drv_baudrate_chg() + */ +int osmo_sercomm_change_speed(struct osmo_sercomm_inst *sercomm, uint32_t bdrt) +{ + unsigned int i, count; + unsigned long flags; + + while (1) { + /* count the number of pending messages */ + count = 0; + for (i = 0; i < ARRAY_SIZE(sercomm->tx.dlci_queues); i++) + count += osmo_sercomm_tx_queue_depth(sercomm, i); + /* if we still have any in the queue, restart */ + if (count == 0) + break; + } + + while (1) { + /* no messages in the queue, grab the lock to ensure it + * stays that way */ + sercomm_drv_lock(&flags); + if (!sercomm->tx.msg && !sercomm->tx.next_char) { + int rc; + /* change speed */ + rc = sercomm_drv_baudrate_chg(sercomm, bdrt); + sercomm_drv_unlock(&flags); + return rc; + } else + sercomm_drv_unlock(&flags); + } + return -1; +} + +/*! fetch one octet of to-be-transmitted serial data + * \param[in] sercomm Sercomm Instance from which to fetch pending data + * \param[out] ch pointer to caller-allocaed output memory + * \returns 1 in case of succss; 0 if no data available; negative on error */ +int osmo_sercomm_drv_pull(struct osmo_sercomm_inst *sercomm, uint8_t *ch) +{ + unsigned long flags; + + /* we may be called from interrupt context, but we stiff need to lock + * because sercomm could be accessed from a FIQ context ... */ + + sercomm_drv_lock(&flags); + + if (!sercomm->tx.msg) { + unsigned int i; + /* dequeue a new message from the queues */ + for (i = 0; i < ARRAY_SIZE(sercomm->tx.dlci_queues); i++) { + sercomm->tx.msg = msgb_dequeue(&sercomm->tx.dlci_queues[i]); + if (sercomm->tx.msg) + break; + } + if (sercomm->tx.msg) { + /* start of a new message, send start flag octet */ + *ch = HDLC_FLAG; + sercomm->tx.next_char = sercomm->tx.msg->data; + sercomm_drv_unlock(&flags); + return 1; + } else { + /* no more data avilable */ + sercomm_drv_unlock(&flags); + return 0; + } + } + + if (sercomm->tx.state == RX_ST_ESCAPE) { + /* we've already transmitted the ESCAPE octet, + * we now need to transmit the escaped data */ + *ch = *sercomm->tx.next_char++; + sercomm->tx.state = RX_ST_DATA; + } else if (sercomm->tx.next_char >= sercomm->tx.msg->tail) { + /* last character has already been transmitted, + * send end-of-message octet */ + *ch = HDLC_FLAG; + /* we've reached the end of the message buffer */ + msgb_free(sercomm->tx.msg); + sercomm->tx.msg = NULL; + sercomm->tx.next_char = NULL; + /* escaping for the two control octets */ + } else if (*sercomm->tx.next_char == HDLC_FLAG || + *sercomm->tx.next_char == HDLC_ESCAPE || + *sercomm->tx.next_char == 0x00) { + /* send an escape octet */ + *ch = HDLC_ESCAPE; + /* invert bit 5 of the next octet to be sent */ + *sercomm->tx.next_char ^= (1 << 5); + sercomm->tx.state = RX_ST_ESCAPE; + } else { + /* standard case, simply send next octet */ + *ch = *sercomm->tx.next_char++; + } + + sercomm_drv_unlock(&flags); + return 1; +} + +/*! Register a handler for a given DLCI + * \param sercomm Sercomm Instance in which caller wishes to register + * \param[in] dlci Data Ling Connection Identifier to register + * \param[in] cb Callback function for \a dlci + * \returns 0 on success; negative on error */ +int osmo_sercomm_register_rx_cb(struct osmo_sercomm_inst *sercomm, uint8_t dlci, dlci_cb_t cb) +{ + if (dlci >= ARRAY_SIZE(sercomm->rx.dlci_handler)) + return -EINVAL; + + if (sercomm->rx.dlci_handler[dlci]) + return -EBUSY; + + sercomm->rx.dlci_handler[dlci] = cb; + return 0; +} + +/* dispatch an incoming message once it is completely received */ +static void dispatch_rx_msg(struct osmo_sercomm_inst *sercomm, uint8_t dlci, struct msgb *msg) +{ + if (dlci >= ARRAY_SIZE(sercomm->rx.dlci_handler) || + !sercomm->rx.dlci_handler[dlci]) { + msgb_free(msg); + return; + } + sercomm->rx.dlci_handler[dlci](sercomm, dlci, msg); +} + +/*! the driver has received one byte, pass it into sercomm layer + * \param[in] sercomm Sercomm Instance for which a byte was received + * \param[in] ch byte that was received from line for said instance + * \returns 1 on success; 0 on unrecognized char; negative on error */ +int osmo_sercomm_drv_rx_char(struct osmo_sercomm_inst *sercomm, uint8_t ch) +{ + uint8_t *ptr; + + /* we are always called from interrupt context in this function, + * which means that any data structures we use need to be for + * our exclusive access */ + if (!sercomm->rx.msg) + sercomm->rx.msg = osmo_sercomm_alloc_msgb(sercomm->rx.msg_size); + + if (msgb_tailroom(sercomm->rx.msg) == 0) { + //cons_puts("sercomm_drv_rx_char() overflow!\n"); + msgb_free(sercomm->rx.msg); + sercomm->rx.msg = osmo_sercomm_alloc_msgb(sercomm->rx.msg_size); + sercomm->rx.state = RX_ST_WAIT_START; + return 0; + } + + switch (sercomm->rx.state) { + case RX_ST_WAIT_START: + if (ch != HDLC_FLAG) + break; + sercomm->rx.state = RX_ST_ADDR; + break; + case RX_ST_ADDR: + sercomm->rx.dlci = ch; + sercomm->rx.state = RX_ST_CTRL; + break; + case RX_ST_CTRL: + sercomm->rx.ctrl = ch; + sercomm->rx.state = RX_ST_DATA; + break; + case RX_ST_DATA: + if (ch == HDLC_ESCAPE) { + /* drop the escape octet, but change state */ + sercomm->rx.state = RX_ST_ESCAPE; + break; + } else if (ch == HDLC_FLAG) { + /* message is finished */ + dispatch_rx_msg(sercomm, sercomm->rx.dlci, sercomm->rx.msg); + /* allocate new buffer */ + sercomm->rx.msg = NULL; + /* start all over again */ + sercomm->rx.state = RX_ST_WAIT_START; + + /* do not add the control char */ + break; + } + /* default case: store the octet */ + ptr = msgb_put(sercomm->rx.msg, 1); + *ptr = ch; + break; + case RX_ST_ESCAPE: + /* store bif-5-inverted octet in buffer */ + ch ^= (1 << 5); + ptr = msgb_put(sercomm->rx.msg, 1); + *ptr = ch; + /* transition back to normal DATA state */ + sercomm->rx.state = RX_ST_DATA; + break; + } + + return 1; +} + +/*! @} */ diff --git a/src/core/serial.c b/src/core/serial.c new file mode 100644 index 00000000..117c049b --- /dev/null +++ b/src/core/serial.c @@ -0,0 +1,279 @@ +/*! \file serial.c + * Utility functions to deal with serial ports */ +/* + * Copyright (C) 2011 Sylvain Munaut <tnt@246tNt.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 serial + * @{ + * Osmocom serial port helpers + * + * \file serial.c */ + +#include <errno.h> +#include <fcntl.h> +#include <stdio.h> +#include <termios.h> +#include <unistd.h> +#include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> +#ifdef __linux__ +#include <linux/serial.h> +#endif + +#include <osmocom/core/serial.h> + + +#if 0 +# define dbg_perror(x) perror(x) +#else +# define dbg_perror(x) do { } while (0) +#endif + +/*! Open serial device and does base init + * \param[in] dev Path to the device node to open + * \param[in] baudrate Baudrate constant (speed_t: B9600, B...) + * \returns >=0 file descriptor in case of success or negative errno. + */ +int +osmo_serial_init(const char *dev, speed_t baudrate) +{ + int rc, fd=-1, v24, flags; + struct termios tio; + + /* Use nonblock as the device might block otherwise */ + fd = open(dev, O_RDWR | O_NOCTTY | O_SYNC | O_NONBLOCK); + if (fd < 0) { + dbg_perror("open"); + return -errno; + } + + /* now put it into blocking mode */ + flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + dbg_perror("fcntl get flags"); + rc = -errno; + goto error; + } + + flags &= ~O_NONBLOCK; + rc = fcntl(fd, F_SETFL, flags); + if (rc != 0) { + dbg_perror("fcntl set flags"); + rc = -errno; + goto error; + } + + /* Configure serial interface */ + rc = tcgetattr(fd, &tio); + if (rc < 0) { + dbg_perror("tcgetattr()"); + rc = -errno; + goto error; + } + + 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); + tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tio.c_iflag |= (INPCK); + tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR); + tio.c_oflag &= ~(OPOST | ONLCR); + + rc = tcsetattr(fd, TCSANOW, &tio); + if (rc < 0) { + dbg_perror("tcsetattr()"); + rc = -errno; + goto error; + } + + /* Set ready to read/write */ + v24 = TIOCM_DTR | TIOCM_RTS; + rc = ioctl(fd, TIOCMBIS, &v24); + if (rc < 0) { + dbg_perror("ioctl(TIOCMBIS)"); + /* some serial porst don't support this, so let's not + * return an error here */ + } + + return fd; + +error: + if (fd >= 0) + close(fd); + return rc; +} + +static int +_osmo_serial_set_baudrate(int fd, speed_t baudrate) +{ + int rc; + struct termios tio; + + rc = tcgetattr(fd, &tio); + if (rc < 0) { + dbg_perror("tcgetattr()"); + return -errno; + } + + 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("tcsetattr()"); + return -errno; + } + + return 0; +} + +/*! Change current baudrate + * \param[in] fd File descriptor of the open device + * \param[in] baudrate Baudrate constant (speed_t: B9600, B...) + * \returns 0 for success or negative errno. + */ +int +osmo_serial_set_baudrate(int fd, speed_t baudrate) +{ + osmo_serial_clear_custom_baudrate(fd); + return _osmo_serial_set_baudrate(fd, baudrate); +} + +/*! Change current baudrate to a custom one using OS specific method + * \param[in] fd File descriptor of the open device + * \param[in] baudrate Baudrate as integer + * \returns 0 for success or negative errno. + * + * This function might not work on all OS or with all type of serial adapters + */ +int +osmo_serial_set_custom_baudrate(int fd, int baudrate) +{ +#ifdef __linux__ + int rc; + struct serial_struct ser_info; + + rc = ioctl(fd, TIOCGSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCGSERIAL)"); + return -errno; + } + + ser_info.flags = ASYNC_SPD_CUST | ASYNC_LOW_LATENCY; + ser_info.custom_divisor = ser_info.baud_base / baudrate; + + rc = ioctl(fd, TIOCSSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCSSERIAL)"); + return -errno; + } + + return _osmo_serial_set_baudrate(fd, B38400); /* 38400 is a kind of magic ... */ +#elif defined(__APPLE__) +#ifndef IOSSIOSPEED +#define IOSSIOSPEED _IOW('T', 2, speed_t) +#endif + int rc; + + unsigned int speed = baudrate; + rc = ioctl(fd, IOSSIOSPEED, &speed); + if (rc < 0) { + dbg_perror("ioctl(IOSSIOSPEED)"); + return -errno; + } + return 0; +#else +#pragma message ("osmo_serial_set_custom_baudrate: unsupported platform") + return 0; +#endif +} + +/*! Clear any custom baudrate + * \param[in] fd File descriptor of the open device + * \returns 0 for success or negative errno. + * + * This function might not work on all OS or with all type of serial adapters + */ +int +osmo_serial_clear_custom_baudrate(int fd) +{ +#ifdef __linux__ + int rc; + struct serial_struct ser_info; + + rc = ioctl(fd, TIOCGSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCGSERIAL)"); + return -errno; + } + + ser_info.flags = ASYNC_LOW_LATENCY; + ser_info.custom_divisor = 0; + + rc = ioctl(fd, TIOCSSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCSSERIAL)"); + return -errno; + } +#endif + 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/core/signal.c b/src/core/signal.c new file mode 100644 index 00000000..ba1555ae --- /dev/null +++ b/src/core/signal.c @@ -0,0 +1,118 @@ +/*! \file signal.c + * Generic signalling/notification infrastructure. */ +/* + * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.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/signal.h> +#include <osmocom/core/talloc.h> +#include <osmocom/core/linuxlist.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> + +/*! \addtogroup signal + * @{ + * Generic signalling/notification infrastructure. + * + * \file signal.c */ + + +void *tall_sigh_ctx; +static LLIST_HEAD(signal_handler_list); + +struct signal_handler { + struct llist_head entry; + unsigned int subsys; + osmo_signal_cbfn *cbfn; + void *data; +}; + +/*! Initialize a signal_handler talloc context for \ref osmo_signal_register_handler. + * Create a talloc context called "osmo_signal". + * \param[in] root_ctx talloc context used as parent for the new "osmo_signal" ctx. + * \returns the new osmo_signal talloc context, e.g. for reporting + */ +void *osmo_signal_talloc_ctx_init(void *root_ctx) { + tall_sigh_ctx = talloc_named_const(root_ctx, 0, "osmo_signal"); + return tall_sigh_ctx; +} + +/*! Register a new signal handler + * \param[in] subsys Subsystem number + * \param[in] cbfn Callback function + * \param[in] data Data passed through to callback + * \returns 0 on success; negative in case of error + */ +int osmo_signal_register_handler(unsigned int subsys, + osmo_signal_cbfn *cbfn, void *data) +{ + struct signal_handler *sig_data; + + sig_data = talloc_zero(tall_sigh_ctx, struct signal_handler); + if (!sig_data) + return -ENOMEM; + + sig_data->subsys = subsys; + sig_data->data = data; + sig_data->cbfn = cbfn; + + /* FIXME: check if we already have a handler for this subsys/cbfn/data */ + + llist_add_tail(&sig_data->entry, &signal_handler_list); + + return 0; +} + +/*! Unregister signal handler + * \param[in] subsys Subsystem number + * \param[in] cbfn Callback function + * \param[in] data Data passed through to callback + */ +void osmo_signal_unregister_handler(unsigned int subsys, + osmo_signal_cbfn *cbfn, void *data) +{ + struct signal_handler *handler; + + llist_for_each_entry(handler, &signal_handler_list, entry) { + if (handler->cbfn == cbfn && handler->data == data + && subsys == handler->subsys) { + llist_del(&handler->entry); + talloc_free(handler); + break; + } + } +} + +/*! dispatch (deliver) a new signal to all registered handlers + * \param[in] subsys Subsystem number + * \param[in] signal Signal number, + * \param[in] signal_data Data to be passed along to handlers + */ +void osmo_signal_dispatch(unsigned int subsys, unsigned int signal, + void *signal_data) +{ + struct signal_handler *handler; + + llist_for_each_entry(handler, &signal_handler_list, entry) { + if (handler->subsys != subsys) + continue; + (*handler->cbfn)(subsys, signal, handler->data, signal_data); + } +} + +/*! @} */ diff --git a/src/core/sockaddr_str.c b/src/core/sockaddr_str.c new file mode 100644 index 00000000..9f1e8972 --- /dev/null +++ b/src/core/sockaddr_str.c @@ -0,0 +1,513 @@ +/*! \file sockaddr_str.c + * Common implementation to store an IP address and port. + */ +/* + * (C) 2019 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * Author: neels@hofmeyr.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" + +#ifdef HAVE_NETINET_IN_H +#include <string.h> +#include <errno.h> +#include <netinet/in.h> +#include <arpa/inet.h> + +#include <osmocom/core/sockaddr_str.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/byteswap.h> + +/*! \addtogroup sockaddr_str + * + * Common operations to store IP address as a char string along with a uint16_t port number. + * + * Convert IP address string to/from in_addr and in6_addr, with bounds checking and basic housekeeping. + * + * The initial purpose is to store and translate IP address info between GSM CC and MGCP protocols -- GSM mostly using + * 32-bit IPv4 addresses, and MGCP forwarding addresses as ASCII character strings. + * + * (At the time of writing, there are no immediate IPv6 users that come to mind, but it seemed appropriate to + * accommodate both address families from the start.) + * + * @{ + * \file sockaddr_str.c + */ + +/*! Return true if all elements of the osmo_sockaddr_str instance are set. + * \param[in] sockaddr_str The instance to examine. + * \return True iff ip is nonempty, port is not 0 and af is set to either AF_INET or AF_INET6. + */ +bool osmo_sockaddr_str_is_set(const struct osmo_sockaddr_str *sockaddr_str) +{ + return sockaddr_str + && *sockaddr_str->ip + && sockaddr_str->port + && (sockaddr_str->af == AF_INET || sockaddr_str->af == AF_INET6); +} + +/*! Return true if IP and port are valid and nonzero. + * \param[in] sockaddr_str The instance to examine. + * \return True iff ip can be converted to a nonzero IP address, and port is not 0. + */ +bool osmo_sockaddr_str_is_nonzero(const struct osmo_sockaddr_str *sockaddr_str) +{ + uint32_t ipv4; + struct in6_addr ipv6_zero = {}; + struct in6_addr ipv6; + + if (!osmo_sockaddr_str_is_set(sockaddr_str)) + return false; + + switch (sockaddr_str->af) { + case AF_INET: + if (osmo_sockaddr_str_to_32(sockaddr_str, &ipv4)) + return false; + return ipv4 != 0; + + case AF_INET6: + if (osmo_sockaddr_str_to_in6_addr(sockaddr_str, &ipv6)) + return false; + return memcmp(&ipv6, &ipv6_zero, sizeof(ipv6)) != 0; + + default: + return false; + } +} + +/*! 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 + * feed invalid address strings, but the return value is only guaranteed to be meaningful if the input was valid. + * \param[in] ip Valid IP address string. + * \return AF_INET or AF_INET6, or AF_UNSPEC if neither '.' nor ':' are found in the string. + */ +int osmo_ip_str_type(const char *ip) +{ + if (!ip) + return AF_UNSPEC; + /* Could also be IPv4-mapped IPv6 format with both colons and dots: x:x:x:x:x:x:d.d.d.d */ + if (strchr(ip, ':')) + return AF_INET6; + if (strchr(ip, '.')) + return AF_INET; + return AF_UNSPEC; +} + +/*! 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. + * \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_str2(struct osmo_sockaddr_str *sockaddr_str, const char *ip) +{ + int rc; + if (!sockaddr_str) + return -ENOSPC; + if (!ip) + ip = ""; + 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; + if (rc >= sizeof(sockaddr_str->ip)) + return -ENOSPC; + if (sockaddr_str->af == AF_UNSPEC) + return -EINVAL; + 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. + * \param[in] port Port number. + * \return 0 on success, negative on error. + */ +int osmo_sockaddr_str_from_in_addr(struct osmo_sockaddr_str *sockaddr_str, const struct in_addr *addr, uint16_t port) +{ + if (!sockaddr_str) + return -ENOSPC; + *sockaddr_str = (struct osmo_sockaddr_str){ + .af = AF_INET, + .port = port, + }; + if (!inet_ntop(AF_INET, addr, sockaddr_str->ip, sizeof(sockaddr_str->ip))) + return -ENOSPC; + return 0; +} + +/*! Convert IPv6 address to osmo_sockaddr_str, and set port. + * \param[out] sockaddr_str The instance to copy to. + * \param[in] addr IPv6 address data. + * \param[in] port Port number. + * \return 0 on success, negative on error. + */ +int osmo_sockaddr_str_from_in6_addr(struct osmo_sockaddr_str *sockaddr_str, const struct in6_addr *addr, uint16_t port) +{ + if (!sockaddr_str) + return -ENOSPC; + *sockaddr_str = (struct osmo_sockaddr_str){ + .af = AF_INET6, + .port = port, + }; + if (!inet_ntop(AF_INET6, addr, sockaddr_str->ip, sizeof(sockaddr_str->ip))) + return -ENOSPC; + return 0; +} + +/*! 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. + * \return 0 on success, negative on error. + */ +int osmo_sockaddr_str_from_32(struct osmo_sockaddr_str *sockaddr_str, uint32_t ip, uint16_t port) +{ + struct in_addr addr; + if (!sockaddr_str) + return -ENOSPC; + addr.s_addr = ip; + return osmo_sockaddr_str_from_in_addr(sockaddr_str, &addr, 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_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. + * \return 0 on success, negative on error. + */ +int osmo_sockaddr_str_from_sockaddr_in(struct osmo_sockaddr_str *sockaddr_str, const struct sockaddr_in *src) +{ + if (!sockaddr_str) + return -ENOSPC; + if (!src) + return -EINVAL; + if (src->sin_family != AF_INET) + return -EINVAL; + return osmo_sockaddr_str_from_in_addr(sockaddr_str, &src->sin_addr, osmo_ntohs(src->sin_port)); +} + +/*! Convert IPv6 address and port to osmo_sockaddr_str. + * \param[out] sockaddr_str The instance to copy to. + * \param[in] src IPv6 address and port data. + * \return 0 on success, negative on error. + */ +int osmo_sockaddr_str_from_sockaddr_in6(struct osmo_sockaddr_str *sockaddr_str, const struct sockaddr_in6 *src) +{ + if (!sockaddr_str) + return -ENOSPC; + if (!src) + return -EINVAL; + if (src->sin6_family != AF_INET6) + return -EINVAL; + return osmo_sockaddr_str_from_in6_addr(sockaddr_str, &src->sin6_addr, osmo_ntohs(src->sin6_port)); +} + +/*! 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_sockaddr(struct osmo_sockaddr_str *sockaddr_str, const struct sockaddr_storage *src) +{ + const struct sockaddr_in *sin = (void*)src; + const struct sockaddr_in6 *sin6 = (void*)src; + if (!sockaddr_str) + return -ENOSPC; + if (!src) + return -EINVAL; + if (sin->sin_family == AF_INET) + return osmo_sockaddr_str_from_sockaddr_in(sockaddr_str, sin); + if (sin6->sin6_family == AF_INET6) + return osmo_sockaddr_str_from_sockaddr_in6(sockaddr_str, sin6); + return -EINVAL; +} + +/*! 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. + * \return 0 on success, negative on error (e.g. invalid IPv4 address string). + */ +int osmo_sockaddr_str_to_in_addr(const struct osmo_sockaddr_str *sockaddr_str, struct in_addr *dst) +{ + int rc; + if (!sockaddr_str) + return -EINVAL; + if (!dst) + return -ENOSPC; + if (sockaddr_str->af != AF_INET) + return -EAFNOSUPPORT; + rc = inet_pton(AF_INET, sockaddr_str->ip, dst); + if (rc != 1) + return -EINVAL; + return 0; +} + +/*! Convert osmo_sockaddr_str address string to IPv6 address data. + * \param[in] sockaddr_str The instance to convert the IP of. + * \param[out] dst IPv6 address data to write to. + * \return 0 on success, negative on error (e.g. invalid IPv6 address string). + */ +int osmo_sockaddr_str_to_in6_addr(const struct osmo_sockaddr_str *sockaddr_str, struct in6_addr *dst) +{ + int rc; + if (!sockaddr_str) + return -EINVAL; + if (!dst) + return -ENOSPC; + if (sockaddr_str->af != AF_INET6) + return -EINVAL; + rc = inet_pton(AF_INET6, sockaddr_str->ip, dst); + if (rc != 1) + return -EINVAL; + return 0; +} + +/*! 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 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) +{ + int rc; + struct in_addr addr; + if (!sockaddr_str) + return -EINVAL; + if (!ip) + return -ENOSPC; + rc = osmo_sockaddr_str_to_in_addr(sockaddr_str, &addr); + if (rc) + return rc; + *ip = addr.s_addr; + return 0; +} + +/*! 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 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_32h(const struct osmo_sockaddr_str *sockaddr_str, uint32_t *ip) +{ + int rc; + uint32_t ip_h; + if (!sockaddr_str) + return -EINVAL; + if (!ip) + return -ENOSPC; + rc = osmo_sockaddr_str_to_32(sockaddr_str, &ip_h); + if (rc) + return rc; + *ip = osmo_htonl(ip_h); + 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. + * \return 0 on success, negative on error (e.g. invalid IPv4 address string). + */ +int osmo_sockaddr_str_to_sockaddr_in(const struct osmo_sockaddr_str *sockaddr_str, struct sockaddr_in *dst) +{ + if (!sockaddr_str) + return -EINVAL; + if (!dst) + return -ENOSPC; + if (sockaddr_str->af != AF_INET) + return -EINVAL; + *dst = (struct sockaddr_in){ + .sin_family = sockaddr_str->af, + .sin_port = osmo_htons(sockaddr_str->port), + }; + return osmo_sockaddr_str_to_in_addr(sockaddr_str, &dst->sin_addr); +} + +/*! Convert osmo_sockaddr_str address string and port to IPv6 address and port data. + * \param[in] sockaddr_str The instance to convert the IP and port of. + * \param[out] dst IPv6 address and port data to write to. + * \return 0 on success, negative on error (e.g. invalid IPv6 address string). + */ +int osmo_sockaddr_str_to_sockaddr_in6(const struct osmo_sockaddr_str *sockaddr_str, struct sockaddr_in6 *dst) +{ + if (!sockaddr_str) + return -EINVAL; + if (!dst) + return -ENOSPC; + if (sockaddr_str->af != AF_INET6) + return -EINVAL; + *dst = (struct sockaddr_in6){ + .sin6_family = sockaddr_str->af, + .sin6_port = osmo_htons(sockaddr_str->port), + }; + return osmo_sockaddr_str_to_in6_addr(sockaddr_str, &dst->sin6_addr); +} + +/*! Convert osmo_sockaddr_str address string and port to IPv4 or IPv6 address and port data. + * Depending on sockaddr_str->af, dst will be handled as struct sockaddr_in or struct sockaddr_in6. + * \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_sockaddr(const struct osmo_sockaddr_str *sockaddr_str, struct sockaddr_storage *dst) +{ + if (!sockaddr_str) + return -EINVAL; + if (!dst) + return -ENOSPC; + switch (sockaddr_str->af) { + case AF_INET: + return osmo_sockaddr_str_to_sockaddr_in(sockaddr_str, (void*)dst); + case AF_INET6: + return osmo_sockaddr_str_to_sockaddr_in6(sockaddr_str, (void*)dst); + default: + return -EINVAL; + } +} + +/*! @} */ +#endif // HAVE_NETINET_IN_H diff --git a/src/core/socket.c b/src/core/socket.c new file mode 100644 index 00000000..fa5fb88a --- /dev/null +++ b/src/core/socket.c @@ -0,0 +1,2381 @@ +/* + * (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 + +#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 IPv6 Addr as first option of any addrinfo item in the addrinfo set */ +static void addrinfo_has_v4v6addr(const struct addrinfo **result, size_t result_count, bool *has_v4, bool *has_v6) +{ + size_t host_idx; + *has_v4 = false; + *has_v6 = false; + + for (host_idx = 0; host_idx < result_count; host_idx++) { + 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 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; + + for (host_idx = 0; host_idx < result_count; host_idx++) { + if (result[host_idx]->ai_family != AF_INET6) + continue; + if (memcmp(&((struct sockaddr_in6 *)result[host_idx]->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) + continue; + 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; + 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; +} + +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, rem_has_v4addr; + bool loc_has_v6addr, rem_has_v6addr; + struct sockaddr_in6 addrs_buf[OSMO_SOCK_MAX_ADDRS]; + char strbuf[512]; + + /* updated later in case of AF_UNSPEC */ + loc_has_v4addr = rem_has_v4addr = (family == AF_INET); + loc_has_v6addr = rem_has_v6addr = (family == AF_INET6); + + /* 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 addr in the set */ + if (family == AF_UNSPEC) + addrinfo_has_v4v6addr((const struct addrinfo **)res_loc, local_hosts_cnt, + &loc_has_v4addr, &loc_has_v6addr); + } + /* 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 addr in the set */ + if (family == AF_UNSPEC) + addrinfo_has_v4v6addr((const struct addrinfo **)res_rem, remote_hosts_cnt, + &rem_has_v4addr, &rem_has_v6addr); + } + + if (((flags & OSMO_SOCK_F_BIND) && (flags & OSMO_SOCK_F_CONNECT)) && + !addrinfo_has_in6addr_any((const struct addrinfo **)res_loc, local_hosts_cnt) && + (loc_has_v4addr != rem_has_v4addr || loc_has_v6addr != rem_has_v6addr)) { + 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(loc_has_v6addr ? AF_INET6 : AF_INET, + type, proto, flags); + if (sfd < 0) { + rc = sfd; + goto ret_freeaddrinfo; + } + + 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; + } + + if (pars && 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 && 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 && 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 */ + } + } + + /* 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; + } +} + +/*! 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; +} + +/*! 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; + + if (fd < 0) { + osmo_strlcpy(str, "<error-bad-fd>", str_len); + return -EBADF; + } + + /* 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; + } +} + +/*! 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)); +} + +#endif /* HAVE_SYS_SOCKET_H */ + +/*! @} */ 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/core/stats.c b/src/core/stats.c new file mode 100644 index 00000000..16e6f620 --- /dev/null +++ b/src/core/stats.c @@ -0,0 +1,807 @@ +/*! \file stats.c */ +/* + * (C) 2015 by sysmocom - s.f.m.c. GmbH + * Author: Jacob Erlbeck <jerlbeck@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 + * @{ + * + * This module implements periodic reporting of statistics / counters. + * It supports the notion of multiple \ref osmo_stats_reporter objects + * which independently of each other can report statistics at different + * configurable intervals to different destinations. + * + * In order to use this facility, you have to call \ref + * osmo_stats_init() once at application start-up and then create one or + * more \ref osmo_stats_reporter, either using the direct API functions + * or by using the optional VTY bindings: + * + * - reporting to any of the libosmocore log targets + * \ref osmo_stats_reporter_create_log() creates a new stats_reporter + * which reports to the libosmcoore \ref logging subsystem. + * + * - reporting to statsd (a front-end proxy for the Graphite/Carbon + * metrics server + * \ref osmo_stats_reporter_create_statsd() creates a new stats_reporter + * which reports via UDP to statsd. + * + * You can either use the above API functions directly to create \ref + * osmo_stats_reporter instances, or you can use the VTY support + * contained in libosmovty. See the "stats" configuration node + * installed by osmo_stats_vty_Add_cmds(). + * + * An \ref osmo_stats_reporter reports statistics on all of the following + * libosmocore internal counter/statistics objects: + * - \ref osmo_counter + * - \ref rate_ctr + * - \ref osmo_stat_item + * + * You do not need to do anything in particular to expose a given + * counter or stat_item, they are all exported automatically via any + * \ref osmo_stats_reporter. If you have multiple \ref + * osmo_stats_reporter, they will each report all counters/stat_items. + * + * \file stats.c */ + +#include "config.h" +#if !defined(EMBEDDED) + +#include <osmocom/core/byteswap.h> +#include <osmocom/core/stats.h> + +#include <unistd.h> +#include <string.h> +#include <stdint.h> +#include <errno.h> +#include <stdio.h> +#include <sys/types.h> +#include <inttypes.h> + +#ifdef HAVE_SYS_SOCKET_H +#include <sys/socket.h> +#include <netinet/in.h> +#include <arpa/inet.h> +#endif + +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.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/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 + +LLIST_HEAD(osmo_stats_reporter_list); +static void *osmo_stats_ctx = NULL; +static int is_initialised = 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_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, + const struct rate_ctr_desc *desc, + int64_t value, int64_t delta); +static int osmo_stats_reporter_log_send_item(struct osmo_stats_reporter *srep, + const struct osmo_stat_item_group *statg, + const struct osmo_stat_item_desc *desc, int64_t value); + +static int update_srep_config(struct osmo_stats_reporter *srep) +{ + int rc = 0; + + if (srep->running) { + if (srep->close) + rc = srep->close(srep); + srep->running = 0; + } + + if (!srep->enabled) + return rc; + + if (srep->open) + rc = srep->open(srep); + else + rc = 0; + + if (rc < 0) + srep->enabled = 0; + else + srep->running = 1; + + srep->force_single_flush = 1; + + return rc; +} + +static int osmo_stats_timer_cb(struct osmo_fd *ofd, unsigned int what) +{ + 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(); + + return 0; +} + +static int start_timer(void) +{ + int rc; + int interval = osmo_stats_config->interval; + + if (!is_initialised) + return -ESRCH; + + 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; +} + +struct osmo_stats_reporter *osmo_stats_reporter_alloc(enum osmo_stats_reporter_type type, + const char *name) +{ + struct osmo_stats_reporter *srep; + srep = talloc_zero(osmo_stats_ctx, struct osmo_stats_reporter); + if (!srep) + return NULL; + + srep->type = type; + if (name) + srep->name = talloc_strdup(srep, name); + srep->fd = -1; + + llist_add_tail(&srep->list, &osmo_stats_reporter_list); + + return srep; +} + +/*! Destroy a given stats_reporter. Takes care of first disabling it. + * \param[in] srep stats_reporter that shall be disabled + destroyed */ +void osmo_stats_reporter_free(struct osmo_stats_reporter *srep) +{ + osmo_stats_reporter_disable(srep); + llist_del(&srep->list); + talloc_free(srep); +} + +/*! 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; + 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. + * \param[in] type Type of stats_reporter to find + * \param[in] name Name of stats_reporter to find + * \returns stats_reporter matching \a type and \a name; NULL otherwise */ +struct osmo_stats_reporter *osmo_stats_reporter_find(enum osmo_stats_reporter_type type, + const char *name) +{ + struct osmo_stats_reporter *srep; + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (srep->type != type) + continue; + if (srep->name != name) { + if (name == NULL || srep->name == NULL || + strcmp(name, srep->name) != 0) + continue; + } + return srep; + } + return NULL; +} + +#ifdef HAVE_SYS_SOCKET_H + +/*! Set the remote (IP) address of a given stats_reporter. + * \param[in] srep stats_reporter whose remote address is to be set + * \param[in] addr String representation of remote IPv4 address + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_set_remote_addr(struct osmo_stats_reporter *srep, const char *addr) +{ + int rc; + struct sockaddr_in *sock_addr = (struct sockaddr_in *)&srep->dest_addr; + struct in_addr inaddr; + + if (!srep->have_net_config) + return -ENOTSUP; + + OSMO_ASSERT(addr != NULL); + + rc = inet_pton(AF_INET, addr, &inaddr); + if (rc <= 0) + return -EINVAL; + + sock_addr->sin_addr = inaddr; + sock_addr->sin_family = AF_INET; + srep->dest_addr_len = sizeof(*sock_addr); + + talloc_free(srep->dest_addr_str); + srep->dest_addr_str = talloc_strdup(srep, addr); + + return update_srep_config(srep); +} + +/*! Set the remote (UDP) port of a given stats_reporter + * \param[in] srep stats_reporter whose remote address is to be set + * \param[in] port UDP port of remote statsd to which we report + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_set_remote_port(struct osmo_stats_reporter *srep, int port) +{ + struct sockaddr_in *sock_addr = (struct sockaddr_in *)&srep->dest_addr; + + if (!srep->have_net_config) + return -ENOTSUP; + + srep->dest_port = port; + sock_addr->sin_port = osmo_htons(port); + + return update_srep_config(srep); +} + +/*! Set the local (IP) address of a given stats_reporter. + * \param[in] srep stats_reporter whose remote address is to be set + * \param[in] addr String representation of local IP address + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_set_local_addr(struct osmo_stats_reporter *srep, const char *addr) +{ + int rc; + struct sockaddr_in *sock_addr = (struct sockaddr_in *)&srep->bind_addr; + struct in_addr inaddr; + + if (!srep->have_net_config) + return -ENOTSUP; + + if (addr) { + rc = inet_pton(AF_INET, addr, &inaddr); + if (rc <= 0) + return -EINVAL; + } else { + inaddr.s_addr = INADDR_ANY; + } + + sock_addr->sin_addr = inaddr; + sock_addr->sin_family = AF_INET; + srep->bind_addr_len = addr ? sizeof(*sock_addr) : 0; + + talloc_free(srep->bind_addr_str); + srep->bind_addr_str = addr ? talloc_strdup(srep, addr) : NULL; + + return update_srep_config(srep); +} + +/*! Set the maximum transmission unit of a given stats_reporter. + * \param[in] srep stats_reporter whose remote address is to be set + * \param[in] mtu Maximum Transmission Unit of \a srep + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_set_mtu(struct osmo_stats_reporter *srep, int mtu) +{ + if (!srep->have_net_config) + return -ENOTSUP; + + if (mtu < 0) + return -EINVAL; + + srep->mtu = mtu; + + return update_srep_config(srep); +} +#endif /* HAVE_SYS_SOCKETS_H */ + +int osmo_stats_reporter_set_max_class(struct osmo_stats_reporter *srep, + enum osmo_stats_class class_id) +{ + if (class_id == OSMO_STATS_CLASS_UNKNOWN) + return -EINVAL; + + srep->max_class = class_id; + + return 0; +} + +/*! 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) + return -EINVAL; + + osmo_stats_config->interval = interval; + if (is_initialised) + start_timer(); + + 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 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) +{ + talloc_free(srep->name_prefix); + srep->name_prefix = prefix && strlen(prefix) > 0 ? + talloc_strdup(srep, prefix) : NULL; + + return update_srep_config(srep); +} + + +/*! Enable the given stats_reporter. + * \param[in] srep stats_reporter who is to be enabled + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_enable(struct osmo_stats_reporter *srep) +{ + srep->enabled = 1; + + return update_srep_config(srep); +} + +/*! Disable the given stats_reporter. + * \param[in] srep stats_reporter who is to be disabled + * \returns 0 on success; negative on error */ +int osmo_stats_reporter_disable(struct osmo_stats_reporter *srep) +{ + srep->enabled = 0; + + return update_srep_config(srep); +} + +/*** i/o helper functions ***/ + +#ifdef HAVE_SYS_SOCKET_H + +/*! Open the UDP socket for given stats_reporter. + * \param[in] srep stats_reporter whose UDP socket is to be opened + * ]returns 0 on success; negative otherwise */ +int osmo_stats_reporter_udp_open(struct osmo_stats_reporter *srep) +{ + int sock; + int rc; + int buffer_size = STATS_DEFAULT_BUFLEN; + + if (srep->fd != -1 && srep->close) + srep->close(srep); + + sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock == -1) + return -errno; + +#if defined(__APPLE__) && !defined(MSG_NOSIGNAL) + { + static int val = 1; + + rc = setsockopt(sock, SOL_SOCKET, SO_NOSIGPIPE, (void*)&val, sizeof(val)); + goto failed; + } +#endif + if (srep->bind_addr_len > 0) { + rc = bind(sock, &srep->bind_addr, srep->bind_addr_len); + if (rc == -1) + goto failed; + } + + srep->fd = sock; + + if (srep->mtu > 0) { + buffer_size = srep->mtu - 20 /* IP */ - 8 /* UDP */; + srep->agg_enabled = 1; + } + + srep->buffer = msgb_alloc(buffer_size, "stats buffer"); + if (!srep->buffer) + goto failed; + + return 0; + +failed: + rc = -errno; + close(sock); + + return rc; +} + +/*! Closee the UDP socket for given stats_reporter. + * \param[in] srep stats_reporter whose UDP socket is to be closed + * ]returns 0 on success; negative otherwise */ +int osmo_stats_reporter_udp_close(struct osmo_stats_reporter *srep) +{ + int rc; + if (srep->fd == -1) + return -EBADF; + + osmo_stats_reporter_send_buffer(srep); + + rc = close(srep->fd); + srep->fd = -1; + msgb_free(srep->buffer); + srep->buffer = NULL; + return rc == -1 ? -errno : 0; +} + +/*! Send given date to given stats_reporter. + * \param[in] srep stats_reporter whose UDP socket is to be opened + * \param[in] data string data to be sent + * \param[in] data_len Length of \a data in bytes + * \returns number of bytes on success; negative otherwise */ +int osmo_stats_reporter_send(struct osmo_stats_reporter *srep, const char *data, + int data_len) +{ + int rc; + + rc = sendto(srep->fd, data, data_len, +#ifdef MSG_NOSIGNAL + MSG_NOSIGNAL | +#endif + MSG_DONTWAIT, + &srep->dest_addr, srep->dest_addr_len); + + if (rc == -1) + rc = -errno; + + return rc; +} + +/*! Send current accumulated buffer to given stats_reporter. + * \param[in] srep stats_reporter whose UDP socket is to be opened + * \returns number of bytes on success; negative otherwise */ +int osmo_stats_reporter_send_buffer(struct osmo_stats_reporter *srep) +{ + int rc; + + if (!srep->buffer || msgb_length(srep->buffer) == 0) + return 0; + + rc = osmo_stats_reporter_send(srep, + (const char *)msgb_data(srep->buffer), msgb_length(srep->buffer)); + + msgb_trim(srep->buffer, 0); + + return rc; +} +#endif /* HAVE_SYS_SOCKET_H */ + +/*** log reporter ***/ + +/*! Create a stats_reporter that logs via libosmocore logging. + * A stats_reporter created via this function will simply print the statistics + * via the libosmocore logging framework, using DLSTATS subsystem and LOGL_INFO + * priority. The configuration of the libosmocore log targets define where this + * information will end up (ignored, text file, stderr, syslog, ...). + * \param[in] name Name of the to-be-created stats_reporter + * \returns stats_reporter on success; NULL on error */ +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; + + srep->send_counter = osmo_stats_reporter_log_send_counter; + srep->send_item = osmo_stats_reporter_log_send_item; + + return srep; +} + +static int osmo_stats_reporter_log_send(struct osmo_stats_reporter *srep, + const char *type, + const char *name1, unsigned int index1, const char *name2, int value, + const char *unit) +{ + LOGP(DLSTATS, LOGL_INFO, + "stats t=%s p=%s g=%s i=%u n=%s v=%d u=%s\n", + type, srep->name_prefix ? srep->name_prefix : "", + name1 ? name1 : "", index1, + name2, value, unit ? unit : ""); + + return 0; +} + + +static int osmo_stats_reporter_log_send_counter(struct osmo_stats_reporter *srep, + const struct rate_ctr_group *ctrg, + const struct rate_ctr_desc *desc, + int64_t value, int64_t delta) +{ + if (ctrg) + return osmo_stats_reporter_log_send(srep, "c", + ctrg->desc->group_name_prefix, + ctrg->idx, + desc->name, value, NULL); + else + return osmo_stats_reporter_log_send(srep, "c", + NULL, 0, + desc->name, value, NULL); +} + +static int osmo_stats_reporter_log_send_item(struct osmo_stats_reporter *srep, + const struct osmo_stat_item_group *statg, + const struct osmo_stat_item_desc *desc, int64_t value) +{ + return osmo_stats_reporter_log_send(srep, "i", + statg->desc->group_name_prefix, statg->idx, + desc->name, value, desc->unit); +} + +/*** helper for reporting ***/ + +static int osmo_stats_reporter_check_config(struct osmo_stats_reporter *srep, + unsigned int index, int class_id) +{ + if (class_id == OSMO_STATS_CLASS_UNKNOWN) + class_id = index != 0 ? + OSMO_STATS_CLASS_SUBSCRIBER : OSMO_STATS_CLASS_GLOBAL; + + return class_id <= srep->max_class; +} + +/*** generic rate counter support ***/ + +static int osmo_stats_reporter_send_counter(struct osmo_stats_reporter *srep, + const struct rate_ctr_group *ctrg, + const struct rate_ctr_desc *desc, + int64_t value, int64_t delta) +{ + if (!srep->send_counter) + return 0; + + return srep->send_counter(srep, ctrg, desc, value, delta); +} + +static int rate_ctr_handler( + struct rate_ctr_group *ctrg, struct rate_ctr *ctr, + const struct rate_ctr_desc *desc, void *sctx_) +{ + struct osmo_stats_reporter *srep; + int64_t delta = rate_ctr_difference(ctr); + + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (!srep->running) + continue; + + if (delta == 0 && !srep->force_single_flush) + continue; + + if (!osmo_stats_reporter_check_config(srep, + ctrg->idx, ctrg->desc->class_id)) + continue; + + osmo_stats_reporter_send_counter(srep, ctrg, desc, + ctr->current, delta); + + /* TODO: handle result (log?, inc counter(!)?) or remove it */ + } + + return 0; +} + +static int rate_ctr_group_handler(struct rate_ctr_group *ctrg, void *sctx_) +{ + rate_ctr_for_each_counter(ctrg, rate_ctr_handler, sctx_); + + return 0; +} + +/*** stat item support ***/ + +static int osmo_stats_reporter_send_item(struct osmo_stats_reporter *srep, + const struct osmo_stat_item_group *statg, + const struct osmo_stat_item_desc *desc, + int32_t value) +{ + if (!srep->send_item) + return 0; + + return srep->send_item(srep, statg, desc, value); +} + +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 prev_reported_value = item->reported.max; + int32_t new_value = item->value.max; + + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (!srep->running) + 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; + + if (!osmo_stats_reporter_check_config(srep, + statg->idx, statg->desc->class_id)) + continue; + + osmo_stats_reporter_send_item(srep, statg, item->desc, new_value); + } + + osmo_stat_item_flush(item); + + return 0; +} + +static int osmo_stat_item_group_handler(struct osmo_stat_item_group *statg, void *sctx_) +{ + osmo_stat_item_for_each_item(statg, osmo_stat_item_handler, sctx_); + + return 0; +} + +/*** osmo counter support ***/ + +static int handle_counter(struct osmo_counter *counter, void *sctx_) +{ + struct osmo_stats_reporter *srep; + struct rate_ctr_desc desc = {0}; + /* Fake a rate counter description */ + desc.name = counter->name; + desc.description = counter->description; + + int delta = osmo_counter_difference(counter); + + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (!srep->running) + continue; + + if (delta == 0 && !srep->force_single_flush) + continue; + + osmo_stats_reporter_send_counter(srep, NULL, &desc, + counter->value, delta); + + /* TODO: handle result (log?, inc counter(!)?) */ + } + + return 0; +} + + +/*** main reporting function ***/ + +static void flush_all_reporters(void) +{ + struct osmo_stats_reporter *srep; + + llist_for_each_entry(srep, &osmo_stats_reporter_list, list) { + if (!srep->running) + 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(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 */ + flush_all_reporters(); + TRACE(LIBOSMOCORE_STATS_DONE()); + + return 0; +} + +#endif /* !EMBEDDED */ + +/*! @} */ diff --git a/src/core/stats_statsd.c b/src/core/stats_statsd.c new file mode 100644 index 00000000..b27baff8 --- /dev/null +++ b/src/core/stats_statsd.c @@ -0,0 +1,202 @@ +/* + * (C) 2015 by sysmocom - s.f.m.c. GmbH + * Author: Jacob Erlbeck <jerlbeck@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_statsd.c */ + +#include "config.h" +#if !defined(EMBEDDED) + +#include <osmocom/core/stats.h> + +#include <string.h> +#include <stdint.h> +#include <inttypes.h> +#include <errno.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/logging.h> +#include <osmocom/core/rate_ctr.h> +#include <osmocom/core/stat_item.h> +#include <osmocom/core/msgb.h> +#include <osmocom/core/stats.h> + +static int osmo_stats_reporter_statsd_send_counter(struct osmo_stats_reporter *srep, + const struct rate_ctr_group *ctrg, + const struct rate_ctr_desc *desc, + int64_t value, int64_t delta); +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); + +/*! Create a stats_reporter reporting to statsd. This creates a stats_reporter + * instance which reports the related statistics data to statsd. + * \param[in] name Name of the to-be-created stats_reporter + * \returns stats_reporter on success; NULL on error */ +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; + + srep->open = osmo_stats_reporter_udp_open; + srep->close = osmo_stats_reporter_udp_close; + srep->send_counter = osmo_stats_reporter_statsd_send_counter; + srep->send_item = osmo_stats_reporter_statsd_send_item; + + return srep; +} + +/*! Replace all illegal ':' in the stats name, but not when used as value seperator. + * ':' is used as seperator between the name and the value in the statsd protocol. + * \param[inout] buf is a null terminated string containing name, value, unit. */ +static void osmo_stats_reporter_sanitize_name(char *buf) +{ + /* e.g. msc.loc_update_type:normal:1|c -> msc.loc_update_type.normal:1|c + * last is the seperator between name and value */ + char *last = strrchr(buf, ':'); + char *tmp = strchr(buf, ':'); + + if (!last) + return; + + while (tmp < last) { + *tmp = '.'; + tmp = strchr(buf, ':'); + } +} + +static int osmo_stats_reporter_statsd_send(struct osmo_stats_reporter *srep, + const char *name1, const char *index1, const char *name2, int64_t value, + const char *unit) +{ + char *buf; + int buf_size; + int nchars, rc = 0; + char *fmt = NULL; + char *prefix = srep->name_prefix; + int old_len = msgb_length(srep->buffer); + + if (prefix) { + 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) + 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) { + if (msgb_length(srep->buffer) > 0 && + msgb_tailroom(srep->buffer) > 0) + { + msgb_put_u8(srep->buffer, '\n'); + } + } + + buf = (char *)msgb_put(srep->buffer, 0); + buf_size = msgb_tailroom(srep->buffer); + + nchars = snprintf(buf, buf_size, fmt, + prefix, name1, name2, + value, unit, index1); + + if (nchars >= buf_size) { + /* Truncated */ + /* Restore original buffer (without trailing LF) */ + msgb_trim(srep->buffer, old_len); + /* Send it */ + rc = osmo_stats_reporter_send_buffer(srep); + + /* Try again */ + buf = (char *)msgb_put(srep->buffer, 0); + buf_size = msgb_tailroom(srep->buffer); + + nchars = snprintf(buf, buf_size, fmt, + prefix, name1, name2, + value, unit, index1); + + if (nchars >= buf_size) + return -EMSGSIZE; + } + + if (nchars > 0) { + osmo_stats_reporter_sanitize_name(buf); + msgb_trim(srep->buffer, msgb_length(srep->buffer) + nchars); + } + + if (!srep->agg_enabled) + rc = osmo_stats_reporter_send_buffer(srep); + + return rc; +} + +static int osmo_stats_reporter_statsd_send_counter(struct osmo_stats_reporter *srep, + const struct rate_ctr_group *ctrg, + const struct rate_ctr_desc *desc, + int64_t value, int64_t delta) +{ + 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) +{ + 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/core/strrb.c b/src/core/strrb.c new file mode 100644 index 00000000..c5a5ed6e --- /dev/null +++ b/src/core/strrb.c @@ -0,0 +1,172 @@ +/*! \file strrb.c + * Ringbuffer implementation, tailored for logging. + * This is a lossy ringbuffer. It keeps up to N of the newest messages, + * overwriting the oldest as newer ones come in. + * + * Ringbuffer assumptions, invarients, and notes: + * - start is the index of the first used index slot in the ring buffer. + * - end is the index of the next index slot in the ring buffer. + * - start == end => buffer is empty + * - Consequence: the buffer can hold at most size - 1 messages + * (if this were not the case, full and empty buffers would be indistinguishable + * given the conventions in this implementation). + * - Whenever the ringbuffer is full, start is advanced. The second oldest + * message becomes unreachable by valid indexes (end is not a valid index) + * and the oldest message is overwritten (if there was a message there, which + * is the case unless this is the first time the ringbuffer becomes full). + */ +/* + * (C) 2012-2013, Katerina Barone-Adesi <kat.obsc@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 utils + * @{ + * \file strrb.c */ + +#include <stdio.h> +#include <string.h> +#include <string.h> + +#include <osmocom/core/strrb.h> +#include <osmocom/core/talloc.h> + +/*! Create an empty, initialized osmo_strrb. + * \param[in] ctx The talloc memory context which should own this. + * \param[in] rb_size The number of message slots the osmo_strrb can hold. + * \returns A struct osmo_strrb* on success, NULL in case of error. + * + * This function creates and initializes a ringbuffer. + * Note that the ringbuffer stores at most rb_size - 1 messages. + */ +struct osmo_strrb *osmo_strrb_create(void *talloc_ctx, size_t rb_size) +{ + struct osmo_strrb *rb = NULL; + unsigned int i; + + rb = talloc_zero(talloc_ctx, struct osmo_strrb); + if (!rb) + goto alloc_error; + + /* start and end are zero already, which is correct */ + rb->size = rb_size; + + rb->buffer = talloc_array(rb, char *, rb->size); + if (!rb->buffer) + goto alloc_error; + for (i = 0; i < rb->size; i++) { + rb->buffer[i] = + talloc_zero_size(rb->buffer, RB_MAX_MESSAGE_SIZE); + if (!rb->buffer[i]) + goto alloc_error; + } + + return rb; + +alloc_error: /* talloc_free(NULL) is safe */ + talloc_free(rb); + return NULL; +} + +/*! Check if an osmo_strrb is empty. + * \param[in] rb The osmo_strrb to check. + * \returns True if the osmo_strrb is empty, false otherwise. + */ +bool osmo_strrb_is_empty(const struct osmo_strrb *rb) +{ + return rb->end == rb->start; +} + +/*! Return a pointer to the Nth string in the osmo_strrb. + * \param[in] rb The osmo_strrb to search. + * \param[in] string_index The index sought (N), zero-indexed. + * + * Return a pointer to the Nth string in the osmo_strrb. + * Return NULL if there is no Nth string. + * Note that N is zero-indexed. + * \returns A pointer to the target string on success, NULL in case of error. + */ +const char *osmo_strrb_get_nth(const struct osmo_strrb *rb, + unsigned int string_index) +{ + unsigned int offset = string_index + rb->start; + + if ((offset >= rb->size) && (rb->start > rb->end)) + offset -= rb->size; + if (_osmo_strrb_is_bufindex_valid(rb, offset)) + return rb->buffer[offset]; + + return NULL; +} + +bool _osmo_strrb_is_bufindex_valid(const struct osmo_strrb *rb, + unsigned int bufi) +{ + if (osmo_strrb_is_empty(rb)) + return 0; + if (bufi >= rb->size) + return 0; + if (rb->start < rb->end) + return (bufi >= rb->start) && (bufi < rb->end); + return (bufi < rb->end) || (bufi >= rb->start); +} + +/*! Count the number of log messages in an osmo_strrb. + * \param[in] rb The osmo_strrb to count the elements of. + * + * \returns The number of log messages in the osmo_strrb. + */ +size_t osmo_strrb_elements(const struct osmo_strrb *rb) +{ + if (rb->end < rb->start) + return rb->end + (rb->size - rb->start); + + return rb->end - rb->start; +} + +/*! Add a string to the osmo_strrb. + * \param[in] rb The osmo_strrb to add to. + * \param[in] data The string to add. + * + * Add a message to the osmo_strrb. + * Older messages will be overwritten as necessary. + * \returns 0 normally, 1 as a warning (ie, if data was truncated). + */ +int osmo_strrb_add(struct osmo_strrb *rb, const char *data) +{ + size_t len = strlen(data); + int ret = 0; + + if (len >= RB_MAX_MESSAGE_SIZE) { + len = RB_MAX_MESSAGE_SIZE - 1; + ret = 1; + } + + memcpy(rb->buffer[rb->end], data, len); + rb->buffer[rb->end][len] = '\0'; + + rb->end += 1; + rb->end %= rb->size; + + /* The buffer is full; oldest message is forgotten - see notes above */ + if (rb->end == rb->start) { + rb->start += 1; + rb->start %= rb->size; + } + return ret; +} + +/*! @} */ diff --git a/src/core/tdef.c b/src/core/tdef.c new file mode 100644 index 00000000..abbe581a --- /dev/null +++ b/src/core/tdef.c @@ -0,0 +1,371 @@ +/*! \file tdef.c + * Implementation to define Tnnn timers globally and use for FSM state changes. + */ +/* + * (C) 2018-2019 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * 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 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 <limits.h> +#include <errno.h> + +#include <osmocom/core/fsm.h> +#include <osmocom/core/tdef.h> + +/*! \addtogroup Tdef + * + * Implementation to define Tnnn timers globally and use for FSM state changes. + * + * See also \ref Tdef_VTY + * + * osmo_tdef provides: + * + * - a list of Tnnnn (GSM) timers with description, unit and default value. + * - vty UI to allow users to configure non-default timeouts. + * - API to tie T timers to osmo_fsm states and set them on state transitions. + * + * - a few standard units (minute, second, millisecond) as well as a custom unit + * (which relies on the timer's human readable description to indicate the + * meaning of the value). + * - conversion for standard units: for example, some GSM timers are defined in + * minutes, while our FSM definitions need timeouts in seconds. Conversion is + * for convenience only and can be easily avoided via the custom unit. + * + * 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/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/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/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. + * + * osmo_tdef was introduced because: + * + * - without osmo_tdef, each invocation of osmo_fsm_inst_state_chg() needs to be + * programmed with the right timeout value, for all code paths that invoke this + * state change. It is a likely source of errors to get one of them wrong. By + * defining a T timer exactly for an FSM state, the caller can merely invoke the + * state change and trust on the original state definition to apply the correct + * timeout. + * + * - it is helpful to have a standardized config file UI to provide user + * configurable timeouts, instead of inventing new VTY commands for each + * separate application of T timer numbers. See \ref tdef_vty.h. + * + * @{ + * \file tdef.c + */ + +/*! a = return_val * b. \return 0 if factor is below 1. */ +static unsigned long osmo_tdef_factor(enum osmo_tdef_unit a, enum osmo_tdef_unit b) +{ + if (b == a + || b == OSMO_TDEF_CUSTOM || a == OSMO_TDEF_CUSTOM) + 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: + return 1000; + case OSMO_TDEF_M: + return 60*1000; + default: + return 0; + } + case OSMO_TDEF_S: + switch (a) { + case OSMO_TDEF_M: + return 60; + default: + return 0; + } + default: + return 0; + } +} + +/*! \return val in unit to_unit, rounded up to the next integer value and clamped to ULONG_MAX, or 0 if val == 0. */ +static unsigned long osmo_tdef_round(unsigned long val, enum osmo_tdef_unit from_unit, enum osmo_tdef_unit to_unit) +{ + unsigned long f; + if (!val) + return 0; + + f = osmo_tdef_factor(from_unit, to_unit); + if (f == 1) + return val; + if (f < 1) { + f = osmo_tdef_factor(to_unit, from_unit); + return (val / f) + (val % f? 1 : 0); + } + /* range checking */ + if (f > (ULONG_MAX / val)) + return ULONG_MAX; + return val * f; +} + +/*! Set all osmo_tdef values to the default_val. + * It is convenient to define a tdefs array by setting only the default_val, and calling osmo_tdefs_reset() once for + * program startup. (See also osmo_tdef_vty_init()). + * During call to this function, default values are verified to be inside valid range; process is aborted otherwise. + * \param[in] tdefs Array of timer definitions, last entry being fully zero. + */ +void osmo_tdefs_reset(struct osmo_tdef *tdefs) +{ + struct osmo_tdef *t; + osmo_tdef_for_each(t, tdefs) { + if (!osmo_tdef_val_in_range(t, t->default_val)) { + char range_str[64]; + osmo_tdef_range_str_buf(range_str, sizeof(range_str), t); + osmo_panic("%s:%d Timer " OSMO_T_FMT " contains default value %lu not in range %s\n", + __FILE__, __LINE__, OSMO_T_FMT_ARGS(t->T), t->default_val, range_str); + } + t->val = t->default_val; + } +} + +/*! Return the value of a T timer from a list of osmo_tdef, in the given unit. + * If no such timer is defined, return the default value passed, or abort the program if default < 0. + * + * Round up any value match as_unit: 1100 ms as OSMO_TDEF_S becomes 2 seconds, as OSMO_TDEF_M becomes one minute. + * However, always return a value of zero as zero (0 ms as OSMO_TDEF_M still is 0 m). + * + * Range: even though the value range is unsigned long here, in practice, using ULONG_MAX as value for a timeout in + * seconds may actually wrap to negative or low timeout values (e.g. in struct timeval). It is recommended to stay below + * INT_MAX seconds. See also osmo_fsm_inst_state_chg(). + * + * Usage example: + * + * struct osmo_tdef global_T_defs[] = { + * { .T=7, .default_val=50, .desc="Water Boiling Timeout" }, // default is .unit=OSMO_TDEF_S == 0 + * { .T=8, .default_val=300, .desc="Tea brewing" }, + * { .T=9, .default_val=5, .unit=OSMO_TDEF_M, .desc="Let tea cool down before drinking" }, + * { .T=10, .default_val=20, .unit=OSMO_TDEF_M, .desc="Forgot to drink tea while it's warm" }, + * {} // <-- important! last entry shall be zero + * }; + * osmo_tdefs_reset(global_T_defs); // make all values the default + * osmo_tdef_vty_init(global_T_defs, CONFIG_NODE); + * + * val = osmo_tdef_get(global_T_defs, 7, OSMO_TDEF_S, -1); // -> 50 + * sleep(val); + * + * val = osmo_tdef_get(global_T_defs, 7, OSMO_TDEF_M, -1); // 50 seconds becomes 1 minute -> 1 + * sleep_minutes(val); + * + * val = osmo_tdef_get(global_T_defs, 99, OSMO_TDEF_S, 3); // not defined, returns 3 + * + * val = osmo_tdef_get(global_T_defs, 99, OSMO_TDEF_S, -1); // not defined, program aborts! + * + * \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; 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) +{ + const struct osmo_tdef *t = osmo_tdef_get_entry((struct osmo_tdef*)tdefs, T); + if (!t) { + OSMO_ASSERT(val_if_not_present >= 0); + return val_if_not_present; + } + return osmo_tdef_round(t->val, t->unit, as_unit); +} + +/*! Find tdef entry matching T. + * This is useful for manipulation, which is usually limited to the VTY configuration. To retrieve a timeout value, + * most callers probably should use osmo_tdef_get() instead. + * \param[in] tdefs Array of timer definitions, last entry being fully zero. + * \param[in] T Timer number to get the entry for. + * \return osmo_tdef entry matching T in given array, or NULL if no match is found. + */ +struct osmo_tdef *osmo_tdef_get_entry(struct osmo_tdef *tdefs, int T) +{ + struct osmo_tdef *t; + osmo_tdef_for_each(t, tdefs) { + if (t->T == T) + return t; + } + return NULL; +} + +/*! Set value in entry matching T, converting val from val_unit to unit of T. + * The converted value is rounded up to the next integer value of T's unit and clamped to ULONG_MAX, or 0 if val == 0. + * \param[in] tdefs Array of timer definitions, last entry being fully zero. + * \param[in] T Timer number to set the value for. + * \param[in] val The new timer value to set. + * \param[in] val_unit Units of value in parameter val. + * \return 0 on success, negative on error. + */ +int osmo_tdef_set(struct osmo_tdef *tdefs, int T, unsigned long val, enum osmo_tdef_unit val_unit) +{ + unsigned long new_val; + struct osmo_tdef *t = osmo_tdef_get_entry(tdefs, T); + if (!t) + return -EEXIST; + + new_val = osmo_tdef_round(val, val_unit, t->unit); + if (!osmo_tdef_val_in_range(t, new_val)) + return -ERANGE; + + t->val = new_val; + return 0; +} + +/*! Check if value new_val is in range of valid possible values for timer entry tdef. + * \param[in] tdef Timer entry from a timer definition table. + * \param[in] new_val The value whose validity to check, in units as per this timer entry. + * \return true if inside range, false otherwise. + */ +bool osmo_tdef_val_in_range(struct osmo_tdef *tdef, unsigned long new_val) +{ + return new_val >= tdef->min_val && (!tdef->max_val || new_val <= tdef->max_val); +} + +/*! Write string representation of osmo_tdef range into buf. + * \param[in] buf The buffer where the string representation is stored. + * \param[in] buf_len Length of buffer in bytes. + * \param[in] tdef Timer entry from a timer definition table. + * \return The number of characters printed on success (or number of characters + * which would have been written to the final string if enough space + * had been available), negative on error. See snprintf(). + */ +int osmo_tdef_range_str_buf(char *buf, size_t buf_len, struct osmo_tdef *t) +{ + int ret, len = 0, offset = 0, rem = buf_len; + + buf[0] = '\0'; + ret = snprintf(buf + offset, rem, "[%lu .. ", t->min_val); + if (ret < 0) + return ret; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + + if (t->max_val) + ret = snprintf(buf + offset, rem, "%lu]", t->max_val); + else + ret = snprintf(buf + offset, rem, "inf]"); + if (ret < 0) + return ret; + OSMO_SNPRINTF_RET(ret, rem, offset, len); + return len; +} + +/*! Using osmo_tdef for osmo_fsm_inst: find a given state's osmo_tdef_state_timeout entry. + * + * The timeouts_array shall contain exactly 32 elements, regardless whether only some of them are actually populated + * with nonzero values. 32 corresponds to the number of states allowed by the osmo_fsm_* API. Lookup is by array index. + * Not populated entries imply a state change invocation without timeout. + * + * For example: + * + * struct osmo_tdef_state_timeout my_fsm_timeouts[32] = { + * [MY_FSM_STATE_3] = { .T = 423 }, // look up timeout configured for T423 + * [MY_FSM_STATE_7] = { .keep_timer = true, .T = 235 }, // keep previous timer if running, or start T235 + * [MY_FSM_STATE_8] = { .keep_timer = true }, // keep previous state's T number, continue timeout. + * // any state that is omitted will remain zero == no timeout + * }; + * osmo_tdef_get_state_timeout(MY_FSM_STATE_0, &my_fsm_timeouts) -> NULL, + * osmo_tdef_get_state_timeout(MY_FSM_STATE_7, &my_fsm_timeouts) -> { .T = 235 } + * + * The intention is then to obtain the timer like osmo_tdef_get(global_T_defs, T=235); see also + * fsm_inst_state_chg_T() below. + * + * \param[in] state State constant to look up. + * \param[in] timeouts_array Array[32] of struct osmo_tdef_state_timeout defining which timer number to use per state. + * \return A struct osmo_tdef_state_timeout entry, or NULL if that entry is zero initialized. + */ +const struct osmo_tdef_state_timeout *osmo_tdef_get_state_timeout(uint32_t state, const struct osmo_tdef_state_timeout *timeouts_array) +{ + const struct osmo_tdef_state_timeout *t; + OSMO_ASSERT(state < 32); + t = &timeouts_array[state]; + if (!t->keep_timer && !t->T) + return NULL; + return t; +} + +/*! See invocation macro osmo_tdef_fsm_inst_state_chg() instead. + * \param[in] file Source file name, like __FILE__. + * \param[in] line Source file line number, like __LINE__. + */ +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, 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; + + /* 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->keep_timer) { + if (t->T) + return _osmo_fsm_inst_state_chg_keep_or_start_timer(fi, state, val, 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. + * 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); +} + +const struct value_string osmo_tdef_unit_names[] = { + { OSMO_TDEF_S, "s" }, + { 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/core/timer.c b/src/core/timer.c new file mode 100644 index 00000000..20d87a05 --- /dev/null +++ b/src/core/timer.c @@ -0,0 +1,290 @@ +/* + * (C) 2008,2009 by Holger Hans Peter Freyther <zecke@selfish.org> + * (C) 2011 by Harald Welte <laforge@gnumonks.org> + * All Rights Reserved + * + * Authors: Holger Hans Peter Freyther <zecke@selfish.org> + * Harald Welte <laforge@gnumonks.org> + * Pablo Neira Ayuso <pablo@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. + * + */ + + +/*! \addtogroup timer + * @{ + * Osmocom timer abstraction; modelled after linux kernel timers + * + * \file timer.c */ + +#include <assert.h> +#include <string.h> +#include <limits.h> +#include <osmocom/core/timer.h> +#include <osmocom/core/timer_compat.h> +#include <osmocom/core/linuxlist.h> + +/* These store the amount of time that we wait until next timer expires. */ +static __thread struct timeval nearest; +static __thread struct timeval *nearest_p; + +static __thread struct rb_root timer_root = RB_ROOT; + +static void __add_timer(struct osmo_timer_list *timer) +{ + struct rb_node **new = &(timer_root.rb_node); + struct rb_node *parent = NULL; + + while (*new) { + struct osmo_timer_list *this; + + this = container_of(*new, struct osmo_timer_list, node); + + parent = *new; + if (timercmp(&timer->timeout, &this->timeout, <)) + new = &((*new)->rb_left); + else + new = &((*new)->rb_right); + } + + rb_link_node(&timer->node, parent, new); + rb_insert_color(&timer->node, &timer_root); +} + +/*! set up timer callback and data + * \param[in] timer the timer that should be added + * \param[in] cb function to be called when timer expires + * \param[in] data pointer to data that passed to the callback function + */ +void osmo_timer_setup(struct osmo_timer_list *timer, void (*cb)(void *data), + void *data) +{ + timer->cb = cb; + timer->data = data; +} + +/*! add a new timer to the timer management + * \param[in] timer the timer that should be added + */ +void osmo_timer_add(struct osmo_timer_list *timer) +{ + osmo_timer_del(timer); + timer->active = 1; + INIT_LLIST_HEAD(&timer->list); + __add_timer(timer); +} + +/*! schedule a timer at a given future relative time + * \param[in] timer the to-be-added timer + * \param[in] seconds number of seconds from now + * \param[in] microseconds number of microseconds from now + * + * This function can be used to (re-)schedule a given timer at a + * specified number of seconds+microseconds in the future. It will + * internally add it to the timer management data structures, thus + * osmo_timer_add() is automatically called. + */ +void +osmo_timer_schedule(struct osmo_timer_list *timer, int seconds, int microseconds) +{ + struct timeval current_time; + + osmo_gettimeofday(¤t_time, NULL); + timer->timeout.tv_sec = seconds; + timer->timeout.tv_usec = microseconds; + timeradd(&timer->timeout, ¤t_time, &timer->timeout); + osmo_timer_add(timer); +} + +/*! delete a timer from timer management + * \param[in] timer the to-be-deleted timer + * + * This function can be used to delete a previously added/scheduled + * timer from the timer management code. + */ +void osmo_timer_del(struct osmo_timer_list *timer) +{ + if (timer->active) { + timer->active = 0; + rb_erase(&timer->node, &timer_root); + /* make sure this is not already scheduled for removal. */ + if (!llist_empty(&timer->list)) + llist_del_init(&timer->list); + } +} + +/*! check if given timer is still pending + * \param[in] timer the to-be-checked timer + * \return 1 if pending, 0 otherwise + * + * 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(const struct osmo_timer_list *timer) +{ + return timer->active; +} + +/*! compute the remaining time of a timer + * \param[in] timer the to-be-checked timer + * \param[in] now the current time (NULL if not known) + * \param[out] remaining remaining time until timer fires + * \return 0 if timer has not expired yet, -1 if it has + * + * This function can be used to determine the amount of time + * remaining until the expiration of the timer. + */ +int osmo_timer_remaining(const struct osmo_timer_list *timer, + const struct timeval *now, + struct timeval *remaining) +{ + struct timeval current_time; + + if (!now) + osmo_gettimeofday(¤t_time, NULL); + else + current_time = *now; + + timersub(&timer->timeout, ¤t_time, remaining); + + if (remaining->tv_sec < 0) + return -1; + + return 0; +} + +/*! Determine time between now and the nearest timer + * \returns pointer to timeval of nearest timer, NULL if there is none + * + * if we have a nearest time return the delta between the current + * time and the time of the nearest timer. + * If the nearest timer timed out return NULL and then we will + * dispatch everything after the select + */ +struct timeval *osmo_timers_nearest(void) +{ + /* nearest_p is exactly what we need already: NULL if nothing is + * waiting, {0,0} if we must dispatch immediately, and the correct + * delay if we need to wait */ + 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; + nearest_ms += nearest_p->tv_usec / 1000; + + return nearest_ms; +} + +static void update_nearest(struct timeval *cand, struct timeval *current) +{ + if (cand->tv_sec != LONG_MAX) { + if (timercmp(cand, current, >)) + timersub(cand, current, &nearest); + else { + /* loop again inmediately */ + timerclear(&nearest); + } + nearest_p = &nearest; + } else { + nearest_p = NULL; + } +} + +/*! Find the nearest time and update nearest_p */ +void osmo_timers_prepare(void) +{ + struct rb_node *node; + struct timeval current; + + osmo_gettimeofday(¤t, NULL); + + node = rb_first(&timer_root); + if (node) { + struct osmo_timer_list *this; + this = container_of(node, struct osmo_timer_list, node); + update_nearest(&this->timeout, ¤t); + } else { + nearest_p = NULL; + } +} + +/*! fire all timers... and remove them */ +int osmo_timers_update(void) +{ + struct timeval current_time; + struct rb_node *node; + struct llist_head timer_eviction_list; + struct osmo_timer_list *this; + int work = 0; + + osmo_gettimeofday(¤t_time, NULL); + + INIT_LLIST_HEAD(&timer_eviction_list); + for (node = rb_first(&timer_root); node; node = rb_next(node)) { + this = container_of(node, struct osmo_timer_list, node); + + if (timercmp(&this->timeout, ¤t_time, >)) + break; + + llist_add(&this->list, &timer_eviction_list); + } + + /* + * The callbacks might mess with our list and in this case + * even llist_for_each_entry_safe is not safe to use. To allow + * osmo_timer_del to be called from within the callback we need + * to restart the iteration for each element scheduled for removal. + * + * The problematic scenario is the following: Given two timers A + * and B that have expired at the same time. Thus, they are both + * in the eviction list in this order: A, then B. If we remove + * timer B from the A's callback, we continue with B in the next + * iteration step, leading to an access-after-release. + */ +restart: + llist_for_each_entry(this, &timer_eviction_list, list) { + osmo_timer_del(this); + if (this->cb) + this->cb(this->data); + work = 1; + goto restart; + } + + return work; +} + +/*! Check how many timers we have in the system + * \returns number of \ref osmo_timer_list registered */ +int osmo_timers_check(void) +{ + struct rb_node *node; + int i = 0; + + for (node = rb_first(&timer_root); node; node = rb_next(node)) { + i++; + } + return i; +} + +/*! @} */ diff --git a/src/core/timer_clockgettime.c b/src/core/timer_clockgettime.c new file mode 100644 index 00000000..6112b8a5 --- /dev/null +++ b/src/core/timer_clockgettime.c @@ -0,0 +1,139 @@ +/* + * (C) 2016 by sysmocom s.f.m.c. GmbH <info@sysmocom.de> + * All Rights Reserved + * + * Authors: 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 timer + * @{ + * \file timer_clockgettime.c + * Overriding Time: osmo_clock_gettime() + * - Useful to write and reproduce tests that depend on specific time + * factors. This API allows to fake the timespec provided by `clock_gettime()` + * by using a small shim osmo_clock_gettime(). + * - Choose the clock you want to override, for instance CLOCK_MONOTONIC. + * - If the clock override is disabled (default) for a given clock, + * osmo_clock_gettime() will do the same as regular `clock_gettime()`. + * - If you want osmo_clock_gettime() to provide a specific time, you must + * enable time override with osmo_clock_override_enable(), + * then set a pointer to the timespec storing the fake time for that + * specific clock (`struct timespec *ts = + * osmo_clock_override_gettimespec()`) and set it as + * desired. Next time osmo_clock_gettime() is called, it will return the + * values previously set through the ts pointer. + * - A helper osmo_clock_override_add() is provided to increment a given + * overriden clock with a specific amount of time. + */ + +/*! \file timer_clockgettime.c + */ + +#include "config.h" +#ifdef HAVE_CLOCK_GETTIME + +#include <stdlib.h> +#include <stdbool.h> +#include <sys/time.h> +#include <time.h> + +#include <osmocom/core/timer_compat.h> + +/*! An internal structure to handle overriden time for each clock type. */ +struct fakeclock { + bool override; + struct timespec time; +}; + +static struct fakeclock realtime; +static struct fakeclock realtime_coarse; +static struct fakeclock mono; +static struct fakeclock mono_coarse; +static struct fakeclock mono_raw; +static struct fakeclock boottime; +static struct fakeclock boottime; +static struct fakeclock proc_cputime_id; +static struct fakeclock th_cputime_id; + +static struct fakeclock* clkid_to_fakeclock(clockid_t clk_id) +{ + switch(clk_id) { + case CLOCK_REALTIME: + return &realtime; + case CLOCK_REALTIME_COARSE: + return &realtime_coarse; + case CLOCK_MONOTONIC: + return &mono; + case CLOCK_MONOTONIC_COARSE: + return &mono_coarse; + case CLOCK_MONOTONIC_RAW: + return &mono_raw; + case CLOCK_BOOTTIME: + return &boottime; + case CLOCK_PROCESS_CPUTIME_ID: + return &proc_cputime_id; + case CLOCK_THREAD_CPUTIME_ID: + return &th_cputime_id; + default: + return NULL; + } +} + +/*! Shim around clock_gettime to be able to set the time manually. + * + * To override, use osmo_clock_override_enable and set the desired + * current time with osmo_clock_gettimespec. */ +int osmo_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + struct fakeclock* c = clkid_to_fakeclock(clk_id); + if (!c || !c->override) + return clock_gettime(clk_id, tp); + + *tp = c->time; + return 0; +} + +/*! Convenience function to enable or disable a specific clock fake time. + */ +void osmo_clock_override_enable(clockid_t clk_id, bool enable) +{ + struct fakeclock* c = clkid_to_fakeclock(clk_id); + if (c) + c->override = enable; +} + +/*! Convenience function to return a pointer to the timespec handling the + * fake time for clock clk_id. */ +struct timespec *osmo_clock_override_gettimespec(clockid_t clk_id) +{ + struct fakeclock* c = clkid_to_fakeclock(clk_id); + if (c) + return &c->time; + return NULL; +} + +/*! Convenience function to advance the fake time. + * + * Adds the given values to the clock time. */ +void osmo_clock_override_add(clockid_t clk_id, time_t secs, long nsecs) +{ + struct timespec val = { secs, nsecs }; + struct fakeclock* c = clkid_to_fakeclock(clk_id); + if (c) + timespecadd(&c->time, &val, &c->time); +} + +#endif /* HAVE_CLOCK_GETTIME */ + +/*! @} */ diff --git a/src/core/timer_gettimeofday.c b/src/core/timer_gettimeofday.c new file mode 100644 index 00000000..e0212b5a --- /dev/null +++ b/src/core/timer_gettimeofday.c @@ -0,0 +1,75 @@ +/* + * (C) 2016 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de> + * Authors: 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. + * + */ + +/*! \addtogroup timer + * @{ + * \file timer_gettimeofday.c + * Overriding Time: osmo_gettimeofday() + * - Useful to write and reproduce tests that depend on specific time + * factors. This API allows to fake the timeval provided by `gettimeofday()` + * by using a small shim osmo_gettimeofday(). + * - If the clock override is disabled (default) for a given clock, + * osmo_gettimeofday() will do the same as regular `gettimeofday()`. + * - If you want osmo_gettimeofday() to provide a specific time, you must + * enable time override by setting the global variable + * osmo_gettimeofday_override (`osmo_gettimeofday_override = true`), then + * set the global struct timeval osmo_gettimeofday_override_time wih the + * desired value. Next time osmo_gettimeofday() is called, it will return + * the values previously set. + * - A helper osmo_gettimeofday_override_add() is provided to easily + * increment osmo_gettimeofday_override_time with a specific amount of + * time. + */ + +#include <stdbool.h> +#include <sys/time.h> +#include <osmocom/core/timer_compat.h> + +bool osmo_gettimeofday_override = false; +struct timeval osmo_gettimeofday_override_time = { 23, 424242 }; + +/*! shim around gettimeofday to be able to set the time manually. + * To override, set osmo_gettimeofday_override == true and set the desired + * current time in osmo_gettimeofday_override_time. + * + * N. B: gettimeofday() is affected by discontinuous jumps in the system time + * (e.g., if the system administrator manually changes the system time). + * Hence this should NEVER be used for elapsed time computation. + * Instead, osmo_clock_gettime() with CLOCK_MONOTONIC should be used for that. + */ +int osmo_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + if (osmo_gettimeofday_override) { + *tv = osmo_gettimeofday_override_time; + return 0; + } + + return gettimeofday(tv, tz); +} + +/*! convenience function to advance the fake time. + * Add the given values to osmo_gettimeofday_override_time. */ +void osmo_gettimeofday_override_add(time_t secs, suseconds_t usecs) +{ + struct timeval val = { secs, usecs }; + timeradd(&osmo_gettimeofday_override_time, &val, + &osmo_gettimeofday_override_time); +} + +/*! @} */ 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/core/use_count.c b/src/core/use_count.c new file mode 100644 index 00000000..9714403d --- /dev/null +++ b/src/core/use_count.c @@ -0,0 +1,306 @@ +/*! \file use_count.c + * Generic object usage counter Implementation (get, put and deallocate on zero count). + */ +/* + * (C) 2019 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 <inttypes.h> +#include <string.h> + +#include <osmocom/core/linuxlist.h> +#include <osmocom/core/utils.h> +#include <osmocom/core/use_count.h> + +/*! \addtogroup use_count + * + * Generic object usage counter (get, put and deallocate on zero count). + * + * For an example and a detailed description, see struct osmo_use_count. + * + * @{ + * \file use_count.c + */ + +/*! Add two int32_t but make sure to min- and max-clamp at INT32_MIN and INT32_MAX, respectively. */ +static inline bool count_safe(int32_t *val_p, int32_t add) +{ + int32_t val = *val_p; + + /* A simpler implementation would just let the integer overflow and compare with previous value afterwards, but + * that causes runtime errors in the address sanitizer. So let's just do this without tricks. */ + if (add < 0 && val < 0 && val - INT32_MIN < -add) { + *val_p = INT32_MIN; + return false; + } + + if (add > 0 && val > 0 && INT32_MAX - val < add) { + *val_p = INT32_MAX; + return false; + } + + *val_p = val + add; + return true; +} + +/*! Return the sum of all use counts, min- and max-clamped at INT32_MIN and INT32_MAX. + * \param[in] uc Use counts to sum up. + * \return Accumulated counts, or 0 if uc is NULL. + */ +int32_t osmo_use_count_total(const struct osmo_use_count *uc) +{ + struct osmo_use_count_entry *e; + int32_t total = 0; + + if (!uc || !uc->use_counts.next) + return 0; + + llist_for_each_entry(e, &uc->use_counts, entry) { + count_safe(&total, e->count); + } + return total; +} + +/*! Return use count by a single use token. + * \param[in] uc Use counts to look up in. + * \param[in] use Use token. + * \return Use count, or 0 if uc is NULL or use token is not present. + */ +int32_t osmo_use_count_by(const struct osmo_use_count *uc, const char *use) +{ + const struct osmo_use_count_entry *e; + if (!uc) + return 0; + e = osmo_use_count_find(uc, use); + if (!e) + return 0; + return e->count; +} + +/*! 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 buf, always nul-terminated (except when buf_len < 1). + */ +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; + bool first; + + 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) + continue; + if (!first) + OSMO_STRBUF_PRINTF(sb, ","); + first = false; + if (e->count != 1) + OSMO_STRBUF_PRINTF(sb, "%" PRId32 "*", e->count); + OSMO_STRBUF_PRINTF(sb, "%s", e->use ? : "NULL"); + } + if (first) + OSMO_STRBUF_PRINTF(sb, "-"); + +uninitialized: + OSMO_STRBUF_PRINTF(sb, ")"); + 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. + * \param[in] uc Use counts to look up in. + * \param[in] use Use token. + * \return matching entry, or NULL if not present. + */ +struct osmo_use_count_entry *osmo_use_count_find(const struct osmo_use_count *uc, const char *use) +{ + struct osmo_use_count_entry *e; + if (!uc->use_counts.next) + return NULL; + llist_for_each_entry(e, &uc->use_counts, entry) { + if (e->use == use || (use && e->use && !strcmp(e->use, use))) + return e; + } + return NULL; +} + +/*! Find a use count entry that currently has zero count, and re-use that for this new use token. */ +static struct osmo_use_count_entry *osmo_use_count_repurpose_zero_entry(struct osmo_use_count *uc, const char *use) +{ + struct osmo_use_count_entry *e; + if (!uc->use_counts.next) + return NULL; + llist_for_each_entry(e, &uc->use_counts, entry) { + if (!e->count) { + e->use = use; + return e; + } + } + return NULL; +} + +/*! Allocate a new use count entry, happens implicitly in osmo_use_count_get_put(). */ +static struct osmo_use_count_entry *osmo_use_count_create(struct osmo_use_count *uc, const char *use) +{ + struct osmo_use_count_entry *e = talloc_zero(uc->talloc_object, struct osmo_use_count_entry); + if (!e) + return NULL; + *e = (struct osmo_use_count_entry){ + .use_count = uc, + .use = use, + }; + if (!uc->use_counts.next) + INIT_LLIST_HEAD(&uc->use_counts); + llist_add_tail(&e->entry, &uc->use_counts); + return e; +} + +/*! Deallocate a use count entry. + * Normally, this is not necessary -- it is ok and even desirable to leave use count entries around even when they reach + * a count of zero, until the use_count->talloc_object deallocates and removes all of them in one flush. This avoids + * repeated allocation and deallocation for use tokens, because use count entries that have reached zero count are + * repurposed for any other use tokens. A cleanup makes sense only if a very large number of differing use tokens surged + * at the same time, and the owning object will not be deallocated soon; if so, this should be done by the + * osmo_use_count_cb_t implementation. + * + * osmo_use_count_free() must *not* be called on use count entries that were added by + * osmo_use_count_make_static_entries(). This is the responsibility of the osmo_use_count_cb_t() implementation. + * + * \param[in] use_count_entry Use count entry to unlist and free. + */ +void osmo_use_count_free(struct osmo_use_count_entry *use_count_entry) +{ + if (!use_count_entry) + return; + llist_del(&use_count_entry->entry); + talloc_free(use_count_entry); +} + +/*! Implementation for osmo_use_count_get_put(), which can also be directly invoked to pass source file information. For + * arguments besides file and line, see osmo_use_count_get_put(). + * \param[in] file Source file path, as in __FILE__. + * \param[in] line Source file line, as in __LINE__. + */ +int _osmo_use_count_get_put(struct osmo_use_count *uc, const char *use, int32_t change, + const char *file, int line) +{ + struct osmo_use_count_entry *e; + int32_t old_use_count; + if (!uc) + return -EINVAL; + if (!change) + return 0; + + e = osmo_use_count_find(uc, use); + if (!e) + e = osmo_use_count_repurpose_zero_entry(uc, use); + if (!e) + e = osmo_use_count_create(uc, use); + if (!e) + return -ENOMEM; + + if (!e->count) { + /* move to end */ + llist_del(&e->entry); + llist_add_tail(&e->entry, &uc->use_counts); + } + + old_use_count = e->count; + if (!count_safe(&e->count, change)) { + e->count = old_use_count; + return -ERANGE; + } + + if (uc->use_cb) + return uc->use_cb(e, old_use_count, file, line); + return 0; +} + +/*! Add N static use token entries to avoid dynamic allocation of use count tokens. + * When not using this function, use count entries are talloc allocated from uc->talloc_object as talloc context. This + * means that there are small dynamic allocations for each use count token. osmo_use_count_get_put() normally leaves + * zero-count entries around and re-purposes them later, so the number of small allocations is at most the number of + * concurrent differently-named uses of the same object. If that is not enough, this function allows completely avoiding + * dynamic use count allocations, by adding N static entries with a zero count and a NULL use token. They will be used + * by osmo_use_count_get_put(), and, if the caller avoids using osmo_use_count_free(), the osmo_use_count implementation + * never deallocates them. The idea is that the entries are members of the uc->talloc_object, or that they will by other + * means be implicitly deallocated by the talloc_object. It is fine to call + * osmo_use_count_make_static_entries(buf_n_entries=N) and later have more than N concurrent uses, i.e. it is no problem + * to mix static and dynamic entries. To completely avoid dynamic use count entries, N has to >= the maximum number of + * concurrent differently-named uses that will occur in the lifetime of the talloc_object. + * + * struct my_object { + * struct osmo_use_count use_count; + * struct osmo_use_count_entry use_count_buf[3]; // planning for 3 concurrent users + * }; + * + * void example() { + * struct my_object *o = talloc_zero(ctx, struct my_object); + * osmo_use_count_make_static_entries(&o->use_count, o->use_count_buf, ARRAY_SIZE(o->use_count_buf)); + * } + */ +void osmo_use_count_make_static_entries(struct osmo_use_count *uc, struct osmo_use_count_entry *buf, + size_t buf_n_entries) +{ + size_t idx; + if (!uc->use_counts.next) + INIT_LLIST_HEAD(&uc->use_counts); + for (idx = 0; idx < buf_n_entries; idx++) { + struct osmo_use_count_entry *e = &buf[idx]; + *e = (struct osmo_use_count_entry){ + .use_count = uc, + }; + llist_add_tail(&e->entry, &uc->use_counts); + } +} + +/*! @} */ diff --git a/src/core/utils.c b/src/core/utils.c new file mode 100644 index 00000000..231b65c9 --- /dev/null +++ b/src/core/utils.c @@ -0,0 +1,1539 @@ +/* + * (C) 2011 by Harald Welte <laforge@gnumonks.org> + * (C) 2011 by Sylvain Munaut <tnt@246tNt.com> + * (C) 2014 by Nils O. Selåsdal <noselasd@fiane.dyndns.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 <stdbool.h> +#include <string.h> +#include <stdint.h> +#include <errno.h> +#include <stdio.h> +#include <inttypes.h> +#include <limits.h> + +#include <osmocom/core/utils.h> +#include <osmocom/core/bit64gen.h> + + +/*! \addtogroup utils + * @{ + * various utility routines + * + * \file utils.c */ + +static __thread char namebuf[255]; +/* shared by osmo_str_tolower() and osmo_str_toupper() */ +static __thread char capsbuf[128]; + +/*! get human-readable string for given value + * \param[in] vs Array of value_string tuples + * \param[in] val Value to be converted + * \returns pointer to human-readable string + * + * If val is found in vs, the array's string entry is returned. Otherwise, an + * "unknown" string containing the actual value is composed in a static buffer + * that is reused across invocations. + */ +const char *get_value_string(const struct value_string *vs, uint32_t val) +{ + const char *str = get_value_string_or_null(vs, val); + if (str) + return str; + + snprintf(namebuf, sizeof(namebuf), "unknown 0x%"PRIx32, val); + namebuf[sizeof(namebuf) - 1] = '\0'; + return namebuf; +} + +/*! get human-readable string or NULL for given value + * \param[in] vs Array of value_string tuples + * \param[in] val Value to be converted + * \returns pointer to human-readable string or NULL if val is not found + */ +const char *get_value_string_or_null(const struct value_string *vs, + uint32_t val) +{ + int i; + + if (!vs) + return NULL; + + for (i = 0;; i++) { + if (vs[i].value == 0 && vs[i].str == NULL) + break; + if (vs[i].value == val) + return vs[i].str; + } + + return NULL; +} + +/*! get numeric value for given human-readable string + * \param[in] vs Array of value_string tuples + * \param[in] str human-readable string + * \returns numeric value (>0) or negative numer in case of error + */ +int get_string_value(const struct value_string *vs, const char *str) +{ + int i; + + for (i = 0;; i++) { + if (vs[i].value == 0 && vs[i].str == NULL) + break; + if (!strcasecmp(vs[i].str, str)) + return vs[i].value; + } + return -EINVAL; +} + +/*! Convert BCD-encoded digit into printable character + * \param[in] bcd A single BCD-encoded digit + * \returns single printable character + */ +char osmo_bcd2char(uint8_t bcd) +{ + if (bcd < 0xa) + return '0' + bcd; + else + return 'A' + (bcd - 0xa); +} + +/*! Convert number in ASCII to BCD value + * \param[in] c ASCII character + * \returns BCD encoded value of character + */ +uint8_t osmo_char2bcd(char c) +{ + if (c >= '0' && c <= '9') + return c - 0x30; + else if (c >= 'A' && c <= 'F') + return 0xa + (c - 'A'); + else if (c >= 'a' && c <= 'f') + return 0xa + (c - 'a'); + else + return 0; +} + +/*! Convert BCD to string. + * The given nibble offsets are interpreted in BCD order, i.e. nibble 0 is bcd[0] & 0xf, nibble 1 is bcd[0] >> 4, nibble + * 3 is bcd[1] & 0xf, etc.. + * \param[out] dst Output string buffer, is always nul terminated when dst_size > 0. + * \param[in] dst_size sizeof() the output string buffer. + * \param[in] bcd Binary coded data buffer. + * \param[in] start_nibble Offset to start from, in nibbles, typically 1 to skip the first nibble. + * \param[in] end_nibble Offset to stop before, in nibbles, e.g. sizeof(bcd)*2 - (bcd[0] & GSM_MI_ODD? 0:1). + * \param[in] allow_hex If false, return error if there are digits other than 0-9. If true, return those as [A-F]. + * \returns The strlen that would be written if the output buffer is large enough, excluding nul byte (like + * snprintf()), or -EINVAL if allow_hex is false and a digit > 9 is encountered. On -EINVAL, the conversion is + * still completed as if allow_hex were passed as true. Return -ENOMEM if dst is NULL or dst_size is zero. + * If end_nibble <= start_nibble, write an empty string to dst and return 0. + */ +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; + int nibble_i; + int rc = 0; + + if (!dst || dst_size < 1 || start_nibble < 0) + return -ENOMEM; + + 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)) + nibble >>= 4; + nibble &= 0xf; + + if (!allow_hex && nibble > 9) + rc = -EINVAL; + + *dst = osmo_bcd2char(nibble); + } + *dst = '\0'; + + if (rc < 0) + return rc; + 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, unsigned int max_len) + +{ + char c; + uint8_t v; + const char *strpos; + unsigned int nibblepos = 0; + + memset(b, 0x00, max_len); + + for (strpos = str; (c = *strpos); strpos++) { + /* skip whitespace */ + if (c == ' ' || c == '\t' || c == '\n' || c == '\r') + continue; + + /* If the buffer is too small, error out */ + if (nibblepos >= (max_len << 1)) + return -1; + + if (c >= '0' && c <= '9') + v = c - '0'; + else if (c >= 'a' && c <= 'f') + v = 10 + (c - 'a'); + else if (c >= 'A' && c <= 'F') + v = 10 + (c - 'A'); + else + return -1; + + b[nibblepos >> 1] |= v << (nibblepos & 1 ? 0 : 4); + nibblepos ++; + } + + /* In case of uneven amount of digits, the last byte is not complete + * and that's an error. */ + if (nibblepos & 1) + return -1; + + return nibblepos >> 1; +} + +static __thread char hexd_buff[4096]; +static const char hex_chars[] = "0123456789abcdef"; + +/*! Convert binary sequence to hexadecimal ASCII string. + * \param[out] out_buf Output buffer to write the resulting string to. + * \param[in] out_buf_size sizeof(out_buf). + * \param[in] buf Input buffer, pointer to sequence of bytes. + * \param[in] len Length of input buf in number of bytes. + * \param[in] delim String to separate each byte; NULL or "" for no delim. + * \param[in] delim_after_last If true, end the string in delim (true: "1a:ef:d9:", false: "1a:ef:d9"); + * if out_buf has insufficient space, the string will always end in a delim. + * \returns out_buf, containing a zero-terminated string, or "" (empty string) if out_buf == NULL or out_buf_size < 1. + * + * This function will print a sequence of bytes as hexadecimal numbers, adding one delim between each byte (e.g. for + * delim passed as ":", return a string like "1a:ef:d9"). + * + * The delim_after_last argument exists to be able to exactly show the original osmo_hexdump() behavior, which always + * ends the string with a delimiter. + */ +const char *osmo_hexdump_buf(char *out_buf, size_t out_buf_size, const unsigned char *buf, int len, const char *delim, + bool delim_after_last) +{ + int i; + char *cur = out_buf; + size_t delim_len; + + if (!out_buf || !out_buf_size) + return ""; + + delim = delim ? : ""; + delim_len = strlen(delim); + + for (i = 0; i < len; i++) { + const char *delimp = delim; + int len_remain = out_buf_size - (cur - out_buf) - 1; + if (len_remain < (int) (2 + delim_len) + && !(!delim_after_last && i == (len - 1) && len_remain >= 2)) + break; + + *cur++ = hex_chars[buf[i] >> 4]; + *cur++ = hex_chars[buf[i] & 0xf]; + + if (i == (len - 1) && !delim_after_last) + break; + + while (len_remain > 1 && *delimp) { + *cur++ = *delimp++; + len_remain--; + } + } + *cur = '\0'; + return out_buf; +} + +/*! Convert a sequence of unpacked bits to ASCII string, in user-supplied buffer. + * \param[out] buf caller-provided output string buffer + * \param[out] buf_len size of buf in bytes + * \param[in] bits A sequence of unpacked bits + * \param[in] len Length of bits + * \return The output buffer (buf). + */ +char *osmo_ubit_dump_buf(char *buf, size_t buf_len, const uint8_t *bits, unsigned int len) +{ + unsigned int i; + + if (len > buf_len-1) + len = buf_len-1; + memset(buf, 0, buf_len); + + for (i = 0; i < len; i++) { + char outch; + switch (bits[i]) { + case 0: + outch = '0'; + break; + case 0xff: + outch = '?'; + break; + case 1: + outch = '1'; + break; + default: + outch = 'E'; + break; + } + buf[i] = outch; + } + buf[buf_len-1] = 0; + return buf; +} + +/*! Convert a sequence of unpacked bits to ASCII string, in static buffer. + * \param[in] bits A sequence of unpacked bits + * \param[in] len Length of bits + * \returns string representation in static buffer. + */ +char *osmo_ubit_dump(const uint8_t *bits, unsigned int len) +{ + return osmo_ubit_dump_buf(hexd_buff, sizeof(hexd_buff), bits, len); +} + +/*! Convert binary sequence to hexadecimal ASCII string + * \param[in] buf pointer to sequence of bytes + * \param[in] len length of buf in number of bytes + * \returns pointer to zero-terminated string + * + * 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(const unsigned char *buf, int len) +{ + osmo_hexdump_buf(hexd_buff, sizeof(hexd_buff), buf, len, " ", true); + return hexd_buff; +} + +/*! Convert binary sequence to hexadecimal ASCII string + * \param[in] ctx talloc context from where to allocate the output string + * \param[in] buf pointer to sequence of bytes + * \param[in] len length of buf in number of bytes + * \returns pointer to zero-terminated string + * + * This function will print a sequence of bytes as hexadecimal numbers, + * adding one space character between each byte (e.g. "1a ef d9") + */ +char *osmo_hexdump_c(const void *ctx, const unsigned char *buf, int len) +{ + size_t hexd_buff_len = len * 3 + 1; + char *hexd_buff = talloc_size(ctx, hexd_buff_len); + if (!hexd_buff) + return NULL; + osmo_hexdump_buf(hexd_buff, hexd_buff_len, buf, len, " ", true); + return hexd_buff; +} + +/*! Convert binary sequence to hexadecimal ASCII string + * \param[in] buf pointer to sequence of bytes + * \param[in] len length of buf in number of bytes + * \returns pointer to zero-terminated string + * + * 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(const unsigned char *buf, int len) +{ + osmo_hexdump_buf(hexd_buff, sizeof(hexd_buff), buf, len, "", true); + return hexd_buff; +} + +/*! Convert binary sequence to hexadecimal ASCII string + * \param[in] ctx talloc context from where to allocate the output string + * \param[in] buf pointer to sequence of bytes + * \param[in] len length of buf in number of bytes + * \returns pointer to zero-terminated string + * + * This function will print a sequence of bytes as hexadecimal numbers, + * without any space character between each byte (e.g. "1aefd9") + */ +char *osmo_hexdump_nospc_c(const void *ctx, const unsigned char *buf, int len) +{ + size_t hexd_buff_len = len * 2 + 1; + char *hexd_buff = talloc_size(ctx, hexd_buff_len); + if (!hexd_buff) + return NULL; + osmo_hexdump_buf(hexd_buff, hexd_buff_len, buf, len, "", true); + return hexd_buff; +} + + +/* Compat with previous typo to preserve abi */ +char *osmo_osmo_hexdump_nospc(const unsigned char *buf, int len) +#if defined(__MACH__) && defined(__APPLE__) + ; +#else + __attribute__((weak, alias("osmo_hexdump_nospc"))); +#endif + +#include "config.h" +#ifdef HAVE_CTYPE_H +#include <ctype.h> +/*! Convert an entire string to lower case + * \param[out] out output string, caller-allocated + * \param[in] in input string + */ +void osmo_str2lower(char *out, const char *in) +{ + unsigned int i; + + for (i = 0; i < strlen(in); i++) + out[i] = tolower((const unsigned char)in[i]); + out[strlen(in)] = '\0'; +} + +/*! Convert an entire string to upper case + * \param[out] out output string, caller-allocated + * \param[in] in input string + */ +void osmo_str2upper(char *out, const char *in) +{ + unsigned int i; + + for (i = 0; i < strlen(in); i++) + out[i] = toupper((const unsigned char)in[i]); + out[strlen(in)] = '\0'; +} +#endif /* HAVE_CTYPE_H */ + +/*! Wishful thinking to generate a constant time compare + * \param[in] exp Expected data + * \param[in] rel Comparison value + * \param[in] count Number of bytes to compare + * \returns 1 in case \a exp equals \a rel; zero otherwise + * + * Compare count bytes of exp to rel. Return 0 if they are identical, 1 + * otherwise. Do not return a mismatch on the first mismatching byte, + * but always compare all bytes, regardless. The idea is that the amount of + * matching bytes cannot be inferred from the time the comparison took. */ +int osmo_constant_time_cmp(const uint8_t *exp, const uint8_t *rel, const int count) +{ + int x = 0, i; + + for (i = 0; i < count; ++i) + x |= exp[i] ^ rel[i]; + + /* if x is zero, all data was identical */ + return x? 1 : 0; +} + +/*! Generic retrieval of 1..8 bytes as big-endian uint64_t + * \param[in] data Input data as byte-array + * \param[in] data_len Length of \a data in octets + * \returns uint64_t of \a data interpreted as big-endian + * + * This is like osmo_load64be_ext, except that if data_len is less than + * sizeof(uint64_t), the data is interpreted as the least significant bytes + * (osmo_load64be_ext loads them as the most significant bytes into the + * returned uint64_t). In this way, any integer size up to 64 bits can be + * decoded conveniently by using sizeof(), without the need to call specific + * numbered functions (osmo_load16, 32, ...). */ +uint64_t osmo_decode_big_endian(const uint8_t *data, size_t data_len) +{ + uint64_t value = 0; + + while (data_len > 0) { + value = (value << 8) + *data; + data += 1; + data_len -= 1; + } + + return value; +} + +/*! 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 + * \returns static buffer containing big-endian stored value + * + * This is like osmo_store64be_ext, except that this returns a static buffer of + * the result (for convenience, but not threadsafe). If data_len is less than + * sizeof(uint64_t), only the least significant bytes of value are encoded. */ +uint8_t *osmo_encode_big_endian(uint64_t value, size_t data_len) +{ + static __thread uint8_t buf[sizeof(uint64_t)]; + OSMO_ASSERT(data_len <= ARRAY_SIZE(buf)); + osmo_store64be_ext(value, buf, data_len); + return buf; +} + +/*! Copy a C-string into a sized buffer + * \param[in] src source string + * \param[out] dst destination string + * \param[in] siz size of the \a dst buffer + * \returns length of \a src + * + * 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 = 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. + * \param str A nul-terminated string to validate, or NULL. + * \param min_digits least permitted amount of digits. + * \param max_digits most permitted amount of digits. + * \param require_even if true, require an even amount of digits. + * \returns true when the hex_str contains only hexadecimal digits (no + * whitespace) and matches the requested length; also true + * when min_digits <= 0 and str is NULL. + */ +bool osmo_is_hexstr(const char *str, int min_digits, int max_digits, + bool require_even) +{ + int len; + /* Use unsigned char * to avoid a compiler warning of + * "error: array subscript has type 'char' [-Werror=char-subscripts]" */ + const unsigned char *pos = (const unsigned char*)str; + if (!pos) + return min_digits < 1; + for (len = 0; *pos && len < max_digits; len++, pos++) + if (!isxdigit(*pos)) + return false; + if (len < min_digits) + return false; + /* With not too many digits, we should have reached *str == nul */ + if (*pos) + return false; + if (require_even && (len & 1)) + return false; + + return true; +} + +static const char osmo_identifier_illegal_chars[] = "., {}[]()<>|~\\^`'\"?=;/+*&%$#!"; + +/*! Determine if a given identifier is valid, i.e. doesn't contain illegal chars + * \param[in] str String to validate + * \param[in] sep_chars Permitted separation characters between identifiers. + * \returns true in case \a str contains only valid identifiers and sep_chars, false otherwise + */ +bool osmo_separated_identifiers_valid(const char *str, const char *sep_chars) +{ + /* characters that are illegal in names */ + unsigned int i; + size_t len; + + /* an empty string is not a valid identifier */ + if (!str || (len = strlen(str)) == 0) + return false; + + for (i = 0; i < len; i++) { + if (sep_chars && strchr(sep_chars, str[i])) + continue; + /* check for 7-bit ASCII */ + if (str[i] & 0x80) + return false; + if (!isprint((int)str[i])) + return false; + /* check for some explicit reserved control characters */ + if (strchr(osmo_identifier_illegal_chars, str[i])) + return false; + } + + return true; +} + +/*! Determine if a given identifier is valid, i.e. doesn't contain illegal chars + * \param[in] str String to validate + * \returns true in case \a str contains valid identifier, false otherwise + */ +bool osmo_identifier_valid(const char *str) +{ + return osmo_separated_identifiers_valid(str, NULL); +} + +/*! Replace characters in the given string buffer so that it is guaranteed to pass osmo_separated_identifiers_valid(). + * 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 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) +{ + char *pos; + if (!str) + return; + for (pos = str; *pos; pos++) { + if (strchr(osmo_identifier_illegal_chars, *pos) + || (sep_chars && strchr(sep_chars, *pos))) + *pos = replace_with; + } +} + +/*! Like osmo_escape_str_buf2, but with unusual ordering of arguments, and may sometimes return string constants instead + * of writing to buf for error cases or empty input. + * Most *_buf() functions have the buffer and size as first arguments, here the arguments are last. + * In particular, this function signature doesn't work with OSMO_STRBUF_APPEND_NOLEN(). + * \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. + * \param[inout] buf string buffer to write escaped characters to. + * \param[in] bufsize size of \a buf. + * \returns buf containing an escaped representation, possibly truncated, + * or "(null)" if str == NULL, or "(error)" in case of errors. + */ +const char *osmo_escape_str_buf(const char *str, int in_len, char *buf, size_t bufsize) +{ + if (!str) + return "(null)"; + if (!buf || !bufsize) + return "(error)"; + return osmo_escape_str_buf2(buf, bufsize, str, in_len); +} + +/*! Copy N characters to a buffer with a function signature useful for OSMO_STRBUF_APPEND(). + * Similarly to snprintf(), the result is always nul terminated (except if buf is NULL or bufsize is 0). + * \param[out] buf Target buffer. + * \param[in] bufsize sizeof(buf). + * \param[in] str String to copy. + * \param[in] n Maximum number of non-nul characters to copy. + * \return Number of characters that would be written if bufsize were large enough excluding '\0' (like snprintf()). + */ +int osmo_print_n(char *buf, size_t bufsize, const char *str, size_t n) +{ + size_t write_n; + + if (!str) + str = ""; + + n = strnlen(str, n); + + if (!buf || !bufsize) + return n; + write_n = n; + if (write_n >= bufsize) + write_n = bufsize - 1; + if (write_n) + strncpy(buf, str, write_n); + buf[write_n] = '\0'; + + return 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()). + */ +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; + int next_unprintable = 0; + + if (!str) + in_len = 0; + + if (in_len < 0) + in_len = strlen(str); + + /* Make sure of '\0' termination */ + if (!in_len) + OSMO_STRBUF_PRINTF(sb, "%s", ""); + + while (in_pos < in_len) { + for (next_unprintable = in_pos; + next_unprintable < in_len && isprint((int)str[next_unprintable]) + && str[next_unprintable] != '"' + && str[next_unprintable] != '\\'; + next_unprintable++); + + OSMO_STRBUF_APPEND(sb, osmo_print_n, &str[in_pos], next_unprintable - in_pos); + in_pos = next_unprintable; + + if (in_pos == in_len) + goto done; + + switch (str[next_unprintable]) { +#define BACKSLASH_CASE(c, repr) \ + case c: \ + OSMO_STRBUF_PRINTF(sb, "\\%c", repr); \ + break + + BACKSLASH_CASE('\n', 'n'); + BACKSLASH_CASE('\r', 'r'); + BACKSLASH_CASE('\t', 't'); + BACKSLASH_CASE('\0', '0'); + BACKSLASH_CASE('\\', '\\'); + BACKSLASH_CASE('"', '"'); + + default: + 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; +} + +/*! Return the string with all non-printable characters escaped. + * Call osmo_escape_str_buf() with a static buffer. + * \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 buf containing an escaped representation, possibly truncated, or str itself. + */ +const char *osmo_escape_str(const char *str, int in_len) +{ + return osmo_escape_str_buf(str, in_len, namebuf, sizeof(namebuf)); +} + +/*! Return the string with all non-printable characters escaped, in dynamically-allocated buffer. + * \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 output buffer, containing an escaped representation + */ +char *osmo_escape_str_c(const 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, true); +} + +/*! 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 (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()). + */ +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(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; +} + +/*! Like osmo_quote_str_buf2, but with unusual ordering of arguments, and may sometimes return string constants instead + * of writing to buf for error cases or empty input. + * Most *_buf() functions have the buffer and size as first arguments, here the arguments are last. + * In particular, this function signature doesn't work with OSMO_STRBUF_APPEND_NOLEN(). + * \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 buf containing a quoted and escaped representation, possibly truncated. + */ +const char *osmo_quote_str_buf(const char *str, int in_len, char *buf, size_t bufsize) +{ + if (!str) + return "NULL"; + if (!buf || !bufsize) + return "(error)"; + _osmo_quote_str_buf(buf, bufsize, str, in_len, true); + return buf; +} + +/*! Like osmo_quote_str_buf() but returns the result in a static 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 static buffer containing a quoted and escaped representation, possibly truncated. + */ +const char *osmo_quote_str(const char *str, int in_len) +{ + _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. + * \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) +{ + /* 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); +} + +/*! 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); +} + +/*! 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); +} + +/*! 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 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. + * This implementation is taken from "Hacker's Delight" Figure 11-1 "Integer square root, Newton's + * method", which can also be found at http://www.hackersdelight.org/hdcodetxt/isqrt.c.txt */ +uint32_t osmo_isqrt32(uint32_t x) +{ + uint32_t x1; + int s, g0, g1; + + if (x <= 1) + return x; + + s = 1; + x1 = x - 1; + if (x1 > 0xffff) { + s = s + 8; + x1 = x1 >> 16; + } + if (x1 > 0xff) { + s = s + 4; + x1 = x1 >> 8; + } + if (x1 > 0xf) { + s = s + 2; + x1 = x1 >> 4; + } + if (x1 > 0x3) { + s = s + 1; + } + + g0 = 1 << s; /* g0 = 2**s */ + g1 = (g0 + (x >> s)) >> 1; /* g1 = (g0 + x/g0)/2 */ + + /* converges after four to five divisions for arguments up to 16,785,407 */ + while (g1 < g0) { + g0 = g1; + g1 = (g0 + (x/g0)) >> 1; + } + return g0; +} + +/*! Convert a string to lowercase, while checking buffer size boundaries. + * The result written to \a dest is guaranteed to be nul terminated if \a dest_len > 0. + * If dest == src, the string is converted in-place, if necessary truncated at dest_len - 1 characters + * length as well as nul terminated. + * Note: similar osmo_str2lower(), but safe to use for src strings of arbitrary length. + * \param[out] dest Target buffer to write lowercase string. + * \param[in] dest_len Maximum buffer size of dest (e.g. sizeof(dest)). + * \param[in] src String to convert to lowercase. + * \returns Length of \a src, like osmo_strlcpy(), but if \a dest == \a src at most \a dest_len - 1. + */ +size_t osmo_str_tolower_buf(char *dest, size_t dest_len, const char *src) +{ + size_t rc; + if (dest == src) { + if (dest_len < 1) + return 0; + dest[dest_len - 1] = '\0'; + rc = strlen(dest); + } else { + if (dest_len < 1) + return strlen(src); + rc = osmo_strlcpy(dest, src, dest_len); + } + for (; *dest; dest++) + *dest = tolower(*dest); + return rc; +} + +/*! Convert a string to lowercase, using a static buffer. + * The resulting string may be truncated if the internally used static buffer is shorter than src. + * The internal buffer is at least 128 bytes long, i.e. guaranteed to hold at least 127 characters and a + * terminating nul. The static buffer returned is shared with osmo_str_toupper(). + * See also osmo_str_tolower_buf(). + * \param[in] src String to convert to lowercase. + * \returns Resulting lowercase string in a static buffer, always nul terminated. + */ +const char *osmo_str_tolower(const char *src) +{ + osmo_str_tolower_buf(capsbuf, sizeof(capsbuf), src); + return capsbuf; +} + +/*! Convert a string to lowercase, dynamically allocating the output from given talloc context + * See also osmo_str_tolower_buf(). + * \param[in] ctx talloc context from where to allocate the output string + * \param[in] src String to convert to lowercase. + * \returns Resulting lowercase string in a dynamically allocated buffer, always nul terminated. + */ +char *osmo_str_tolower_c(const void *ctx, const char *src) +{ + size_t buf_len = strlen(src) + 1; + char *buf = talloc_size(ctx, buf_len); + if (!buf) + return NULL; + osmo_str_tolower_buf(buf, buf_len, src); + return buf; +} + +/*! Convert a string to uppercase, while checking buffer size boundaries. + * The result written to \a dest is guaranteed to be nul terminated if \a dest_len > 0. + * If dest == src, the string is converted in-place, if necessary truncated at dest_len - 1 characters + * length as well as nul terminated. + * Note: similar osmo_str2upper(), but safe to use for src strings of arbitrary length. + * \param[out] dest Target buffer to write uppercase string. + * \param[in] dest_len Maximum buffer size of dest (e.g. sizeof(dest)). + * \param[in] src String to convert to uppercase. + * \returns Length of \a src, like osmo_strlcpy(), but if \a dest == \a src at most \a dest_len - 1. + */ +size_t osmo_str_toupper_buf(char *dest, size_t dest_len, const char *src) +{ + size_t rc; + if (dest == src) { + if (dest_len < 1) + return 0; + dest[dest_len - 1] = '\0'; + rc = strlen(dest); + } else { + if (dest_len < 1) + return strlen(src); + rc = osmo_strlcpy(dest, src, dest_len); + } + for (; *dest; dest++) + *dest = toupper(*dest); + return rc; +} + +/*! Convert a string to uppercase, using a static buffer. + * The resulting string may be truncated if the internally used static buffer is shorter than src. + * The internal buffer is at least 128 bytes long, i.e. guaranteed to hold at least 127 characters and a + * terminating nul. The static buffer returned is shared with osmo_str_tolower(). + * See also osmo_str_toupper_buf(). + * \param[in] src String to convert to uppercase. + * \returns Resulting uppercase string in a static buffer, always nul terminated. + */ +const char *osmo_str_toupper(const char *src) +{ + osmo_str_toupper_buf(capsbuf, sizeof(capsbuf), src); + return capsbuf; +} + +/*! Convert a string to uppercase, dynamically allocating the output from given talloc context + * See also osmo_str_tolower_buf(). + * \param[in] ctx talloc context from where to allocate the output string + * \param[in] src String to convert to uppercase. + * \returns Resulting uppercase string in a dynamically allocated buffer, always nul terminated. + */ +char *osmo_str_toupper_c(const void *ctx, const char *src) +{ + size_t buf_len = strlen(src) + 1; + char *buf = talloc_size(ctx, buf_len); + if (!buf) + return NULL; + osmo_str_toupper_buf(buf, buf_len, src); + return buf; +} + +/*! Calculate the Luhn checksum (as used for IMEIs). + * \param[in] in Input digits in ASCII string representation. + * \param[in] in_len Count of digits to use for the input (14 for IMEI). + * \returns checksum char (e.g. '3'); negative on error + */ +char osmo_luhn(const char* in, int in_len) +{ + int i, sum = 0; + + /* All input must be numbers */ + for (i = 0; i < in_len; i++) { + if (!isdigit((unsigned char)in[i])) + return -EINVAL; + } + + /* Double every second digit and add it to sum */ + for (i = in_len - 1; i >= 0; i -= 2) { + int dbl = (in[i] - '0') * 2; + if (dbl > 9) + dbl -= 9; + sum += dbl; + } + + /* Add other digits to sum */ + for (i = in_len - 2; i >= 0; i -= 2) + sum += in[i] - '0'; + + /* Final checksum */ + return (sum * 9) % 10 + '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. + * \param startswith_str (Shorter) string to compare with the start of str. + * \return true iff the first characters of str fully match startswith_str or startswith_str is empty. */ +bool osmo_str_startswith(const char *str, const char *startswith_str) +{ + if (!startswith_str || !*startswith_str) + return true; + if (!str) + return false; + 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/core/write_queue.c b/src/core/write_queue.c new file mode 100644 index 00000000..8fb73a67 --- /dev/null +++ b/src/core/write_queue.c @@ -0,0 +1,170 @@ +/* + * (C) 2010-2016 by Holger Hans Peter Freyther + * (C) 2010 by On-Waves + * + * 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 <osmocom/core/write_queue.h> +#include <osmocom/core/logging.h> + +/*! \addtogroup write_queue + * @{ + * Write queue for writing \ref msgb to sockets/fds. + * + * \file write_queue.c */ + +/*! Select loop function for write queue handling + * \param[in] fd osmocom file descriptor + * \param[in] what bit-mask of events that have happened + * \returns 0 on success; negative on error + * + * This function is provided so that it can be registered with the + * select loop abstraction code (\ref osmo_fd::cb). + */ +int osmo_wqueue_bfd_cb(struct osmo_fd *fd, unsigned int what) +{ + struct osmo_wqueue *queue; + int rc; + + queue = container_of(fd, struct osmo_wqueue, bfd); + + if (what & OSMO_FD_READ) { + rc = queue->read_cb(fd); + if (rc == -EBADF) + goto err_badfd; + } + + if (what & OSMO_FD_EXCEPT) { + rc = queue->except_cb(fd); + if (rc == -EBADF) + goto err_badfd; + } + + if (what & OSMO_FD_WRITE) { + struct msgb *msg; + + fd->when &= ~OSMO_FD_WRITE; + + msg = msgb_dequeue_count(&queue->msg_queue, &queue->current_length); + /* the queue might have been emptied */ + if (msg) { + rc = queue->write_cb(fd, msg); + 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; + } + } + +err_badfd: + /* Return value is not checked in osmo_select_main() */ + return 0; +} + +/*! Initialize a \ref osmo_wqueue structure + * \param[in] queue Write queue to operate on + * \param[in] max_length Maximum length of write queue + */ +void osmo_wqueue_init(struct osmo_wqueue *queue, int max_length) +{ + queue->max_length = max_length; + queue->current_length = 0; + queue->read_cb = NULL; + queue->write_cb = NULL; + queue->except_cb = NULL; + queue->bfd.cb = osmo_wqueue_bfd_cb; + 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 (MESSAGE NOT FREED IN CASE OF ERROR). + */ +int osmo_wqueue_enqueue(struct osmo_wqueue *queue, struct msgb *data) +{ + if (queue->current_length >= queue->max_length) { + LOGP(DLGLOBAL, LOGL_ERROR, + "wqueue(%p) is full. Rejecting msgb\n", queue); + return -ENOSPC; + } + + return osmo_wqueue_enqueue_quiet(queue, data); +} + +/*! Clear a \ref osmo_wqueue + * \param[in] queue Write queue to be cleared + * + * This function will clear (remove/release) all messages in it. + */ +void osmo_wqueue_clear(struct osmo_wqueue *queue) +{ + while (!llist_empty(&queue->msg_queue)) { + struct msgb *msg = msgb_dequeue(&queue->msg_queue); + msgb_free(msg); + } + + queue->current_length = 0; + 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; +} + +/*! @} */ |