pax_global_header00006660000000000000000000000064146053020110014502gustar00rootroot0000000000000052 comment=8f9b2bc3b60f59ecc9e6193764fa885cdef7f0ce qrtr-1.1/000077500000000000000000000000001460530201100123335ustar00rootroot00000000000000qrtr-1.1/.github/000077500000000000000000000000001460530201100136735ustar00rootroot00000000000000qrtr-1.1/.github/workflows/000077500000000000000000000000001460530201100157305ustar00rootroot00000000000000qrtr-1.1/.github/workflows/ci.yml000066400000000000000000000174021460530201100170520ustar00rootroot00000000000000# SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # # Loosely based on: https://github.com/linux-test-project/ltp # https://github.com/linux-nfc/neard # name: "Builds" on: pull_request: push: schedule: # Run at 1:01 PM, every Tuesday - cron: '1 13 * * 2' workflow_dispatch: jobs: job: name: Build runs-on: ubuntu-latest permissions: actions: read contents: read strategy: fail-fast: false matrix: arch: [x86-64] family: [x86-64] compiler: [gcc, clang] container: - archlinux:latest - debian:testing - debian:stable - debian:bookworm - debian:bullseye # Fails on configure on GCC and clang (process restrictions?) # - fedora:rawhide - fedora:latest - fedora:39 - fedora:38 - fedora:37 - ubuntu:lunar # EOL 01.2024 - ubuntu:jammy - ubuntu:focal # On Ubuntu Bionic the Meson doesn't support feature options #- ubuntu:bionic # Meson version on Ubuntu Xenial is really too old #- ubuntu:xenial cross_compile: [""] variant: [""] nosystemd: [""] include: # Alpine (OpenRC) - container: "alpine:edge" arch: x86-64 family: x86-64 compiler: gcc nosystemd: true - container: "alpine:latest" arch: x86-64 family: x86-64 compiler: gcc nosystemd: true # Debian 32-bit builds - container: "debian:testing" arch: i386 family: x86 compiler: gcc cross_compile: i686-linux-gnu pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig variant: i386 - container: "debian:stable" arch: i386 family: x86 compiler: gcc cross_compile: i686-linux-gnu pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig variant: i386 - container: "debian:bookworm" arch: i386 family: x86 compiler: gcc cross_compile: i686-linux-gnu pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig variant: i386 # Debian cross compilation builds - container: "debian:testing" arch: armhf family: arm compiler: arm-linux-gnueabihf-gcc cross_compile: arm-linux-gnueabihf pkg_config_path: /usr/lib/arm-linux-gnueabihf/pkgconfig variant: cross-compile - container: "debian:testing" arch: arm64 family: aarch64 compiler: aarch64-linux-gnu-gcc cross_compile: aarch64-linux-gnu pkg_config_path: /usr/lib/aarch64-linux-gnu/pkgconfig variant: cross-compile - container: "debian:testing" arch: ppc64el family: ppc64 compiler: powerpc64le-linux-gnu-gcc cross_compile: powerpc64le-linux-gnu pkg_config_path: /usr/lib/powerpc64le-linux-gnu/pkgconfig variant: cross-compile - container: "debian:testing" arch: s390x family: s390x compiler: s390x-linux-gnu-gcc cross_compile: s390x-linux-gnu pkg_config_path: /usr/lib/s390x-linux-gnu/pkgconfig variant: cross-compile - container: "debian:stable" arch: armhf family: arm compiler: arm-linux-gnueabihf-gcc cross_compile: arm-linux-gnueabihf pkg_config_path: /usr/lib/arm-linux-gnueabihf/pkgconfig variant: cross-compile - container: "debian:stable" arch: arm64 family: aarch64 compiler: aarch64-linux-gnu-gcc cross_compile: aarch64-linux-gnu pkg_config_path: /usr/lib/aarch64-linux-gnu/pkgconfig variant: cross-compile - container: "debian:stable" arch: ppc64el family: ppc64 compiler: powerpc64le-linux-gnu-gcc cross_compile: powerpc64le-linux-gnu pkg_config_path: /usr/lib/powerpc64le-linux-gnu/pkgconfig variant: cross-compile - container: "debian:stable" arch: s390x family: s390x compiler: s390x-linux-gnu-gcc cross_compile: s390x-linux-gnu pkg_config_path: /usr/lib/s390x-linux-gnu/pkgconfig variant: cross-compile container: image: ${{ matrix.container }} env: ARCH: ${{ matrix.arch }} FAMILY: ${{ matrix.family }} CC: ${{ matrix.compiler }} CROSS_COMPILE: ${{ matrix.cross_compile }} PKG_CONFIG_PATH: ${{ matrix.pkg_config_path }} VARIANT: ${{ matrix.variant }} steps: - name: Show OS run: cat /etc/os-release - name: Show env (matrix settings) run: | echo "ARCH: $ARCH" echo "FAMILY: $FAMILY" echo "CC: $CC" echo "CROSS_COMPILE: $CROSS_COMPILE" echo "VARIANT: $VARIANT" echo "PKG_CONFIG_PATH: $PKG_CONFIG_PATH" - name: Git checkout uses: actions/checkout@v3 - name: Install additional packages run: | INSTALL=${{ matrix.container }} INSTALL="${INSTALL%%:*}" INSTALL="${INSTALL%%/*}" ./ci/$INSTALL.sh if [ "$VARIANT" ]; then ./ci/$INSTALL.$VARIANT.sh; fi - name: Compiler version run: $CC --version - name: Display environment and Linux version run: | test -f /etc/issue && cat /etc/issue echo "############################################" lsb_release -a || true echo "############################################" cat /usr/include/linux/version.h echo "############################################" uname -a echo "############################################" cat /proc/cmdline echo "############################################" printenv # i386 build on x86_64 only requires passing -m32 to CFLAGS & LDFLAGS - name: Meson init for i386 if: ${{ matrix.variant == 'i386' }} run: | mkdir build CFLAGS="-m32" LDFLAGS="-m32" meson setup --errorlogs --werror . build - name: Meson init with cross compile if: ${{ matrix.variant == 'cross-compile' }} run: | # installing systemd (for pkg-config) has personality issues with cross-compilation # Generate cross compile file (see https://mesonbuild.com/Cross-compilation.html#cross-compilation) echo "[binaries]" > cross.txt echo "c = '${CROSS_COMPILE}-gcc'" >> cross.txt echo "strip = '${CROSS_COMPILE}-strip'" >> cross.txt # Forcing pkgconfig binary to 'pkg-config' is required for cross build to work echo "pkgconfig = 'pkg-config'" >> cross.txt echo "[host_machine]" >> cross.txt echo "system = 'linux'" >> cross.txt echo "cpu_family = '${FAMILY}'" >> cross.txt echo "cpu = '${ARCH}'" >> cross.txt echo "endian = 'little'" >> cross.txt echo "[properties]" >> cross.txt echo "pkg_config_libdir = '${PKG_CONFIG_PATH}'" >> cross.txt cat cross.txt mkdir build meson setup --errorlogs --werror --cross-file cross.txt . build - name: Meson init if: ${{ matrix.variant == '' }} run: | mkdir build meson setup --errorlogs --werror . build - name: Compile run: ninja -C build - name: Install run: ninja -C build install qrtr-1.1/.github/workflows/codeql-analysis.yml000066400000000000000000000016431460530201100215470ustar00rootroot00000000000000# SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # name: "CodeQL" on: [push, pull_request, workflow_dispatch] jobs: analyze: name: Analyze runs-on: ubuntu-latest permissions: actions: read contents: read security-events: write strategy: fail-fast: false matrix: language: [ 'cpp' ] steps: - name: Checkout repository uses: actions/checkout@v3 - name: Initialize CodeQL uses: github/codeql-action/init@v2 with: languages: ${{ matrix.language }} - name: Install additional packages run: sudo ./ci/ubuntu.sh - name: Autobuild uses: github/codeql-action/autobuild@v2 - name: Perform CodeQL Analysis uses: github/codeql-action/analyze@v2 qrtr-1.1/.gitignore000066400000000000000000000000471460530201100143240ustar00rootroot00000000000000out/ *.so qrtr-cfg qrtr-lookup qrtr-ns qrtr-1.1/Android.bp000066400000000000000000000016631460530201100142440ustar00rootroot00000000000000cc_library { name: "libqrtr", vendor: true, srcs: [ "lib/logging.c", "lib/qrtr.c", "lib/qmi.c", ], cflags: ["-fPIC", "-Wno-error"], export_include_dirs: ["lib"], local_include_dirs: ["src"], } cc_binary { name: "qrtr-ns", vendor: true, srcs: [ "lib/logging.c", "src/addr.c", "src/ns.c", "src/map.c", "src/hash.c", "src/waiter.c", "src/util.c", ], cflags: ["-Wno-error"], local_include_dirs: ["lib"], } cc_binary { name: "qrtr-cfg", vendor: true, srcs: [ "lib/logging.c", "src/addr.c", "src/cfg.c", ], cflags: ["-Wno-error"], local_include_dirs: ["lib"], } cc_binary { name: "qrtr-lookup", vendor: true, srcs: [ "lib/logging.c", "src/lookup.c", "src/util.c", ], cflags: ["-Wno-error"], local_include_dirs: ["lib"], } qrtr-1.1/LICENSE000066400000000000000000000027761460530201100133540ustar00rootroot00000000000000Copyright (c) 2015, Sony Mobile Communications Inc. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. qrtr-1.1/ci/000077500000000000000000000000001460530201100127265ustar00rootroot00000000000000qrtr-1.1/ci/alpine.sh000077500000000000000000000006201460530201100145330ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex PKGS_CC="gcc" case $CC in clang*) PKGS_CC="clang" ;; esac apk add \ linux-headers \ meson \ musl-dev \ libc-dev \ $PKGS_CC echo "Install finished: $0" qrtr-1.1/ci/archlinux.sh000077500000000000000000000006221460530201100152620ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex PKGS_CC="gcc" case $CC in clang*) PKGS_CC="clang" ;; esac pacman -Syu --noconfirm \ systemd-libs \ pkgconf \ meson \ $PKGS_CC echo "Install finished: $0" qrtr-1.1/ci/debian.cross-compile.sh000077500000000000000000000012031460530201100172610ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2018-2020 Petr Vorel # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex if [ -z "$ARCH" ]; then echo "missing \$ARCH!" >&2 exit 1 fi dpkg --add-architecture $ARCH apt update apt install -y --no-install-recommends \ libc6-dev:${ARCH} \ gcc-`dpkg-architecture -a ${ARCH} -q DEB_TARGET_GNU_TYPE` # Thanks debian apt install -y --no-install-recommends systemd-dev:${ARCH} -a ${ARCH} || true echo "Install finished: $0" qrtr-1.1/ci/debian.i386.sh000077500000000000000000000010531460530201100151760ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2018-2020 Petr Vorel # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex dpkg --add-architecture i386 apt update # gcc-multilib are also needed for clang 32-bit builds PKGS_CC="gcc-multilib" apt install -y --no-install-recommends \ linux-libc-dev:i386 apt install -y --no-install-recommends \ $PKGS_CC echo "Install finished: $0" qrtr-1.1/ci/debian.sh000077500000000000000000000015461460530201100145150ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex apt update # Some distros (e.g. Ubuntu Hirsute) might pull tzdata which asks questions export DEBIAN_FRONTEND=noninteractive DEBCONF_NONINTERACTIVE_SEEN=true # Choose some random place in Europe echo "tzdata tzdata/Areas select Europe tzdata tzdata/Zones/Europe select Berlin " > /tmp/tzdata-preseed.txt debconf-set-selections /tmp/tzdata-preseed.txt PKGS_CC="build-essential" case $CC in clang*) PKGS_CC="clang" ;; esac apt install -y --no-install-recommends \ pkg-config \ meson \ systemd \ libc6-dev \ $PKGS_CC # Thanks debian apt install -y --no-install-recommends systemd-dev || true echo "Install finished: $0" qrtr-1.1/ci/fedora.sh000077500000000000000000000006561460530201100145340ustar00rootroot00000000000000#!/bin/sh # SPDX-License-Identifier: GPL-2.0 # # Copyright (c) 2021 Canonical Ltd. # Copyright (c) 2023 Linaro Ltd # Author: Krzysztof Kozlowski # # set -ex PKGS_CC="gcc" case $CC in clang*) PKGS_CC="clang" ;; esac dnf -y install \ meson \ pkg-config \ libudev-devel \ systemd-devel \ systemd-libs \ $PKGS_CC echo "Install finished: $0" qrtr-1.1/ci/ubuntu.cross-compile.sh000077700000000000000000000000001460530201100237112debian.cross-compile.shustar00rootroot00000000000000qrtr-1.1/ci/ubuntu.sh000077700000000000000000000000001460530201100163552debian.shustar00rootroot00000000000000qrtr-1.1/include/000077500000000000000000000000001460530201100137565ustar00rootroot00000000000000qrtr-1.1/include/libqrtr.h000066400000000000000000000123141460530201100156070ustar00rootroot00000000000000#ifndef _QRTR_LIB_H_ #define _QRTR_LIB_H_ #include #include #include #include #include #ifdef __cplusplus extern "C" { #endif #ifndef offsetof #define offsetof(type, md) ((size_t)&((type *)0)->md) #endif #ifndef container_of #define container_of(ptr, type, member) \ ((type *)((char *)(ptr) - offsetof(type, member))) #endif #ifndef AF_QIPCRTR #define AF_QIPCRTR 42 #endif struct sockaddr_qrtr; struct qrtr_packet { int type; unsigned int node; unsigned int port; unsigned int service; unsigned int instance; unsigned int version; void *data; size_t data_len; }; #define DEFINE_QRTR_PACKET(pkt, size) \ char pkt ## _buf[size]; \ struct qrtr_packet pkt = { .data = pkt ##_buf, \ .data_len = sizeof(pkt ##_buf), } #define QMI_REQUEST 0 #define QMI_RESPONSE 2 #define QMI_INDICATION 4 #define QMI_COMMON_TLV_TYPE 0 enum qmi_elem_type { QMI_EOTI, QMI_OPT_FLAG, QMI_DATA_LEN, QMI_UNSIGNED_1_BYTE, QMI_UNSIGNED_2_BYTE, QMI_UNSIGNED_4_BYTE, QMI_UNSIGNED_8_BYTE, QMI_SIGNED_1_BYTE_ENUM, QMI_SIGNED_2_BYTE_ENUM, QMI_SIGNED_4_BYTE_ENUM, QMI_STRUCT, QMI_STRING, }; enum qmi_array_type { NO_ARRAY, STATIC_ARRAY, VAR_LEN_ARRAY, }; /** * struct qmi_elem_info - describes how to encode a single QMI element * @data_type: Data type of this element. * @elem_len: Array length of this element, if an array. * @elem_size: Size of a single instance of this data type. * @array_type: Array type of this element. * @tlv_type: QMI message specific type to identify which element * is present in an incoming message. * @offset: Specifies the offset of the first instance of this * element in the data structure. * @ei_array: Null-terminated array of @qmi_elem_info to describe nested * structures. */ struct qmi_elem_info { enum qmi_elem_type data_type; uint32_t elem_len; uint32_t elem_size; enum qmi_array_type array_type; uint8_t tlv_type; size_t offset; struct qmi_elem_info *ei_array; }; #define QMI_RESULT_SUCCESS_V01 0 #define QMI_RESULT_FAILURE_V01 1 #define QMI_ERR_NONE_V01 0 #define QMI_ERR_MALFORMED_MSG_V01 1 #define QMI_ERR_NO_MEMORY_V01 2 #define QMI_ERR_INTERNAL_V01 3 #define QMI_ERR_CLIENT_IDS_EXHAUSTED_V01 5 #define QMI_ERR_INVALID_ID_V01 41 #define QMI_ERR_ENCODING_V01 58 #define QMI_ERR_INCOMPATIBLE_STATE_V01 90 #define QMI_ERR_NOT_SUPPORTED_V01 94 /** * qmi_response_type_v01 - common response header (decoded) * @result: result of the transaction * @error: error value, when @result is QMI_RESULT_FAILURE_V01 */ struct qmi_response_type_v01 { uint16_t result; uint16_t error; }; extern struct qmi_elem_info qmi_response_type_v01_ei[]; int qrtr_open(int rport); void qrtr_close(int sock); int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz); int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port); int qrtr_recv(int sock, void *buf, unsigned int bsz); int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance); int qrtr_poll(int sock, unsigned int ms); int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len, const struct sockaddr_qrtr *sq); int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id); int qmi_decode_message(void *c_struct, unsigned int *txn, const struct qrtr_packet *pkt, int type, int id, struct qmi_elem_info *ei); ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id, int txn_id, const void *c_struct, struct qmi_elem_info *ei); /* Initial kernel header didn't expose these */ #ifndef QRTR_NODE_BCAST #define QRTR_NODE_BCAST 0xffffffffu #define QRTR_PORT_CTRL 0xfffffffeu enum qrtr_pkt_type { QRTR_TYPE_DATA = 1, QRTR_TYPE_HELLO = 2, QRTR_TYPE_BYE = 3, QRTR_TYPE_NEW_SERVER = 4, QRTR_TYPE_DEL_SERVER = 5, QRTR_TYPE_DEL_CLIENT = 6, QRTR_TYPE_RESUME_TX = 7, QRTR_TYPE_EXIT = 8, QRTR_TYPE_PING = 9, QRTR_TYPE_NEW_LOOKUP = 10, QRTR_TYPE_DEL_LOOKUP = 11, }; struct qrtr_ctrl_pkt { __le32 cmd; union { struct { __le32 service; __le32 instance; __le32 node; __le32 port; } server; struct { __le32 node; __le32 port; } client; }; } __attribute__((packed)); #endif #ifdef __cplusplus } /* extern "C" */ #endif #endif qrtr-1.1/include/logging.h000066400000000000000000000020251460530201100155540ustar00rootroot00000000000000#ifndef _QRTR_LOGGING_H_ #define _QRTR_LOGGING_H_ #include #include #include #if defined(__GNUC__) || defined(__clang__) #define __PRINTF__(fmt, args) __attribute__((format(__printf__, fmt, args))) #else #define __PRINTF__(fmt, args) #endif void qlog_setup(const char *tag, bool use_syslog); void qlog_set_min_priority(int priority); void qlog(int priority, const char *format, ...) __PRINTF__(2, 3); #define LOGD(fmt, ...) qlog(LOG_DEBUG, fmt, ##__VA_ARGS__) #define LOGW(fmt, ...) qlog(LOG_WARNING, fmt, ##__VA_ARGS__) #define PLOGW(fmt, ...) \ qlog(LOG_WARNING, fmt ": %s", ##__VA_ARGS__, strerror(errno)) #define LOGE(fmt, ...) qlog(LOG_ERR, fmt, ##__VA_ARGS__) #define PLOGE(fmt, ...) qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno)) #define LOGE_AND_EXIT(fmt, ...) do { \ qlog(LOG_ERR, fmt, ##__VA_ARGS__); \ exit(1); \ } while(0) #define PLOGE_AND_EXIT(fmt, ...) do { \ qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno)); \ exit(1); \ } while(0) #endif qrtr-1.1/include/meson.build000066400000000000000000000001061460530201100161150ustar00rootroot00000000000000# SPDX-License-Identifier: BSD-3-Clause install_headers('libqrtr.h') qrtr-1.1/include/ns.h000066400000000000000000000003321460530201100145450ustar00rootroot00000000000000#ifndef __NS_H_ #define __NS_H_ #include #include static inline __le32 cpu_to_le32(uint32_t x) { return htole32(x); } static inline uint32_t le32_to_cpu(__le32 x) { return le32toh(x); } #endif qrtr-1.1/lib/000077500000000000000000000000001460530201100131015ustar00rootroot00000000000000qrtr-1.1/lib/logging.c000066400000000000000000000024371460530201100147010ustar00rootroot00000000000000#include #include #include #include #define QLOG_BUF_SIZE 512 static const char default_tag[] = "libqrtr"; static const char *current_tag = default_tag; static int min_priority = LOG_INFO; static bool logging_to_syslog = false; void qlog_setup(const char *tag, bool use_syslog) { current_tag = tag; logging_to_syslog = use_syslog; openlog(tag, LOG_PID, LOG_USER); } void qlog_set_min_priority(int priority) { if (priority < LOG_EMERG || priority > LOG_DEBUG) return; min_priority = priority; } static const char *get_priority_string(int priority) { switch (priority) { case LOG_EMERG: return "EMERG"; case LOG_ALERT: return "ALERT"; case LOG_CRIT: return "CRIT"; case LOG_ERR: return "ERROR"; case LOG_WARNING: return "WARNING"; case LOG_NOTICE: return "NOTICE"; case LOG_INFO: return "INFO"; case LOG_DEBUG: return "DEBUG"; } return ""; } void qlog(int priority, const char *format, ...) { va_list ap; if (priority > min_priority) return; va_start(ap, format); if (logging_to_syslog) { vsyslog(priority, format, ap); } else { char buf[QLOG_BUF_SIZE]; vsnprintf(buf, QLOG_BUF_SIZE, format, ap); fprintf(stderr, "%s %s: %s\n", get_priority_string(priority), current_tag, buf); } va_end(ap); } qrtr-1.1/lib/meson.build000066400000000000000000000004571460530201100152510ustar00rootroot00000000000000# SPDX-License-Identifier: BSD-3-Clause pkg = import('pkgconfig') libqrtr_srcs = ['logging.c', 'qmi.c', 'qrtr.c'] libqrtr = shared_library('qrtr', libqrtr_srcs, include_directories : inc, install: true) pkg.generate(libqrtr) qrtr-1.1/lib/qmi.c000066400000000000000000000646441460530201100140510ustar00rootroot00000000000000/* Copyright (c) 2017, The Linux Foundation. All rights reserved. * Copyright (C) 2017-2018 Linaro Ltd. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are * met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of The Linux Foundation nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include "libqrtr.h" #include "logging.h" /** * qmi_header - wireformat header of QMI messages * @type: type of message * @txn_id: transaction id * @msg_id: message id * @msg_len: length of message payload following header */ struct qmi_header { uint8_t type; uint16_t txn_id; uint16_t msg_id; uint16_t msg_len; } __attribute__((packed)); #define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \ *p_dst++ = type; \ *p_dst++ = ((uint8_t)((length) & 0xFF)); \ *p_dst++ = ((uint8_t)(((length) >> 8) & 0xFF)); \ } while (0) #define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \ *p_type = (uint8_t)*p_src++; \ *p_length = (uint8_t)*p_src++; \ *p_length |= ((uint8_t)*p_src) << 8; \ } while (0) #define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \ do { \ memcpy(p_dst, p_src, size); \ p_dst = (uint8_t *)p_dst + size; \ p_src = (uint8_t *)p_src + size; \ } while (0) #define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \ do { \ memcpy(p_dst, p_src, size); \ p_dst = (uint8_t *)p_dst + size; \ p_src = (uint8_t *)p_src + size; \ } while (0) #define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \ encoded_bytes, tlv_len, encode_tlv, rc) \ do { \ buf_dst = (uint8_t *)buf_dst + rc; \ encoded_bytes += rc; \ tlv_len += rc; \ temp_si = temp_si + 1; \ encode_tlv = 1; \ } while (0) #define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \ do { \ buf_src = (uint8_t *)buf_src + rc; \ decoded_bytes += rc; \ } while (0) #define TLV_LEN_SIZE sizeof(uint16_t) #define TLV_TYPE_SIZE sizeof(uint8_t) #define OPTIONAL_TLV_TYPE_START 0x10 static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf, const void *in_c_struct, uint32_t out_buf_len, int enc_level); static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct, const void *in_buf, uint32_t in_buf_len, int dec_level); /** * skip_to_next_elem() - Skip to next element in the structure to be encoded * @ei_array: Struct info describing the element to be skipped. * @level: Depth level of encoding/decoding to identify nested structures. * * This function is used while encoding optional elements. If the flag * corresponding to an optional element is not set, then encoding the * optional element can be skipped. This function can be used to perform * that operation. * * Return: struct info of the next element that can be encoded. */ static struct qmi_elem_info *skip_to_next_elem(struct qmi_elem_info *ei_array, int level) { struct qmi_elem_info *temp_ei = ei_array; uint8_t tlv_type; if (level > 1) { temp_ei = temp_ei + 1; } else { do { tlv_type = temp_ei->tlv_type; temp_ei = temp_ei + 1; } while (tlv_type == temp_ei->tlv_type); } return temp_ei; } /** * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message * @ei_array: Struct info array describing the structure. * @level: Level to identify the depth of the nested structures. * * Return: Expected minimum length of the QMI message or 0 on error. */ static int qmi_calc_min_msg_len(struct qmi_elem_info *ei_array, int level) { int min_msg_len = 0; struct qmi_elem_info *temp_ei = ei_array; if (!ei_array) return min_msg_len; while (temp_ei->data_type != QMI_EOTI) { /* Optional elements do not count in minimum length */ if (temp_ei->data_type == QMI_OPT_FLAG) { temp_ei = skip_to_next_elem(temp_ei, level); continue; } if (temp_ei->data_type == QMI_DATA_LEN) { min_msg_len += (temp_ei->elem_size == sizeof(uint8_t) ? sizeof(uint8_t) : sizeof(uint16_t)); temp_ei++; continue; } else if (temp_ei->data_type == QMI_STRUCT) { min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array, (level + 1)); temp_ei++; } else if (temp_ei->data_type == QMI_STRING) { if (level > 1) min_msg_len += temp_ei->elem_len <= 256 ? sizeof(uint8_t) : sizeof(uint16_t); min_msg_len += temp_ei->elem_len * temp_ei->elem_size; temp_ei++; } else { min_msg_len += (temp_ei->elem_len * temp_ei->elem_size); temp_ei++; } /* * Type & Length info. not prepended for elements in the * nested structure. */ if (level == 1) min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE); } return min_msg_len; } /** * qmi_encode_basic_elem() - Encodes elements of basic/primary data type * @buf_dst: Buffer to store the encoded information. * @buf_src: Buffer containing the elements to be encoded. * @elem_len: Number of elements, in the buf_src, to be encoded. * @elem_size: Size of a single instance of the element to be encoded. * * This function encodes the "elem_len" number of data elements, each of * size "elem_size" bytes from the source buffer "buf_src" and stores the * encoded information in the destination buffer "buf_dst". The elements are * of primary data type which include uint8_t - u64 or similar. This * function returns the number of bytes of encoded information. * * Return: The number of bytes of encoded information. */ static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src, uint32_t elem_len, uint32_t elem_size) { uint32_t i, rc = 0; for (i = 0; i < elem_len; i++) { QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size); rc += elem_size; } return rc; } /** * qmi_encode_struct_elem() - Encodes elements of struct data type * @ei_array: Struct info array descibing the struct element. * @buf_dst: Buffer to store the encoded information. * @buf_src: Buffer containing the elements to be encoded. * @elem_len: Number of elements, in the buf_src, to be encoded. * @out_buf_len: Available space in the encode buffer. * @enc_level: Depth of the nested structure from the main structure. * * This function encodes the "elem_len" number of struct elements, each of * size "ei_array->elem_size" bytes from the source buffer "buf_src" and * stores the encoded information in the destination buffer "buf_dst". The * elements are of struct data type which includes any C structure. This * function returns the number of bytes of encoded information. * * Return: The number of bytes of encoded information on success or negative * errno on error. */ static int qmi_encode_struct_elem(struct qmi_elem_info *ei_array, void *buf_dst, const void *buf_src, uint32_t elem_len, uint32_t out_buf_len, int enc_level) { int i, rc, encoded_bytes = 0; struct qmi_elem_info *temp_ei = ei_array; for (i = 0; i < elem_len; i++) { rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src, out_buf_len - encoded_bytes, enc_level); if (rc < 0) { LOGW("%s: STRUCT Encode failure\n", __func__); return rc; } buf_dst = (void*)((char*)buf_dst + rc); buf_src = (void*)((char*)buf_src + temp_ei->elem_size); encoded_bytes += rc; } return encoded_bytes; } /** * qmi_encode_string_elem() - Encodes elements of string data type * @ei_array: Struct info array descibing the string element. * @buf_dst: Buffer to store the encoded information. * @buf_src: Buffer containing the elements to be encoded. * @out_buf_len: Available space in the encode buffer. * @enc_level: Depth of the string element from the main structure. * * This function encodes a string element of maximum length "ei_array->elem_len" * bytes from the source buffer "buf_src" and stores the encoded information in * the destination buffer "buf_dst". This function returns the number of bytes * of encoded information. * * Return: The number of bytes of encoded information on success or negative * errno on error. */ static int qmi_encode_string_elem(struct qmi_elem_info *ei_array, void *buf_dst, const void *buf_src, uint32_t out_buf_len, int enc_level) { int rc; int encoded_bytes = 0; struct qmi_elem_info *temp_ei = ei_array; uint32_t string_len = 0; uint32_t string_len_sz = 0; string_len = strlen(buf_src); string_len_sz = temp_ei->elem_len <= 256 ? sizeof(uint8_t) : sizeof(uint16_t); if (string_len > temp_ei->elem_len) { LOGW("%s: String to be encoded is longer - %u > %u\n", __func__, string_len, temp_ei->elem_len); return -EINVAL; } if (enc_level == 1) { if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE > out_buf_len) { LOGW("%s: Output len %u > Out Buf len %u\n", __func__, string_len, out_buf_len); return -EINVAL; } } else { if (string_len + string_len_sz > out_buf_len) { LOGW("%s: Output len %u > Out Buf len %u\n", __func__, string_len, out_buf_len); return -EINVAL; } rc = qmi_encode_basic_elem(buf_dst, &string_len, 1, string_len_sz); encoded_bytes += rc; } rc = qmi_encode_basic_elem((void*)((char*)buf_dst + encoded_bytes), buf_src, string_len, temp_ei->elem_size); encoded_bytes += rc; return encoded_bytes; } /** * qmi_encode() - Core Encode Function * @ei_array: Struct info array describing the structure to be encoded. * @out_buf: Buffer to hold the encoded QMI message. * @in_c_struct: Pointer to the C structure to be encoded. * @out_buf_len: Available space in the encode buffer. * @enc_level: Encode level to indicate the depth of the nested structure, * within the main structure, being encoded. * * Return: The number of bytes of encoded information on success or negative * errno on error. */ static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf, const void *in_c_struct, uint32_t out_buf_len, int enc_level) { struct qmi_elem_info *temp_ei = ei_array; uint8_t opt_flag_value = 0; uint32_t data_len_value = 0, data_len_sz; uint8_t *buf_dst = (uint8_t *)out_buf; uint8_t *tlv_pointer; uint32_t tlv_len; uint8_t tlv_type; uint32_t encoded_bytes = 0; const void *buf_src; int encode_tlv = 0; int rc; if (!ei_array) return 0; tlv_pointer = buf_dst; tlv_len = 0; if (enc_level == 1) buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE); while (temp_ei->data_type != QMI_EOTI) { buf_src = (void*)((char*)in_c_struct + temp_ei->offset); tlv_type = temp_ei->tlv_type; if (temp_ei->array_type == NO_ARRAY) { data_len_value = 1; } else if (temp_ei->array_type == STATIC_ARRAY) { data_len_value = temp_ei->elem_len; } else if (data_len_value <= 0 || temp_ei->elem_len < data_len_value) { LOGW("%s: Invalid data length\n", __func__); return -EINVAL; } switch (temp_ei->data_type) { case QMI_OPT_FLAG: rc = qmi_encode_basic_elem(&opt_flag_value, buf_src, 1, sizeof(uint8_t)); if (opt_flag_value) temp_ei = temp_ei + 1; else temp_ei = skip_to_next_elem(temp_ei, enc_level); break; case QMI_DATA_LEN: memcpy(&data_len_value, buf_src, temp_ei->elem_size); data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ? sizeof(uint8_t) : sizeof(uint16_t); /* Check to avoid out of range buffer access */ if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) > out_buf_len) { LOGW("%s: Too Small Buffer @DATA_LEN\n", __func__); return -EINVAL; } rc = qmi_encode_basic_elem(buf_dst, &data_len_value, 1, data_len_sz); UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, encoded_bytes, tlv_len, encode_tlv, rc); if (!data_len_value) temp_ei = skip_to_next_elem(temp_ei, enc_level); else encode_tlv = 0; break; case QMI_UNSIGNED_1_BYTE: case QMI_UNSIGNED_2_BYTE: case QMI_UNSIGNED_4_BYTE: case QMI_UNSIGNED_8_BYTE: case QMI_SIGNED_1_BYTE_ENUM: case QMI_SIGNED_2_BYTE_ENUM: case QMI_SIGNED_4_BYTE_ENUM: /* Check to avoid out of range buffer access */ if (((data_len_value * temp_ei->elem_size) + encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) > out_buf_len) { LOGW("%s: Too Small Buffer @data_type:%u\n", __func__, temp_ei->data_type); return -EINVAL; } rc = qmi_encode_basic_elem(buf_dst, buf_src, data_len_value, temp_ei->elem_size); UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, encoded_bytes, tlv_len, encode_tlv, rc); break; case QMI_STRUCT: rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src, data_len_value, out_buf_len - encoded_bytes, enc_level + 1); if (rc < 0) return rc; UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, encoded_bytes, tlv_len, encode_tlv, rc); break; case QMI_STRING: rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src, out_buf_len - encoded_bytes, enc_level); if (rc < 0) return rc; UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, encoded_bytes, tlv_len, encode_tlv, rc); break; default: LOGW("%s: Unrecognized data type\n", __func__); return -EINVAL; } if (encode_tlv && enc_level == 1) { QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer); encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE); tlv_pointer = buf_dst; tlv_len = 0; buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE; encode_tlv = 0; } } return encoded_bytes; } /** * qmi_decode_basic_elem() - Decodes elements of basic/primary data type * @buf_dst: Buffer to store the decoded element. * @buf_src: Buffer containing the elements in QMI wire format. * @elem_len: Number of elements to be decoded. * @elem_size: Size of a single instance of the element to be decoded. * * This function decodes the "elem_len" number of elements in QMI wire format, * each of size "elem_size" bytes from the source buffer "buf_src" and stores * the decoded elements in the destination buffer "buf_dst". The elements are * of primary data type which include uint8_t - u64 or similar. This * function returns the number of bytes of decoded information. * * Return: The total size of the decoded data elements, in bytes. */ static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src, uint32_t elem_len, uint32_t elem_size) { uint32_t i, rc = 0; for (i = 0; i < elem_len; i++) { QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size); rc += elem_size; } return rc; } /** * qmi_decode_struct_elem() - Decodes elements of struct data type * @ei_array: Struct info array descibing the struct element. * @buf_dst: Buffer to store the decoded element. * @buf_src: Buffer containing the elements in QMI wire format. * @elem_len: Number of elements to be decoded. * @tlv_len: Total size of the encoded inforation corresponding to * this struct element. * @dec_level: Depth of the nested structure from the main structure. * * This function decodes the "elem_len" number of elements in QMI wire format, * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src" * and stores the decoded elements in the destination buffer "buf_dst". The * elements are of struct data type which includes any C structure. This * function returns the number of bytes of decoded information. * * Return: The total size of the decoded data elements on success, negative * errno on error. */ static int qmi_decode_struct_elem(struct qmi_elem_info *ei_array, void *buf_dst, const void *buf_src, uint32_t elem_len, uint32_t tlv_len, int dec_level) { int i, rc, decoded_bytes = 0; struct qmi_elem_info *temp_ei = ei_array; for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) { rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src, tlv_len - decoded_bytes, dec_level); if (rc < 0) return rc; buf_src = (void*)((char*)buf_src + rc); buf_dst = (void*)((char*)buf_dst + temp_ei->elem_size); decoded_bytes += rc; } if ((dec_level <= 2 && decoded_bytes != tlv_len) || (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) { LOGW("%s: Fault in decoding: dl(%d), db(%d), tl(%u), i(%d), el(%u)\n", __func__, dec_level, decoded_bytes, tlv_len, i, elem_len); return -EFAULT; } return decoded_bytes; } /** * qmi_decode_string_elem() - Decodes elements of string data type * @ei_array: Struct info array descibing the string element. * @buf_dst: Buffer to store the decoded element. * @buf_src: Buffer containing the elements in QMI wire format. * @tlv_len: Total size of the encoded inforation corresponding to * this string element. * @dec_level: Depth of the string element from the main structure. * * This function decodes the string element of maximum length * "ei_array->elem_len" from the source buffer "buf_src" and puts it into * the destination buffer "buf_dst". This function returns number of bytes * decoded from the input buffer. * * Return: The total size of the decoded data elements on success, negative * errno on error. */ static int qmi_decode_string_elem(struct qmi_elem_info *ei_array, void *buf_dst, const void *buf_src, uint32_t tlv_len, int dec_level) { int rc; int decoded_bytes = 0; uint32_t string_len = 0; uint32_t string_len_sz = 0; struct qmi_elem_info *temp_ei = ei_array; if (dec_level == 1) { string_len = tlv_len; } else { string_len_sz = temp_ei->elem_len <= 256 ? sizeof(uint8_t) : sizeof(uint16_t); rc = qmi_decode_basic_elem(&string_len, buf_src, 1, string_len_sz); decoded_bytes += rc; } if (string_len > temp_ei->elem_len) { LOGW("%s: String len %u > Max Len %u\n", __func__, string_len, temp_ei->elem_len); return -EINVAL; } else if (string_len > tlv_len) { LOGW("%s: String len %u > Input Buffer Len %u\n", __func__, string_len, tlv_len); return -EFAULT; } rc = qmi_decode_basic_elem(buf_dst, (void*)((char*)buf_src + decoded_bytes), string_len, temp_ei->elem_size); *((char *)buf_dst + string_len) = '\0'; decoded_bytes += rc; return decoded_bytes; } /** * find_ei() - Find element info corresponding to TLV Type * @ei_array: Struct info array of the message being decoded. * @type: TLV Type of the element being searched. * * Every element that got encoded in the QMI message will have a type * information associated with it. While decoding the QMI message, * this function is used to find the struct info regarding the element * that corresponds to the type being decoded. * * Return: Pointer to struct info, if found */ static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array, uint32_t type) { struct qmi_elem_info *temp_ei = ei_array; while (temp_ei->data_type != QMI_EOTI) { if (temp_ei->tlv_type == (uint8_t)type) return temp_ei; temp_ei = temp_ei + 1; } return NULL; } /** * qmi_decode() - Core Decode Function * @ei_array: Struct info array describing the structure to be decoded. * @out_c_struct: Buffer to hold the decoded C struct * @in_buf: Buffer containing the QMI message to be decoded * @in_buf_len: Length of the QMI message to be decoded * @dec_level: Decode level to indicate the depth of the nested structure, * within the main structure, being decoded * * Return: The number of bytes of decoded information on success, negative * errno on error. */ static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct, const void *in_buf, uint32_t in_buf_len, int dec_level) { struct qmi_elem_info *temp_ei = ei_array; uint8_t opt_flag_value = 1; uint32_t data_len_value = 0, data_len_sz = 0; uint8_t *buf_dst = out_c_struct; const uint8_t *tlv_pointer; uint32_t tlv_len = 0; uint32_t tlv_type; uint32_t decoded_bytes = 0; const void *buf_src = in_buf; int rc; while (decoded_bytes < in_buf_len) { if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI) return decoded_bytes; if (dec_level == 1) { tlv_pointer = buf_src; QMI_ENCDEC_DECODE_TLV(&tlv_type, &tlv_len, tlv_pointer); buf_src = (void*)((char*)buf_src + (TLV_TYPE_SIZE + TLV_LEN_SIZE)); decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE); temp_ei = find_ei(ei_array, tlv_type); if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) { LOGW("%s: Inval element info\n", __func__); return -EINVAL; } else if (!temp_ei) { UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, tlv_len); continue; } } else { /* * No length information for elements in nested * structures. So use remaining decodable buffer space. */ tlv_len = in_buf_len - decoded_bytes; } buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); if (temp_ei->data_type == QMI_OPT_FLAG) { memcpy(buf_dst, &opt_flag_value, sizeof(uint8_t)); temp_ei = temp_ei + 1; buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); } if (temp_ei->data_type == QMI_DATA_LEN) { data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ? sizeof(uint8_t) : sizeof(uint16_t); rc = qmi_decode_basic_elem(&data_len_value, buf_src, 1, data_len_sz); memcpy(buf_dst, &data_len_value, sizeof(uint32_t)); temp_ei = temp_ei + 1; buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); tlv_len -= data_len_sz; UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); } if (temp_ei->array_type == NO_ARRAY) { data_len_value = 1; } else if (temp_ei->array_type == STATIC_ARRAY) { data_len_value = temp_ei->elem_len; } else if (data_len_value > temp_ei->elem_len) { LOGW("%s: Data len %u > max spec %u\n", __func__, data_len_value, temp_ei->elem_len); return -EINVAL; } switch (temp_ei->data_type) { case QMI_UNSIGNED_1_BYTE: case QMI_UNSIGNED_2_BYTE: case QMI_UNSIGNED_4_BYTE: case QMI_UNSIGNED_8_BYTE: case QMI_SIGNED_1_BYTE_ENUM: case QMI_SIGNED_2_BYTE_ENUM: case QMI_SIGNED_4_BYTE_ENUM: rc = qmi_decode_basic_elem(buf_dst, buf_src, data_len_value, temp_ei->elem_size); UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); break; case QMI_STRUCT: rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src, data_len_value, tlv_len, dec_level + 1); if (rc < 0) return rc; UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); break; case QMI_STRING: rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src, tlv_len, dec_level); if (rc < 0) return rc; UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); break; default: LOGW("%s: Unrecognized data type\n", __func__); return -EINVAL; } temp_ei = temp_ei + 1; } return decoded_bytes; } /** * qmi_encode_message() - Encode C structure as QMI encoded message * @type: Type of QMI message * @msg_id: Message ID of the message * @len: Passed as max length of the message, updated to actual size * @txn_id: Transaction ID * @ei: QMI message descriptor * @c_struct: Reference to structure to encode * * Return: Buffer with encoded message, or negative ERR_PTR() on error */ ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id, int txn_id, const void *c_struct, struct qmi_elem_info *ei) { struct qmi_header *hdr = pkt->data; ssize_t msglen = 0; int ret; /* Check the possibility of a zero length QMI message */ if (!c_struct) { ret = qmi_calc_min_msg_len(ei, 1); if (ret) { LOGW("%s: Calc. len %d != 0, but NULL c_struct\n", __func__, ret); return -EINVAL; } } if (pkt->data_len < sizeof(*hdr)) return -EMSGSIZE; /* Encode message, if we have a message */ if (c_struct) { msglen = qmi_encode(ei, (void*)((char*)pkt->data + sizeof(*hdr)), c_struct, pkt->data_len - sizeof(*hdr), 1); if (msglen < 0) return msglen; } hdr->type = type; hdr->txn_id = txn_id; hdr->msg_id = msg_id; hdr->msg_len = msglen; pkt->type = QRTR_TYPE_DATA; pkt->data_len = sizeof(*hdr) + msglen; return pkt->data_len; } int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id) { const struct qmi_header *qmi = pkt->data; if (qmi->msg_len != pkt->data_len - sizeof(*qmi)) { LOGW("[RMTFS] Invalid length of incoming qmi request\n"); return -EINVAL; } *msg_id = qmi->msg_id; return 0; } /** * qmi_decode_message() - Decode QMI encoded message to C structure * @buf: Buffer with encoded message * @len: Amount of data in @buf * @ei: QMI message descriptor * @c_struct: Reference to structure to decode into * * Return: The number of bytes of decoded information on success, negative * errno on error. */ int qmi_decode_message(void *c_struct, unsigned int *txn, const struct qrtr_packet *pkt, int type, int id, struct qmi_elem_info *ei) { const struct qmi_header *hdr = pkt->data; if (!ei) return -EINVAL; if (!c_struct || !pkt->data || !pkt->data_len) return -EINVAL; if (hdr->type != type) return -EINVAL; if (hdr->msg_id != id) return -EINVAL; if (txn) *txn = hdr->txn_id; return qmi_decode(ei, c_struct, (void*)((char*)pkt->data + sizeof(*hdr)), pkt->data_len - sizeof(*hdr), 1); } /* Common header in all QMI responses */ struct qmi_elem_info qmi_response_type_v01_ei[] = { { .data_type = QMI_SIGNED_2_BYTE_ENUM, .elem_len = 1, .elem_size = sizeof(uint16_t), .array_type = NO_ARRAY, .tlv_type = QMI_COMMON_TLV_TYPE, .offset = offsetof(struct qmi_response_type_v01, result), .ei_array = NULL, }, { .data_type = QMI_SIGNED_2_BYTE_ENUM, .elem_len = 1, .elem_size = sizeof(uint16_t), .array_type = NO_ARRAY, .tlv_type = QMI_COMMON_TLV_TYPE, .offset = offsetof(struct qmi_response_type_v01, error), .ei_array = NULL, }, { .data_type = QMI_EOTI, .elem_len = 0, .elem_size = 0, .array_type = NO_ARRAY, .tlv_type = QMI_COMMON_TLV_TYPE, .offset = 0, .ei_array = NULL, }, }; qrtr-1.1/lib/qrtr.c000066400000000000000000000125631460530201100142440ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include "libqrtr.h" #include "logging.h" #include "ns.h" static int qrtr_getname(int sock, struct sockaddr_qrtr *sq) { socklen_t sl = sizeof(*sq); int rc; rc = getsockname(sock, (void *)sq, &sl); if (rc) { PLOGE("getsockname()"); return -1; } if (sq->sq_family != AF_QIPCRTR || sl != sizeof(*sq)) return -1; return 0; } int qrtr_open(int rport) { struct timeval tv; int sock; int rc; sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); if (sock < 0) { PLOGE("socket(AF_QIPCRTR)"); return -1; } tv.tv_sec = 1; tv.tv_usec = 0; rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); if (rc) { PLOGE("setsockopt(SO_RCVTIMEO)"); goto err; } if (rport != 0) { struct sockaddr_qrtr sq = {}; sq.sq_family = AF_QIPCRTR; sq.sq_node = 1; sq.sq_port = rport; rc = bind(sock, (void *)&sq, sizeof(sq)); if (rc < 0) { PLOGE("bind(%d)", rport); goto err; } } return sock; err: close(sock); return -1; } void qrtr_close(int sock) { close(sock); } int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz) { struct sockaddr_qrtr sq = {}; int rc; sq.sq_family = AF_QIPCRTR; sq.sq_node = node; sq.sq_port = port; rc = sendto(sock, data, sz, 0, (void *)&sq, sizeof(sq)); if (rc < 0) { PLOGE("sendto()"); return -1; } return 0; } int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; if (qrtr_getname(sock, &sq)) return -1; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); pkt.server.service = cpu_to_le32(service); pkt.server.instance = cpu_to_le32(instance << 8 | version); return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); } int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; if (qrtr_getname(sock, &sq)) return -1; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); pkt.server.service = cpu_to_le32(service); pkt.server.instance = cpu_to_le32(instance << 8 | version); pkt.server.node = cpu_to_le32(sq.sq_node); pkt.server.port = cpu_to_le32(sq.sq_port); return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); } int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance) { return qrtr_new_server(sock, service, version, instance); } int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance) { return qrtr_remove_server(sock, service, version, instance); } int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; if (qrtr_getname(sock, &sq)) return -1; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP); pkt.server.service = cpu_to_le32(service); pkt.server.instance = cpu_to_le32(instance << 8 | version); return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); } int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; if (qrtr_getname(sock, &sq)) return -1; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_LOOKUP); pkt.server.service = cpu_to_le32(service); pkt.server.instance = cpu_to_le32(instance << 8 | version); pkt.server.node = cpu_to_le32(sq.sq_node); pkt.server.port = cpu_to_le32(sq.sq_port); return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); } int qrtr_poll(int sock, unsigned int ms) { struct pollfd fds; fds.fd = sock; fds.revents = 0; fds.events = POLLIN | POLLERR; return poll(&fds, 1, ms); } int qrtr_recv(int sock, void *buf, unsigned int bsz) { int rc; rc = recv(sock, buf, bsz, 0); if (rc < 0) PLOGE("recv()"); return rc; } int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port) { struct sockaddr_qrtr sq; socklen_t sl; int rc; sl = sizeof(sq); rc = recvfrom(sock, buf, bsz, 0, (void *)&sq, &sl); if (rc < 0) { PLOGE("recvfrom()"); return rc; } if (node) *node = sq.sq_node; if (port) *port = sq.sq_port; return rc; } int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len, const struct sockaddr_qrtr *sq) { const struct qrtr_ctrl_pkt *ctrl = buf; if (sq->sq_port == QRTR_PORT_CTRL){ if (len < sizeof(*ctrl)) return -EMSGSIZE; dest->type = le32_to_cpu(ctrl->cmd); switch (dest->type) { case QRTR_TYPE_BYE: dest->node = le32_to_cpu(ctrl->client.node); break; case QRTR_TYPE_DEL_CLIENT: dest->node = le32_to_cpu(ctrl->client.node); dest->port = le32_to_cpu(ctrl->client.port); break; case QRTR_TYPE_NEW_SERVER: case QRTR_TYPE_DEL_SERVER: dest->node = le32_to_cpu(ctrl->server.node); dest->port = le32_to_cpu(ctrl->server.port); dest->service = le32_to_cpu(ctrl->server.service); dest->version = le32_to_cpu(ctrl->server.instance) & 0xff; dest->instance = le32_to_cpu(ctrl->server.instance) >> 8; break; default: dest->type = 0; } } else { dest->type = QRTR_TYPE_DATA; dest->node = sq->sq_node; dest->port = sq->sq_port; dest->data = buf; dest->data_len = len; } return 0; } qrtr-1.1/meson.build000066400000000000000000000024411460530201100144760ustar00rootroot00000000000000# SPDX-License-Identifier: BSD-3-Clause project('qrtr', 'c', license : [ 'BSD-3-Clause'], default_options : [ 'warning_level=1', 'buildtype=release', ]) prefix = get_option('prefix') with_qrtr_ns = get_option('qrtr-ns') install_systemd_unit = get_option('systemd-service') systemd = dependency('systemd', required : install_systemd_unit) if systemd.found() systemd_system_unit_dir = get_option('systemd-unit-prefix') if systemd_system_unit_dir == '' systemd_system_unit_dir = systemd.get_variable( pkgconfig : 'systemdsystemunitdir', pkgconfig_define: ['prefix', prefix]) else message('Could not resolve systemd dependencies, skipping unit file') install_systemd_unit = false endif endif inc = include_directories('include') subdir('lib') subdir('include') subdir('src') if systemd.found() and with_qrtr_ns.enabled() systemd_unit_conf = configuration_data() systemd_unit_conf.set('prefix', prefix) configure_file( input : 'qrtr-ns.service.in', output : 'qrtr-ns.service', configuration : systemd_unit_conf, install_dir : systemd_system_unit_dir) endif qrtr-1.1/meson_options.txt000066400000000000000000000005451460530201100157740ustar00rootroot00000000000000option('qrtr-ns', type: 'feature', value: 'auto', description: 'Whether or not to build the qrtr-ns binary' ) option('systemd-unit-prefix', type: 'string', description: 'Directory for systemd system unit files' ) option('systemd-service', type: 'feature', value: 'auto', description: 'Whether or not the systemd service should be built' ) qrtr-1.1/qrtr-ns.service.in000066400000000000000000000002141460530201100157250ustar00rootroot00000000000000[Unit] Description=QIPCRTR Name Service [Service] ExecStart=@prefix@/bin/qrtr-ns -f 1 Restart=always [Install] WantedBy=multi-user.target qrtr-1.1/qrtr.py000077500000000000000000000065171460530201100137110ustar00rootroot00000000000000#!/usr/bin/env python2.7 import ctypes import collections from ctypes import CDLL, CFUNCTYPE, POINTER, cast, py_object from ctypes import c_char_p, c_void_p, c_int, pointer _qrtr = CDLL("./libqrtr.so") class qrtr: Result = collections.namedtuple('Result', ['service', 'instance', 'addr']) _cbtype = CFUNCTYPE(None, c_void_p, c_int, c_int, c_int, c_int) def __init__(self, port=0): self.sock = _qrtr.qrtr_open(port) if self.sock < 0: raise RuntimeError("unable to open qrtr socket") self.service = None self._qrtr = _qrtr def __del__(self): self._qrtr.qrtr_close(self.sock) def _lookup_list_add(self, ptr, srv, instance, node, port): res = qrtr.Result(srv, instance, (node, port)) cast(ptr, POINTER(py_object)).contents.value.append(res) def lookup(self, srv, instance=0, ifilter=0): results = [] err = _qrtr.qrtr_lookup(self.sock, srv, instance, ifilter, qrtr._cbtype(self._lookup_list_add), cast(pointer(py_object(results)), c_void_p)) if err: raise RuntimeError("query failed") return results def publish(self, service, version, instance): err = _qrtr.qrtr_publish(self.sock, service, version, instance) if err: raise RuntimeError("publish failed") self.service = (service, version, instance) def new_server(self, service, version, instance): err = _qrtr.qrtr_new_server(self.sock, service, version, instance) if err: raise RuntimeError("new_server failed") self.service = (service, version, instance) return self.service def remove_server(self, service): err = _qrtr.qrtr_remove_server(self.sock, *service) if err: raise RuntimeError("remove_server failed") self.service = None def new_lookup(self, service, version, instance): err = _qrtr.qrtr_new_lookup(self.sock, service, version, instance) if err: raise RuntimeError("new_lookup failed") return (service, version, instance) def remove_lookup(self, lookup): err = _qrtr.qrtr_remove_lookup(self.sock, *lookup) if err: raise RuntimeError("remove_lookup failed") def send(self, addr, data): node, port = addr n = _qrtr.qrtr_sendto(self.sock, node, port, c_char_p(data), len(data)) if n: raise RuntimeError("sendto failed") def recv(self, sz=65536): buf = ctypes.create_string_buffer(sz) n = _qrtr.qrtr_recv(self.sock, c_char_p(ctypes.addressof(buf)), sz) if n <= 0: raise RuntimeError("recv failed") return buf[0:n] def recvfrom(self, sz=65536): node = ctypes.c_int() port = ctypes.c_int() buf = ctypes.create_string_buffer(sz) n = _qrtr.qrtr_recvfrom(self.sock, c_char_p(ctypes.addressof(buf)), ctypes.byref(node), ctypes.byref(port)) if n <= 0: raise RuntimeError("recvfrom failed") return (buf[0:n], (node.value, port.value)) def poll(self, tout=0): return _qrtr.qrtr_poll(self.sock, tout) if __name__ == "__main__": svcs = qrtr().lookup(15) # 15 is the test service print " service instance addr" for svc in svcs: print "% 8d % 8d %s" % (svc.service, svc.instance, svc.addr) qrtr-1.1/src/000077500000000000000000000000001460530201100131225ustar00rootroot00000000000000qrtr-1.1/src/addr.c000066400000000000000000000035151460530201100142040ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include "libqrtr.h" #include "logging.h" void qrtr_set_address(uint32_t addr) { struct { struct nlmsghdr nh; struct ifaddrmsg ifa; char attrbuf[32]; } req; struct { struct nlmsghdr nh; struct nlmsgerr err; } resp; struct sockaddr_qrtr sq; struct rtattr *rta; socklen_t sl = sizeof(sq); int sock; int ret; /* Trigger loading of the qrtr kernel module */ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); if (sock < 0) PLOGE_AND_EXIT("failed to create AF_QIPCRTR socket"); ret = getsockname(sock, (void*)&sq, &sl); if (ret < 0) PLOGE_AND_EXIT("getsockname()"); close(sock); /* Skip configuring the address, if it's same as current */ if (sl == sizeof(sq) && sq.sq_node == addr) return; sock = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); if (sock < 0) PLOGE_AND_EXIT("failed to create netlink socket"); memset(&req, 0, sizeof(req)); req.nh.nlmsg_len = NLMSG_SPACE(sizeof(struct ifaddrmsg)); req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; req.nh.nlmsg_type = RTM_NEWADDR; req.ifa.ifa_family = AF_QIPCRTR; rta = (struct rtattr *)(((char *) &req) + req.nh.nlmsg_len); rta->rta_type = IFA_LOCAL; rta->rta_len = RTA_LENGTH(sizeof(addr)); memcpy(RTA_DATA(rta), &addr, sizeof(addr)); req.nh.nlmsg_len += rta->rta_len; ret = send(sock, &req, req.nh.nlmsg_len, 0); if (ret < 0) PLOGE_AND_EXIT("failed to send netlink request"); ret = recv(sock, &resp, sizeof(resp), 0); if (ret < 0) PLOGE_AND_EXIT("failed to receive netlink response"); if (resp.nh.nlmsg_type == NLMSG_ERROR && resp.err.error != 0) { errno = -resp.err.error; PLOGE_AND_EXIT("failed to configure node id"); } close(sock); } qrtr-1.1/src/addr.h000066400000000000000000000001501460530201100142010ustar00rootroot00000000000000#ifndef __ADDR_H_ #define __ADDR_H_ #include void qrtr_set_address(uint32_t addr); #endif qrtr-1.1/src/cfg.c000066400000000000000000000014551460530201100140320ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include #include #include #include "addr.h" #include "libqrtr.h" #include "logging.h" static void usage(const char *progname) { fprintf(stderr, "%s \n", progname); exit(1); } int main(int argc, char **argv) { unsigned long addrul; uint32_t addr; char *ep; const char *progname = basename(argv[0]); qlog_setup(progname, false); if (argc != 2) usage(progname); addrul = strtoul(argv[1], &ep, 10); if (argv[1][0] == '\0' || *ep != '\0' || addrul >= UINT_MAX) usage(progname); addr = addrul; qrtr_set_address(addr); return 0; } qrtr-1.1/src/hash.c000066400000000000000000000012311460530201100142060ustar00rootroot00000000000000#include #include "hash.h" unsigned int hash_mem(const void *data, unsigned int len) { unsigned int h; unsigned int i; h = len; for (i = 0; i < len; ++i) h = ((h >> 27) ^ (h << 5)) ^ ((const unsigned char *)data)[i]; return h; } unsigned int hash_string(const char *value) { return hash_mem(value, strlen(value)); } unsigned int hash_u32(uint32_t value) { return value * 2654435761UL; } unsigned int hash_u64(uint64_t value) { return hash_u32(value & 0xffffffff) ^ hash_u32(value >> 32); } unsigned int hash_pointer(void *value) { if (sizeof(value) == sizeof(uint64_t)) return hash_u64((long)value); return hash_u32((long)value); } qrtr-1.1/src/hash.h000066400000000000000000000004361460530201100142210ustar00rootroot00000000000000#ifndef _HASH_H_ #define _HASH_H_ #include unsigned int hash_mem(const void *data, unsigned int len); unsigned int hash_string(const char *value); unsigned int hash_u32(uint32_t value); unsigned int hash_u64(uint64_t value); unsigned int hash_pointer(void *value); #endif qrtr-1.1/src/list.h000066400000000000000000000052551460530201100142550ustar00rootroot00000000000000#ifndef _LIST_H_ #define _LIST_H_ #include #ifndef offsetof #define offsetof(type, md) ((size_t)&((type *)0)->md) #endif #ifndef container_of #define container_of(ptr, type, member) \ ((type *)((char *)(ptr) - offsetof(type, member))) #endif struct list_item { struct list_item *next; struct list_item *prev; }; struct list { struct list_item *head; struct list_item *tail; }; #define LIST_INIT(name) { 0, 0 } #define LIST(name) \ struct list name = LIST_INIT(name) #define list_entry(ptr, type, member) \ container_of(ptr, type, member) static inline void list_init(struct list *list) { list->head = 0; list->tail = 0; } static inline void list_append(struct list *list, struct list_item *item) { item->next = 0; item->prev = list->tail; if (list->tail != 0) list->tail->next = item; else list->head = item; list->tail = item; } static inline void list_prepend(struct list *list, struct list_item *item) { item->prev = 0; item->next = list->head; if (list->head == 0) list->tail = item; list->head = item; } static inline void list_insert(struct list *list, struct list_item *after, struct list_item *item) { if (after == 0) { list_prepend(list, item); return; } item->prev = after; item->next = after->next; after->next = item; if (item->next) item->next->prev = item; if (list->tail == after) list->tail = item; } static inline void list_remove(struct list *list, struct list_item *item) { if (item->next) item->next->prev = item->prev; if (list->head == item) { list->head = item->next; if (list->head == 0) list->tail = 0; } else { item->prev->next = item->next; if (list->tail == item) list->tail = item->prev; } item->prev = item->next = 0; } static inline struct list_item *list_pop(struct list *list) { struct list_item *item; item = list->head; if (item == 0) return 0; list_remove(list, item); return item; } static inline struct list_item *list_last(struct list *list) { return list->tail; } static inline struct list_item *list_first(struct list *list) { return list->head; } static inline struct list_item *list_next(struct list_item *item) { return item->next; } #define list_push list_append #define list_for_each(_list, _iter) \ for (_iter = (_list)->head; (_iter) != 0; _iter = (_iter)->next) #define list_for_each_after(_node, _iter) \ for (_iter = (_node)->next; (_iter) != 0; _iter = (_iter)->next) #define list_for_each_safe(_list, _iter, _bkup) \ for (_iter = (_list)->head; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup)) #define list_for_each_safe_after(_node, _iter, _bkup) \ for (_iter = (_node)->next; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup)) #endif qrtr-1.1/src/lookup.c000066400000000000000000000154361460530201100146100ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include #include #include "logging.h" #include "ns.h" #include "util.h" #define DIAG_SERVICE 4097 static const struct { unsigned int service; unsigned int ifilter; const char *name; } common_names[] = { { 0, 0, "Control service" }, { 1, 0, "Wireless Data Service" }, { 2, 0, "Device Management Service" }, { 3, 0, "Network Access Service" }, { 4, 0, "Quality Of Service service" }, { 5, 0, "Wireless Messaging Service" }, { 6, 0, "Position Determination Service" }, { 7, 0, "Authentication service" }, { 8, 0, "AT service" }, { 9, 0, "Voice service" }, { 10, 0, "Card Application Toolkit service (v2)" }, { 11, 0, "User Identity Module service" }, { 12, 0, "Phonebook Management service" }, { 13, 0, "QCHAT service" }, { 14, 0, "Remote file system service" }, { 15, 0, "Test service" }, { 16, 0, "Location service (~ PDS v2)" }, { 17, 0, "Specific absorption rate service" }, { 18, 0, "IMS settings service" }, { 19, 0, "Analog to digital converter driver service" }, { 20, 0, "Core sound driver service" }, { 21, 0, "Modem embedded file system service" }, { 22, 0, "Time service" }, { 23, 0, "Thermal sensors service" }, { 24, 0, "Thermal mitigation device service" }, { 25, 0, "Service access proxy service" }, { 26, 0, "Wireless data administrative service" }, { 27, 0, "TSYNC control service" }, { 28, 0, "Remote file system access service" }, { 29, 0, "Circuit switched videotelephony service" }, { 30, 0, "Qualcomm mobile access point service" }, { 31, 0, "IMS presence service" }, { 32, 0, "IMS videotelephony service" }, { 33, 0, "IMS application service" }, { 34, 0, "Coexistence service" }, { 36, 0, "Persistent device configuration service" }, { 38, 0, "Simultaneous transmit service" }, { 39, 0, "Bearer independent transport service" }, { 40, 0, "IMS RTP service" }, { 41, 0, "RF radiated performance enhancement service" }, { 42, 0, "Data system determination service" }, { 43, 0, "Subsystem control service" }, { 47, 0, "Data Port Mapper service" }, { 48, 0, "QMI DFS service" }, { 49, 0, "IPA control service" }, { 50, 0, "UIMRMT service" }, { 51, 0, "CoreSight remote tracing service" }, { 52, 0, "Dynamic Heap Memory Sharing" }, { 55, 0, "QMI-SLIM service" }, { 56, 0, "LOWI (Location) service" }, { 57, 0, "WLPS service" }, { 64, 0, "Service registry locator service" }, { 66, 0, "Service registry notification service" }, { 69, 0, "ATH10k WLAN firmware service" }, { 70, 0, "LTE service" }, { 71, 0, "UIMHTTP service" }, { 75, 0, "QDMA service" }, { 76, 0, "SSGCCS service" }, { 77, 0, "IMS QMI Priv service" }, { 224, 0, "Card Application Toolkit service (v1)" }, { 225, 0, "Remote Management Service" }, { 226, 0, "Open Mobile Alliance device management service" }, { 231, 0, "Vendor-specific service" }, { 235, 0, "Modem service" }, { 312, 0, "QBT1000 Ultrasonic Fingerprint Sensor service" }, { 400, 0, "Snapdragon Sensor Core service" }, { 769, 0, "SLIMbus control service" }, { 770, 0, "IMS data service" }, { 771, 0, "Peripheral Access Control Manager service" }, { 4096, 0, "TFTP" }, { DIAG_SERVICE, 0, "DIAG service" }, }; static const char *diag_instance_base_str(unsigned int instance_base) { switch (instance_base) { case 0: return "MODEM"; case 1: return "LPASS"; case 2: return "WCNSS"; case 3: return "SENSORS"; case 4: return "CDSP"; case 5: return "WDSP"; default: return ""; } } static const char *diag_instance_str(unsigned int instance) { switch (instance) { case 0: return "CNTL"; case 1: return "CMD"; case 2: return "DATA"; case 3: return "DCI_CMD"; case 4: return "DCI"; default: return ""; } } static int get_diag_instance_info(char *str, size_t size, unsigned int instance) { return snprintf(str, size, "%s:%s", diag_instance_base_str(instance >> 6), diag_instance_str(instance & 0x3f)); } static unsigned int read_num_le(const char *str, int *rcp) { unsigned int ret; char *e; if (*rcp) return 0; errno = 0; ret = strtoul(str, &e, 0); *rcp = -(errno || *e); return cpu_to_le32(ret); } int main(int argc, char **argv) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; unsigned int instance; unsigned int service; unsigned int version; unsigned int node; unsigned int port; socklen_t sl = sizeof(sq); struct timeval tv; int sock; int len; int rc; const char *progname = basename(argv[0]); qlog_setup(progname, false); rc = 0; memset(&pkt, 0, sizeof(pkt)); switch (argc) { default: rc = -1; break; case 3: pkt.server.instance = read_num_le(argv[2], &rc); case 2: pkt.server.service = read_num_le(argv[1], &rc); case 1: break; } if (rc) { fprintf(stderr, "Usage: %s [ [ []]]\n", progname); exit(1); } sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); if (sock < 0) PLOGE_AND_EXIT("sock(AF_QIPCRTR)"); rc = getsockname(sock, (void *)&sq, &sl); if (rc || sq.sq_family != AF_QIPCRTR || sl != sizeof(sq)) PLOGE_AND_EXIT("getsockname()"); sq.sq_port = QRTR_PORT_CTRL; tv.tv_sec = 1; tv.tv_usec = 0; pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP); rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); if (rc) PLOGE_AND_EXIT("setsockopt(SO_RCVTIMEO)"); rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq)); if (rc < 0) PLOGE_AND_EXIT("sendto()"); printf(" Service Version Instance Node Port\n"); while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) { unsigned int type = le32_to_cpu(pkt.cmd); const char *name = NULL; unsigned int i; if (len < sizeof(pkt) || type != QRTR_TYPE_NEW_SERVER) { PLOGW("invalid/short packet"); continue; } if (!pkt.server.service && !pkt.server.instance && !pkt.server.node && !pkt.server.port) break; service = le32_to_cpu(pkt.server.service); version = le32_to_cpu(pkt.server.instance) & 0xff; instance = le32_to_cpu(pkt.server.instance) >> 8; node = le32_to_cpu(pkt.server.node); port = le32_to_cpu(pkt.server.port); for (i = 0; i < sizeof(common_names)/sizeof(common_names[0]); ++i) { if (service != common_names[i].service) continue; if (instance && (instance & common_names[i].ifilter) != common_names[i].ifilter) continue; name = common_names[i].name; } if (!name) name = ""; if (service == DIAG_SERVICE) { char buf[24]; instance = le32_to_cpu(pkt.server.instance); get_diag_instance_info(buf, sizeof(buf), instance); printf("%9u %s %8u %4u %5u %s (%s)\n", service, "N/A", instance, node, port, name, buf); } else { printf("%9u %7u %8u %4u %5u %s\n", service, version, instance, node, port, name); } } if (len < 0) PLOGE_AND_EXIT("recv()"); close(sock); return 0; } qrtr-1.1/src/map.c000066400000000000000000000113751460530201100140520ustar00rootroot00000000000000/* * Copyright (c) 2008-2009, Courtney Cavin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * - Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * - Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include "map.h" struct map_entry { struct map_item *item; }; /* Marker for deleted items */ static struct map_item deleted; void map_destroy(struct map *map) { free(map->data); } void map_clear(struct map *map, void (*release)(struct map_item *)) { int i; for (i = 0; i < map->size; ++i){ if (!map->data[i].item) continue; if (map->data[i].item != &deleted) (* release)(map->data[i].item); map->data[i].item = NULL; } map->count = 0; } int map_create(struct map *map) { map->size = 0; map->data = 0; map->count = 0; return 0; } static int map_hash(struct map *map, unsigned int key) { struct map_entry *e; int idx, i; if (map->count == map->size) return -1; idx = key % map->size; for (i = 0; i < map->size; ++i) { e = &map->data[idx]; if (!e->item || e->item == &deleted) { ++map->count; return idx; } if (e->item->key == key) return idx; idx = (idx + 1) % map->size; } return -2; } static int map_rehash(struct map *map); int map_reput(struct map *map, unsigned int key, struct map_item *value, struct map_item **old) { int rc; while ((rc = map_hash(map, key)) < 0) { if ((rc = map_rehash(map)) < 0) return rc; } if (old) { if (map->data[rc].item == &deleted) *old = NULL; else *old = map->data[rc].item; } map->data[rc].item = value; if (value) map->data[rc].item->key = key; return 0; } int map_put(struct map *map, unsigned int key, struct map_item *value) { return map_reput(map, key, value, NULL); } static int map_rehash(struct map *map) { struct map_entry *oldt, *newt; int o_size, i; int rc; newt = calloc(sizeof(struct map_entry), map->size + 256); if (!newt) return -1; oldt = map->data; map->data = newt; o_size = map->size; map->size += 256; map->count = 0; for (i = 0; i < o_size; ++i){ if (!oldt[i].item || oldt[i].item == &deleted) continue; rc = map_put(map, oldt[i].item->key, oldt[i].item); if (rc < 0) return rc; } free(oldt); return 0; } static struct map_entry *map_find(const struct map *map, unsigned int key) { struct map_entry *e; int idx, i; if (map->size == 0) return NULL; idx = key % map->size; for (i = 0; i < map->size; ++i) { e = &map->data[idx]; idx = (idx + 1) % map->size; if (!e->item) break; if (e->item == &deleted) continue; if (e->item->key == key) return e; } return NULL; } int map_contains(const struct map *map, unsigned int key) { return (map_find(map, key) == NULL) ? 0 : 1; } struct map_item *map_get(const struct map *map, unsigned int key) { struct map_entry *e; e = map_find(map, key); if (e == NULL) return NULL; return e->item; } int map_remove(struct map *map, unsigned int key) { struct map_entry *e; e = map_find(map, key); if (e) { e->item = &deleted; --map->count; } return !e; } unsigned int map_length(struct map *map) { return map ? map->count : 0; } static struct map_entry *map_iter_from(const struct map *map, unsigned int start) { unsigned int i = start; for (; i < map->size; ++i) { if (map->data[i].item && map->data[i].item != &deleted) return &map->data[i]; } return NULL; } struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter) { if (iter == NULL) return NULL; return map_iter_from(map, (iter - map->data) + 1); } struct map_entry *map_iter_first(const struct map *map) { return map_iter_from(map, 0); } struct map_item *map_iter_item(struct map_entry *iter) { return iter->item; } qrtr-1.1/src/map.h000066400000000000000000000021161460530201100140500ustar00rootroot00000000000000#ifndef _MAP_H_ #define _MAP_H_ struct map_item { unsigned int key; }; struct map_entry; struct map { unsigned int size; unsigned int count; struct map_entry *data; }; int map_create(struct map *map); void map_destroy(struct map *map); void map_clear(struct map *map, void (*release)(struct map_item *)); int map_put(struct map *map, unsigned int key, struct map_item *v); int map_reput(struct map *map, unsigned int key, struct map_item *v, struct map_item **old); int map_contains(const struct map *map, unsigned int key); struct map_item *map_get(const struct map *map, unsigned int key); int map_remove(struct map *map, unsigned int key); unsigned int map_length(struct map *map); struct map_entry *map_iter_first(const struct map *map); struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter); struct map_item *map_iter_item(struct map_entry *iter); #define map_for_each(map, iter) \ for (iter = map_iter_first(map); iter; iter = map_iter_next(map, iter)) #define map_iter_data(iter, type, member) \ container_of(map_iter_item(iter), type, member) #endif qrtr-1.1/src/meson.build000066400000000000000000000014121460530201100152620ustar00rootroot00000000000000# SPDX-License-Identifier: BSD-3-Clause cfg_srcs = ['addr.c', 'cfg.c', 'hash.c'] executable('qrtr-cfg', cfg_srcs, link_with : libqrtr, include_directories : inc, install : true) if with_qrtr_ns.enabled() ns_srcs = ['addr.c', 'hash.c', 'map.c', 'ns.c', 'util.c', 'waiter.c'] executable('qrtr-ns', ns_srcs, link_with : libqrtr, include_directories : inc, install : true) endif executable('qrtr-lookup', 'lookup.c', link_with : libqrtr, include_directories : inc, install : true)qrtr-1.1/src/ns.c000066400000000000000000000423431460530201100137140ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "addr.h" #include "hash.h" #include "list.h" #include "map.h" #include "ns.h" #include "util.h" #include "waiter.h" #include "libqrtr.h" #include "logging.h" static const char *ctrl_pkt_strings[] = { [QRTR_TYPE_HELLO] = "hello", [QRTR_TYPE_BYE] = "bye", [QRTR_TYPE_NEW_SERVER] = "new-server", [QRTR_TYPE_DEL_SERVER] = "del-server", [QRTR_TYPE_DEL_CLIENT] = "del-client", [QRTR_TYPE_RESUME_TX] = "resume-tx", [QRTR_TYPE_EXIT] = "exit", [QRTR_TYPE_PING] = "ping", [QRTR_TYPE_NEW_LOOKUP] = "new-lookup", [QRTR_TYPE_DEL_LOOKUP] = "del-lookup", }; #define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0])) struct context { int sock; int local_node; struct sockaddr_qrtr bcast_sq; struct list lookups; }; struct server_filter { unsigned int service; unsigned int instance; unsigned int ifilter; }; struct lookup { unsigned int service; unsigned int instance; struct sockaddr_qrtr sq; struct list_item li; }; struct server { unsigned int service; unsigned int instance; unsigned int node; unsigned int port; struct map_item mi; struct list_item qli; }; struct node { unsigned int id; struct map_item mi; struct map services; }; static struct map nodes; static void server_mi_free(struct map_item *mi); static struct node *node_get(unsigned int node_id) { struct map_item *mi; struct node *node; int rc; mi = map_get(&nodes, hash_u32(node_id)); if (mi) return container_of(mi, struct node, mi); node = calloc(1, sizeof(*node)); if (!node) return NULL; node->id = node_id; rc = map_create(&node->services); if (rc) LOGE_AND_EXIT("unable to create map"); rc = map_put(&nodes, hash_u32(node_id), &node->mi); if (rc) { map_destroy(&node->services); free(node); return NULL; } return node; } static int server_match(const struct server *srv, const struct server_filter *f) { unsigned int ifilter = f->ifilter; if (f->service != 0 && srv->service != f->service) return 0; if (!ifilter && f->instance) ifilter = ~0; return (srv->instance & ifilter) == f->instance; } static int server_query(const struct server_filter *f, struct list *list) { struct map_entry *node_me; struct map_entry *me; struct node *node; int count = 0; list_init(list); map_for_each(&nodes, node_me) { node = map_iter_data(node_me, struct node, mi); map_for_each(&node->services, me) { struct server *srv; srv = map_iter_data(me, struct server, mi); if (!server_match(srv, f)) continue; list_append(list, &srv->qli); ++count; } } return count; } static int service_announce_new(struct context *ctx, struct sockaddr_qrtr *dest, struct server *srv) { struct qrtr_ctrl_pkt cmsg; int rc; LOGD("advertising new server [%u:%x]@[%u:%u]\n", srv->service, srv->instance, srv->node, srv->port); cmsg.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); cmsg.server.service = cpu_to_le32(srv->service); cmsg.server.instance = cpu_to_le32(srv->instance); cmsg.server.node = cpu_to_le32(srv->node); cmsg.server.port = cpu_to_le32(srv->port); rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0, (struct sockaddr *)dest, sizeof(*dest)); if (rc < 0) PLOGW("sendto()"); return rc; } static int service_announce_del(struct context *ctx, struct sockaddr_qrtr *dest, struct server *srv) { struct qrtr_ctrl_pkt cmsg; int rc; LOGD("advertising removal of server [%u:%x]@[%u:%u]\n", srv->service, srv->instance, srv->node, srv->port); cmsg.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); cmsg.server.service = cpu_to_le32(srv->service); cmsg.server.instance = cpu_to_le32(srv->instance); cmsg.server.node = cpu_to_le32(srv->node); cmsg.server.port = cpu_to_le32(srv->port); rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0, (struct sockaddr *)dest, sizeof(*dest)); if (rc < 0) PLOGW("sendto()"); return rc; } static int lookup_notify(struct context *ctx, struct sockaddr_qrtr *to, struct server *srv, bool new) { struct qrtr_ctrl_pkt pkt = {}; int rc; pkt.cmd = new ? QRTR_TYPE_NEW_SERVER : QRTR_TYPE_DEL_SERVER; if (srv) { pkt.server.service = cpu_to_le32(srv->service); pkt.server.instance = cpu_to_le32(srv->instance); pkt.server.node = cpu_to_le32(srv->node); pkt.server.port = cpu_to_le32(srv->port); } rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)to, sizeof(*to)); if (rc < 0) PLOGW("send lookup result failed"); return rc; } static int annouce_servers(struct context *ctx, struct sockaddr_qrtr *sq) { struct map_entry *me; struct server *srv; struct node *node; int rc; node = node_get(ctx->local_node); if (!node) return 0; map_for_each(&node->services, me) { srv = map_iter_data(me, struct server, mi); rc = service_announce_new(ctx, sq, srv); if (rc < 0) return rc; } return 0; } static struct server *server_add(unsigned int service, unsigned int instance, unsigned int node_id, unsigned int port) { struct map_item *mi; struct server *srv; struct node *node; int rc; if (!service || !port) return NULL; srv = calloc(1, sizeof(*srv)); if (srv == NULL) return NULL; srv->service = service; srv->instance = instance; srv->node = node_id; srv->port = port; node = node_get(node_id); if (!node) goto err; rc = map_reput(&node->services, hash_u32(port), &srv->mi, &mi); if (rc) goto err; LOGD("add server [%u:%x]@[%u:%u]\n", srv->service, srv->instance, srv->node, srv->port); if (mi) { /* we replaced someone */ struct server *old = container_of(mi, struct server, mi); free(old); } return srv; err: free(srv); return NULL; } static int server_del(struct context *ctx, struct node *node, unsigned int port) { struct lookup *lookup; struct list_item *li; struct map_item *mi; struct server *srv; mi = map_get(&node->services, hash_u32(port)); if (!mi) return -ENOENT; srv = container_of(mi, struct server, mi); map_remove(&node->services, srv->mi.key); /* Broadcast the removal of local services */ if (srv->node == ctx->local_node) service_announce_del(ctx, &ctx->bcast_sq, srv); /* Announce the service's disappearance to observers */ list_for_each(&ctx->lookups, li) { lookup = container_of(li, struct lookup, li); if (lookup->service && lookup->service != srv->service) continue; if (lookup->instance && lookup->instance != srv->instance) continue; lookup_notify(ctx, &lookup->sq, srv, false); } free(srv); return 0; } static int ctrl_cmd_hello(struct context *ctx, struct sockaddr_qrtr *sq, const void *buf, size_t len) { int rc; rc = sendto(ctx->sock, buf, len, 0, (void *)sq, sizeof(*sq)); if (rc > 0) rc = annouce_servers(ctx, sq); return rc; } static int ctrl_cmd_bye(struct context *ctx, struct sockaddr_qrtr *from) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; struct node *local_node; struct map_entry *me; struct server *srv; struct node *node; int rc; node = node_get(from->sq_node); if (!node) return 0; map_for_each(&node->services, me) { srv = map_iter_data(me, struct server, mi); server_del(ctx, node, srv->port); } /* Advertise the removal of this client to all local services */ local_node = node_get(ctx->local_node); if (!local_node) return 0; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = QRTR_TYPE_BYE; pkt.client.node = from->sq_node; map_for_each(&local_node->services, me) { srv = map_iter_data(me, struct server, mi); sq.sq_family = AF_QIPCRTR; sq.sq_node = srv->node; sq.sq_port = srv->port; rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)&sq, sizeof(sq)); if (rc < 0) PLOGW("bye propagation failed"); } return 0; } static int ctrl_cmd_del_client(struct context *ctx, struct sockaddr_qrtr *from, unsigned node_id, unsigned port) { struct qrtr_ctrl_pkt pkt; struct sockaddr_qrtr sq; struct node *local_node; struct list_item *tmp; struct lookup *lookup; struct list_item *li; struct map_entry *me; struct server *srv; struct node *node; int rc; /* Don't accept spoofed messages */ if (from->sq_node != node_id) return -EINVAL; /* Local DEL_CLIENT messages comes from the port being closed */ if (from->sq_node == ctx->local_node && from->sq_port != port) return -EINVAL; /* Remove any lookups by this client */ list_for_each_safe(&ctx->lookups, li, tmp) { lookup = container_of(li, struct lookup, li); if (lookup->sq.sq_node != node_id) continue; if (lookup->sq.sq_port != port) continue; list_remove(&ctx->lookups, &lookup->li); free(lookup); } /* Remove the server belonging to this port*/ node = node_get(node_id); if (node) server_del(ctx, node, port); /* Advertise the removal of this client to all local services */ local_node = node_get(ctx->local_node); if (!local_node) return 0; pkt.cmd = QRTR_TYPE_DEL_CLIENT; pkt.client.node = node_id; pkt.client.port = port; map_for_each(&local_node->services, me) { srv = map_iter_data(me, struct server, mi); sq.sq_family = AF_QIPCRTR; sq.sq_node = srv->node; sq.sq_port = srv->port; rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)&sq, sizeof(sq)); if (rc < 0) PLOGW("del_client propagation failed"); } return 0; } static int ctrl_cmd_new_server(struct context *ctx, struct sockaddr_qrtr *from, unsigned int service, unsigned int instance, unsigned int node_id, unsigned int port) { struct lookup *lookup; struct list_item *li; struct server *srv; int rc = 0; /* Ignore specified node and port for local servers*/ if (from->sq_node == ctx->local_node) { node_id = from->sq_node; port = from->sq_port; } /* Don't accept spoofed messages */ if (from->sq_node != node_id) return -EINVAL; srv = server_add(service, instance, node_id, port); if (!srv) return -EINVAL; if (srv->node == ctx->local_node) rc = service_announce_new(ctx, &ctx->bcast_sq, srv); list_for_each(&ctx->lookups, li) { lookup = container_of(li, struct lookup, li); if (lookup->service && lookup->service != service) continue; if (lookup->instance && lookup->instance != instance) continue; lookup_notify(ctx, &lookup->sq, srv, true); } return rc; } static int ctrl_cmd_del_server(struct context *ctx, struct sockaddr_qrtr *from, unsigned int service, unsigned int instance, unsigned int node_id, unsigned int port) { struct node *node; /* Ignore specified node and port for local servers*/ if (from->sq_node == ctx->local_node) { node_id = from->sq_node; port = from->sq_port; } /* Don't accept spoofed messages */ if (from->sq_node != node_id) return -EINVAL; /* Local servers may only unregister themselves */ if (from->sq_node == ctx->local_node && from->sq_port != port) return -EINVAL; node = node_get(node_id); if (!node) return -ENOENT; return server_del(ctx, node, port); } static int ctrl_cmd_new_lookup(struct context *ctx, struct sockaddr_qrtr *from, unsigned int service, unsigned int instance) { struct server_filter filter; struct list reply_list; struct lookup *lookup; struct list_item *li; struct server *srv; /* Accept only local observers */ if (from->sq_node != ctx->local_node) return -EINVAL; lookup = calloc(1, sizeof(*lookup)); if (!lookup) return -EINVAL; lookup->sq = *from; lookup->service = service; lookup->instance = instance; list_append(&ctx->lookups, &lookup->li); memset(&filter, 0, sizeof(filter)); filter.service = service; filter.instance = instance; server_query(&filter, &reply_list); list_for_each(&reply_list, li) { srv = container_of(li, struct server, qli); lookup_notify(ctx, from, srv, true); } lookup_notify(ctx, from, NULL, true); return 0; } static int ctrl_cmd_del_lookup(struct context *ctx, struct sockaddr_qrtr *from, unsigned int service, unsigned int instance) { struct lookup *lookup; struct list_item *tmp; struct list_item *li; list_for_each_safe(&ctx->lookups, li, tmp) { lookup = container_of(li, struct lookup, li); if (lookup->sq.sq_node != from->sq_node) continue; if (lookup->sq.sq_port != from->sq_port) continue; if (lookup->service != service) continue; if (lookup->instance && lookup->instance != instance) continue; list_remove(&ctx->lookups, &lookup->li); free(lookup); } return 0; } static void ctrl_port_fn(void *vcontext, struct waiter_ticket *tkt) { struct context *ctx = vcontext; struct sockaddr_qrtr sq; int sock = ctx->sock; struct qrtr_ctrl_pkt *msg; unsigned int cmd; char buf[4096]; socklen_t sl; ssize_t len; int rc; sl = sizeof(sq); len = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl); if (len <= 0) { PLOGW("recvfrom()"); close(sock); ctx->sock = -1; goto out; } msg = (void *)buf; if (len < 4) { LOGW("short packet from %u:%u", sq.sq_node, sq.sq_port); goto out; } cmd = le32_to_cpu(msg->cmd); if (cmd < ARRAY_SIZE(ctrl_pkt_strings) && ctrl_pkt_strings[cmd]) LOGD("%s from %u:%u\n", ctrl_pkt_strings[cmd], sq.sq_node, sq.sq_port); else LOGD("UNK (%08x) from %u:%u\n", cmd, sq.sq_node, sq.sq_port); rc = 0; switch (cmd) { case QRTR_TYPE_HELLO: rc = ctrl_cmd_hello(ctx, &sq, buf, len); break; case QRTR_TYPE_BYE: rc = ctrl_cmd_bye(ctx, &sq); break; case QRTR_TYPE_DEL_CLIENT: rc = ctrl_cmd_del_client(ctx, &sq, le32_to_cpu(msg->client.node), le32_to_cpu(msg->client.port)); break; case QRTR_TYPE_NEW_SERVER: rc = ctrl_cmd_new_server(ctx, &sq, le32_to_cpu(msg->server.service), le32_to_cpu(msg->server.instance), le32_to_cpu(msg->server.node), le32_to_cpu(msg->server.port)); break; case QRTR_TYPE_DEL_SERVER: rc = ctrl_cmd_del_server(ctx, &sq, le32_to_cpu(msg->server.service), le32_to_cpu(msg->server.instance), le32_to_cpu(msg->server.node), le32_to_cpu(msg->server.port)); break; case QRTR_TYPE_EXIT: case QRTR_TYPE_PING: case QRTR_TYPE_RESUME_TX: break; case QRTR_TYPE_NEW_LOOKUP: rc = ctrl_cmd_new_lookup(ctx, &sq, le32_to_cpu(msg->server.service), le32_to_cpu(msg->server.instance)); break; case QRTR_TYPE_DEL_LOOKUP: rc = ctrl_cmd_del_lookup(ctx, &sq, le32_to_cpu(msg->server.service), le32_to_cpu(msg->server.instance)); break; } if (rc < 0) LOGW("failed while handling packet from %u:%u", sq.sq_node, sq.sq_port); out: waiter_ticket_clear(tkt); } static int say_hello(struct context *ctx) { struct qrtr_ctrl_pkt pkt; int rc; memset(&pkt, 0, sizeof(pkt)); pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO); rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)&ctx->bcast_sq, sizeof(ctx->bcast_sq)); if (rc < 0) return rc; return 0; } static void server_mi_free(struct map_item *mi) { free(container_of(mi, struct server, mi)); } static void node_mi_free(struct map_item *mi) { struct node *node = container_of(mi, struct node, mi); map_clear(&node->services, server_mi_free); map_destroy(&node->services); free(node); } static void go_dormant(int sock) { close(sock); for (;;) sleep(UINT_MAX); } static void usage(const char *progname) { fprintf(stderr, "%s [-f] [-s] []\n", progname); exit(1); } int main(int argc, char **argv) { struct waiter_ticket *tkt; struct sockaddr_qrtr sq; struct context ctx; unsigned long addr = (unsigned long)-1; struct waiter *w; socklen_t sl = sizeof(sq); bool foreground = false; bool use_syslog = false; bool verbose_log = false; char *ep; int opt; int rc; const char *progname = basename(argv[0]); while ((opt = getopt(argc, argv, "fsv")) != -1) { switch (opt) { case 'f': foreground = true; break; case 's': use_syslog = true; break; case 'v': verbose_log = true; break; default: usage(progname); } } qlog_setup(progname, use_syslog); if (verbose_log) qlog_set_min_priority(LOG_DEBUG); if (optind < argc) { addr = strtoul(argv[optind], &ep, 10); if (argv[1][0] == '\0' || *ep != '\0' || addr >= UINT_MAX) usage(progname); qrtr_set_address(addr); optind++; } if (optind != argc) usage(progname); w = waiter_create(); if (w == NULL) LOGE_AND_EXIT("unable to create waiter"); list_init(&ctx.lookups); rc = map_create(&nodes); if (rc) LOGE_AND_EXIT("unable to create node map"); ctx.sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); if (ctx.sock < 0) PLOGE_AND_EXIT("unable to create control socket"); rc = getsockname(ctx.sock, (void*)&sq, &sl); if (rc < 0) PLOGE_AND_EXIT("getsockname()"); sq.sq_port = QRTR_PORT_CTRL; ctx.local_node = sq.sq_node; rc = bind(ctx.sock, (void *)&sq, sizeof(sq)); if (rc < 0) { if (errno == EADDRINUSE) { PLOGE("nameserver already running, going dormant"); go_dormant(ctx.sock); } PLOGE_AND_EXIT("bind control socket"); } ctx.bcast_sq.sq_family = AF_QIPCRTR; ctx.bcast_sq.sq_node = QRTR_NODE_BCAST; ctx.bcast_sq.sq_port = QRTR_PORT_CTRL; rc = say_hello(&ctx); if (rc) PLOGE_AND_EXIT("unable to say hello"); /* If we're going to background, fork and exit parent */ if (!foreground && fork() != 0) { close(ctx.sock); exit(0); } tkt = waiter_add_fd(w, ctx.sock); waiter_ticket_callback(tkt, ctrl_port_fn, &ctx); while (ctx.sock >= 0) waiter_wait(w); puts("exiting cleanly"); waiter_destroy(w); map_clear(&nodes, node_mi_free); map_destroy(&nodes); return 0; } qrtr-1.1/src/util.c000066400000000000000000000004231460530201100142420ustar00rootroot00000000000000#include #include #include #include #include "util.h" uint64_t time_ms(void) { struct timeval tv; gettimeofday(&tv, NULL); return (uint64_t)tv.tv_sec*1000 + tv.tv_usec/1000; } void util_sleep(int ms) { usleep(ms * 1000); } qrtr-1.1/src/util.h000066400000000000000000000001631460530201100142500ustar00rootroot00000000000000#ifndef __UTIL_H_ #define __UTIL_H_ #include uint64_t time_ms(void); void util_sleep(int ms); #endif qrtr-1.1/src/waiter.c000066400000000000000000000177611460530201100145750ustar00rootroot00000000000000/* * Copyright (c) 2013-2014, Sony Mobile Communications Inc. * Copyright (c) 2014, Courtney Cavin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * - Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * - Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * - Neither the name of the organization nor the names of its contributors * may be used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include "list.h" #include "waiter.h" #include "util.h" struct pollset { int nfds; int cause; }; static struct pollset *pollset_create(int count) { struct pollset *ps; ps = calloc(1, sizeof(*ps) + sizeof(struct pollfd) * count); if (ps == NULL) return NULL; return ps; } static void pollset_destroy(struct pollset *ps) { free(ps); } static void pollset_reset(struct pollset *ps) { ps->nfds = 0; } static void pollset_add_fd(struct pollset *ps, int fd) { struct pollfd *pfd = (struct pollfd *)(ps + 1); pfd[ps->nfds].fd = fd; pfd[ps->nfds].events = POLLERR | POLLIN; ps->nfds++; } static int pollset_wait(struct pollset *ps, int ms) { struct pollfd *pfd = (struct pollfd *)(ps + 1); int rc; int i; rc = poll(pfd, ps->nfds, ms); if (rc <= 0) return rc; ps->cause = -1; for (i = 0; i < ps->nfds; ++i) { if (pfd[i].revents & (POLLERR | POLLIN)) { ps->cause = i; break; } } return rc; } static int pollset_cause_fd(struct pollset *ps, int fd) { struct pollfd *pfd = (struct pollfd *)(ps + 1); return (ps->cause >= 0 && pfd[ps->cause].fd == fd); } enum waiter_type { WATCH_TYPE_NULL, WATCH_TYPE_FD, WATCH_TYPE_TIMEOUT, }; struct waiter_ticket { enum waiter_type type; union { int filedes; unsigned int event; unsigned int interval; }; struct { void (* fn)(void *data, struct waiter_ticket *); void *data; } callback; uint64_t start; int updated; struct waiter *waiter; struct list_item list_item; }; struct waiter { struct list tickets; struct pollset *pollset; int count; }; struct waiter *waiter_create(void) { struct waiter *w; w = calloc(1, sizeof(*w)); if (w == NULL) return NULL; list_init(&w->tickets); return w; } void waiter_destroy(struct waiter *w) { struct waiter_ticket *ticket; struct list_item *safe; struct list_item *node; list_for_each_safe(&w->tickets, node, safe) { ticket = list_entry(node, struct waiter_ticket, list_item); free(ticket); } if (w->pollset) pollset_destroy(w->pollset); free(w); } void waiter_synchronize(struct waiter *w) { struct waiter_ticket *oticket; struct waiter_ticket *ticket; struct list_item *node; list_for_each(&w->tickets, node) { struct list_item *onode; ticket = list_entry(node, struct waiter_ticket, list_item); if (ticket->type != WATCH_TYPE_TIMEOUT) continue; list_for_each_after(node, onode) { oticket = list_entry(onode, struct waiter_ticket, list_item); if (oticket->type != WATCH_TYPE_TIMEOUT) continue; if (oticket->interval == ticket->interval) { oticket->start = ticket->start; break; } } } } void waiter_wait(struct waiter *w) { struct pollset *ps = w->pollset; struct waiter_ticket *ticket; struct list_item *node; uint64_t term_time; uint64_t now; int rc; pollset_reset(ps); term_time = (uint64_t)-1; list_for_each(&w->tickets, node) { ticket = list_entry(node, struct waiter_ticket, list_item); switch (ticket->type) { case WATCH_TYPE_TIMEOUT: if (ticket->start + ticket->interval < term_time) term_time = ticket->start + ticket->interval; break; case WATCH_TYPE_FD: pollset_add_fd(ps, ticket->filedes); break; case WATCH_TYPE_NULL: break; } } if (term_time == (uint64_t)-1) { /* wait forever */ rc = pollset_wait(ps, -1); } else { now = time_ms(); if (now >= term_time) { /* already past timeout, skip poll */ rc = 0; } else { uint64_t delta; delta = term_time - now; if (delta > ((1u << 31) - 1)) delta = ((1u << 31) - 1); rc = pollset_wait(ps, (int)delta); } } if (rc < 0) return; now = time_ms(); list_for_each(&w->tickets, node) { int fresh = 0; ticket = list_entry(node, struct waiter_ticket, list_item); switch (ticket->type) { case WATCH_TYPE_TIMEOUT: if (now >= ticket->start + ticket->interval) { ticket->start = now; fresh = !ticket->updated; } break; case WATCH_TYPE_FD: if (rc == 0) /* timed-out */ break; if (pollset_cause_fd(ps, ticket->filedes)) fresh = !ticket->updated; break; case WATCH_TYPE_NULL: break; } if (fresh) { ticket->updated = 1; if (ticket->callback.fn) (* ticket->callback.fn)( ticket->callback.data, ticket ); } } } int waiter_wait_timeout(struct waiter *w, unsigned int ms) { struct waiter_ticket ticket; int rc; memset(&ticket, 0, sizeof(ticket)); waiter_ticket_set_timeout(&ticket, ms); list_append(&w->tickets, &ticket.list_item); w->count++; waiter_wait(w); rc = waiter_ticket_check(&ticket); list_remove(&w->tickets, &ticket.list_item); w->count--; return -!rc; } void waiter_ticket_set_null(struct waiter_ticket *ticket) { ticket->type = WATCH_TYPE_NULL; } void waiter_ticket_set_fd(struct waiter_ticket *ticket, int fd) { ticket->type = WATCH_TYPE_FD; ticket->filedes = fd; } void waiter_ticket_set_timeout(struct waiter_ticket *ticket, unsigned int ms) { ticket->type = WATCH_TYPE_TIMEOUT; ticket->interval = ms; ticket->start = time_ms(); } struct waiter_ticket *waiter_add_null(struct waiter *w) { struct waiter_ticket *ticket; ticket = calloc(1, sizeof(*ticket)); if (ticket == NULL) return NULL; ticket->waiter = w; list_append(&w->tickets, &ticket->list_item); if ((w->count % 32) == 0) { if (w->pollset) pollset_destroy(w->pollset); w->pollset = pollset_create(w->count + 33); if (w->pollset == NULL) return NULL; } w->count++; waiter_ticket_set_null(ticket); return ticket; } struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd) { struct waiter_ticket *ticket; ticket = waiter_add_null(w); if (ticket == NULL) return NULL; waiter_ticket_set_fd(ticket, fd); return ticket; } struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms) { struct waiter_ticket *ticket; ticket = waiter_add_null(w); if (ticket == NULL) return NULL; waiter_ticket_set_timeout(ticket, ms); return ticket; } void waiter_ticket_delete(struct waiter_ticket *ticket) { struct waiter *w = ticket->waiter; list_remove(&w->tickets, &ticket->list_item); w->count--; free(ticket); } void waiter_ticket_callback(struct waiter_ticket *ticket, waiter_ticket_cb_t cb_fn, void *data) { ticket->callback.fn = cb_fn; ticket->callback.data = data; } int waiter_ticket_check(const struct waiter_ticket *ticket) { return -(ticket->updated == 0); } int waiter_ticket_clear(struct waiter_ticket *ticket) { int ret; ret = waiter_ticket_check(ticket); ticket->updated = 0; return ret; } qrtr-1.1/src/waiter.h000066400000000000000000000051311460530201100145660ustar00rootroot00000000000000#ifndef _WAITER_H_ #define _WAITER_H_ /** Waiter type. */ struct waiter; /** Create a new waiter. * @return Newly created waiter on success, NULL on failure. */ struct waiter *waiter_create(void); /** Destroy existing waiter. * @param w waiter to destroy. */ void waiter_destroy(struct waiter *w); /** Wait for next ticket. * @param w waiter. */ void waiter_wait(struct waiter *w); /** Wait for next ticket or timeout. * @param w waiter. * @param ms timeout in milliseconds. * @return 0 on ticket; !0 on timeout. */ int waiter_wait_timeout(struct waiter *w, unsigned int ms); /** Synchronize timer-based tickets. * @param w waiter. */ void waiter_synchronize(struct waiter *w); /** Waiter ticket type. */ struct waiter_ticket; /** Add a null wait ticket to pool. * @param w waiter * @return wait ticket on success; NULL on failure. */ struct waiter_ticket *waiter_add_null(struct waiter *w); /** Add a file descriptor to the pool. * @param w waiter. * @param fd file descriptor. * @return wait ticket on success; NULL on failure. */ struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd); /** Add a timeout to the pool. * @param w waiter. * @param ms duration of timeout in milliseconds. * @return wait ticket on success; NULL on failure. */ struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms); /** Set ticket type to null. * @param tkt wait ticket. */ void waiter_ticket_set_null(struct waiter_ticket *tkt); /** Set ticket type to file descriptor. * @param tkt wait ticket. * @param fd file descriptor. */ void waiter_ticket_set_fd(struct waiter_ticket *tkt, int fd); /** Set ticket type to timeout. * @param tkt wait ticket. * @param ms timeout in milliseconds. */ void waiter_ticket_set_timeout(struct waiter_ticket *tkt, unsigned int ms); /** Destroy ticket. * @param tkt wait ticket. */ void waiter_ticket_delete(struct waiter_ticket *tkt); /** Check to see if ticket has triggered. * @param tkt wait ticket. * @return 0 if triggered, !0 otherwise. */ int waiter_ticket_check(const struct waiter_ticket *tkt); /** Clear ticket trigger status. * @param tkt wait ticket. * @return 0 if triggered, !0 otherwise. */ int waiter_ticket_clear(struct waiter_ticket *tkt); /** Wait ticket callback function type. */ typedef void (* waiter_ticket_cb_t)(void *, struct waiter_ticket *); /** Register callback function for ticket trigger. * @param tkt wait ticket. * @param cb_fn callback function. * @param data private data to pass to callback function. */ void waiter_ticket_callback(struct waiter_ticket *tkt, waiter_ticket_cb_t cb_fn, void *data); #endif