├── .github └── workflows │ ├── ci.yml │ └── codeql-analysis.yml ├── .gitignore ├── Android.bp ├── LICENSE ├── ci ├── alpine.sh ├── archlinux.sh ├── debian.cross-compile.sh ├── debian.i386.sh ├── debian.sh ├── fedora.sh ├── ubuntu.cross-compile.sh └── ubuntu.sh ├── include ├── libqrtr.h ├── logging.h ├── meson.build └── ns.h ├── lib ├── logging.c ├── meson.build ├── qmi.c └── qrtr.c ├── meson.build ├── meson_options.txt ├── qrtr-ns.service.in ├── qrtr.py └── src ├── addr.c ├── addr.h ├── cfg.c ├── hash.c ├── hash.h ├── list.h ├── lookup.c ├── map.c ├── map.h ├── meson.build ├── ns.c ├── util.c ├── util.h ├── waiter.c └── waiter.h /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: GPL-2.0 2 | # 3 | # Copyright (c) 2021 Canonical Ltd. 4 | # Copyright (c) 2023 Linaro Ltd 5 | # Author: Krzysztof Kozlowski 6 | # 7 | # 8 | # Loosely based on: https://github.com/linux-test-project/ltp 9 | # https://github.com/linux-nfc/neard 10 | # 11 | name: "Builds" 12 | on: 13 | pull_request: 14 | push: 15 | schedule: 16 | # Run at 1:01 PM, every Tuesday 17 | - cron: '1 13 * * 2' 18 | workflow_dispatch: 19 | 20 | jobs: 21 | job: 22 | name: Build 23 | runs-on: ubuntu-latest 24 | permissions: 25 | actions: read 26 | contents: read 27 | 28 | strategy: 29 | fail-fast: false 30 | matrix: 31 | arch: [x86-64] 32 | family: [x86-64] 33 | compiler: [gcc, clang] 34 | container: 35 | - archlinux:latest 36 | - debian:testing 37 | - debian:stable 38 | - debian:bookworm 39 | - debian:bullseye 40 | # Fails on configure on GCC and clang (process restrictions?) 41 | # - fedora:rawhide 42 | - fedora:latest 43 | - fedora:39 44 | - fedora:38 45 | - fedora:37 46 | - ubuntu:lunar # EOL 01.2024 47 | - ubuntu:jammy 48 | - ubuntu:focal 49 | # On Ubuntu Bionic the Meson doesn't support feature options 50 | #- ubuntu:bionic 51 | # Meson version on Ubuntu Xenial is really too old 52 | #- ubuntu:xenial 53 | cross_compile: [""] 54 | variant: [""] 55 | nosystemd: [""] 56 | include: 57 | # Alpine (OpenRC) 58 | - container: "alpine:edge" 59 | arch: x86-64 60 | family: x86-64 61 | compiler: gcc 62 | nosystemd: true 63 | 64 | - container: "alpine:latest" 65 | arch: x86-64 66 | family: x86-64 67 | compiler: gcc 68 | nosystemd: true 69 | 70 | # Debian 32-bit builds 71 | - container: "debian:testing" 72 | arch: i386 73 | family: x86 74 | compiler: gcc 75 | cross_compile: i686-linux-gnu 76 | pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig 77 | variant: i386 78 | 79 | - container: "debian:stable" 80 | arch: i386 81 | family: x86 82 | compiler: gcc 83 | cross_compile: i686-linux-gnu 84 | pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig 85 | variant: i386 86 | 87 | - container: "debian:bookworm" 88 | arch: i386 89 | family: x86 90 | compiler: gcc 91 | cross_compile: i686-linux-gnu 92 | pkg_config_path: /usr/lib/i386-linux-gnu/pkgconfig 93 | variant: i386 94 | 95 | # Debian cross compilation builds 96 | - container: "debian:testing" 97 | arch: armhf 98 | family: arm 99 | compiler: arm-linux-gnueabihf-gcc 100 | cross_compile: arm-linux-gnueabihf 101 | pkg_config_path: /usr/lib/arm-linux-gnueabihf/pkgconfig 102 | variant: cross-compile 103 | 104 | - container: "debian:testing" 105 | arch: arm64 106 | family: aarch64 107 | compiler: aarch64-linux-gnu-gcc 108 | cross_compile: aarch64-linux-gnu 109 | pkg_config_path: /usr/lib/aarch64-linux-gnu/pkgconfig 110 | variant: cross-compile 111 | 112 | - container: "debian:testing" 113 | arch: ppc64el 114 | family: ppc64 115 | compiler: powerpc64le-linux-gnu-gcc 116 | cross_compile: powerpc64le-linux-gnu 117 | pkg_config_path: /usr/lib/powerpc64le-linux-gnu/pkgconfig 118 | variant: cross-compile 119 | 120 | - container: "debian:testing" 121 | arch: s390x 122 | family: s390x 123 | compiler: s390x-linux-gnu-gcc 124 | cross_compile: s390x-linux-gnu 125 | pkg_config_path: /usr/lib/s390x-linux-gnu/pkgconfig 126 | variant: cross-compile 127 | 128 | - container: "debian:stable" 129 | arch: armhf 130 | family: arm 131 | compiler: arm-linux-gnueabihf-gcc 132 | cross_compile: arm-linux-gnueabihf 133 | pkg_config_path: /usr/lib/arm-linux-gnueabihf/pkgconfig 134 | variant: cross-compile 135 | 136 | - container: "debian:stable" 137 | arch: arm64 138 | family: aarch64 139 | compiler: aarch64-linux-gnu-gcc 140 | cross_compile: aarch64-linux-gnu 141 | pkg_config_path: /usr/lib/aarch64-linux-gnu/pkgconfig 142 | variant: cross-compile 143 | 144 | - container: "debian:stable" 145 | arch: ppc64el 146 | family: ppc64 147 | compiler: powerpc64le-linux-gnu-gcc 148 | cross_compile: powerpc64le-linux-gnu 149 | pkg_config_path: /usr/lib/powerpc64le-linux-gnu/pkgconfig 150 | variant: cross-compile 151 | 152 | - container: "debian:stable" 153 | arch: s390x 154 | family: s390x 155 | compiler: s390x-linux-gnu-gcc 156 | cross_compile: s390x-linux-gnu 157 | pkg_config_path: /usr/lib/s390x-linux-gnu/pkgconfig 158 | variant: cross-compile 159 | 160 | container: 161 | image: ${{ matrix.container }} 162 | env: 163 | ARCH: ${{ matrix.arch }} 164 | FAMILY: ${{ matrix.family }} 165 | CC: ${{ matrix.compiler }} 166 | CROSS_COMPILE: ${{ matrix.cross_compile }} 167 | PKG_CONFIG_PATH: ${{ matrix.pkg_config_path }} 168 | VARIANT: ${{ matrix.variant }} 169 | 170 | steps: 171 | - name: Show OS 172 | run: cat /etc/os-release 173 | 174 | - name: Show env (matrix settings) 175 | run: | 176 | echo "ARCH: $ARCH" 177 | echo "FAMILY: $FAMILY" 178 | echo "CC: $CC" 179 | echo "CROSS_COMPILE: $CROSS_COMPILE" 180 | echo "VARIANT: $VARIANT" 181 | echo "PKG_CONFIG_PATH: $PKG_CONFIG_PATH" 182 | 183 | - name: Git checkout 184 | uses: actions/checkout@v3 185 | 186 | - name: Install additional packages 187 | run: | 188 | INSTALL=${{ matrix.container }} 189 | INSTALL="${INSTALL%%:*}" 190 | INSTALL="${INSTALL%%/*}" 191 | ./ci/$INSTALL.sh 192 | if [ "$VARIANT" ]; then ./ci/$INSTALL.$VARIANT.sh; fi 193 | 194 | - name: Compiler version 195 | run: $CC --version 196 | 197 | - name: Display environment and Linux version 198 | run: | 199 | test -f /etc/issue && cat /etc/issue 200 | echo "############################################" 201 | lsb_release -a || true 202 | echo "############################################" 203 | cat /usr/include/linux/version.h 204 | echo "############################################" 205 | uname -a 206 | echo "############################################" 207 | cat /proc/cmdline 208 | echo "############################################" 209 | printenv 210 | 211 | # i386 build on x86_64 only requires passing -m32 to CFLAGS & LDFLAGS 212 | - name: Meson init for i386 213 | if: ${{ matrix.variant == 'i386' }} 214 | run: | 215 | mkdir build 216 | CFLAGS="-m32" LDFLAGS="-m32" meson setup --errorlogs --werror . build 217 | 218 | - name: Meson init with cross compile 219 | if: ${{ matrix.variant == 'cross-compile' }} 220 | run: | 221 | # installing systemd (for pkg-config) has personality issues with cross-compilation 222 | # Generate cross compile file (see https://mesonbuild.com/Cross-compilation.html#cross-compilation) 223 | echo "[binaries]" > cross.txt 224 | echo "c = '${CROSS_COMPILE}-gcc'" >> cross.txt 225 | echo "strip = '${CROSS_COMPILE}-strip'" >> cross.txt 226 | # Forcing pkgconfig binary to 'pkg-config' is required for cross build to work 227 | echo "pkgconfig = 'pkg-config'" >> cross.txt 228 | echo "[host_machine]" >> cross.txt 229 | echo "system = 'linux'" >> cross.txt 230 | echo "cpu_family = '${FAMILY}'" >> cross.txt 231 | echo "cpu = '${ARCH}'" >> cross.txt 232 | echo "endian = 'little'" >> cross.txt 233 | echo "[properties]" >> cross.txt 234 | echo "pkg_config_libdir = '${PKG_CONFIG_PATH}'" >> cross.txt 235 | cat cross.txt 236 | mkdir build 237 | meson setup --errorlogs --werror --cross-file cross.txt . build 238 | 239 | - name: Meson init 240 | if: ${{ matrix.variant == '' }} 241 | run: | 242 | mkdir build 243 | meson setup --errorlogs --werror . build 244 | 245 | - name: Compile 246 | run: ninja -C build 247 | 248 | - name: Install 249 | run: ninja -C build install 250 | -------------------------------------------------------------------------------- /.github/workflows/codeql-analysis.yml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: GPL-2.0 2 | # 3 | # Copyright (c) 2021 Canonical Ltd. 4 | # Copyright (c) 2023 Linaro Ltd 5 | # Author: Krzysztof Kozlowski 6 | # 7 | # 8 | name: "CodeQL" 9 | on: [push, pull_request, workflow_dispatch] 10 | 11 | jobs: 12 | analyze: 13 | name: Analyze 14 | runs-on: ubuntu-latest 15 | permissions: 16 | actions: read 17 | contents: read 18 | security-events: write 19 | 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | language: [ 'cpp' ] 24 | 25 | steps: 26 | - name: Checkout repository 27 | uses: actions/checkout@v3 28 | 29 | - name: Initialize CodeQL 30 | uses: github/codeql-action/init@v2 31 | with: 32 | languages: ${{ matrix.language }} 33 | 34 | - name: Install additional packages 35 | run: sudo ./ci/ubuntu.sh 36 | 37 | - name: Autobuild 38 | uses: github/codeql-action/autobuild@v2 39 | 40 | - name: Perform CodeQL Analysis 41 | uses: github/codeql-action/analyze@v2 42 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | out/ 2 | *.so 3 | qrtr-cfg 4 | qrtr-lookup 5 | qrtr-ns 6 | -------------------------------------------------------------------------------- /Android.bp: -------------------------------------------------------------------------------- 1 | cc_library { 2 | name: "libqrtr", 3 | vendor: true, 4 | srcs: [ 5 | "lib/logging.c", 6 | "lib/qrtr.c", 7 | "lib/qmi.c", 8 | ], 9 | cflags: ["-fPIC", "-Wno-error"], 10 | export_include_dirs: ["lib"], 11 | local_include_dirs: ["src"], 12 | } 13 | 14 | cc_binary { 15 | name: "qrtr-ns", 16 | vendor: true, 17 | srcs: [ 18 | "lib/logging.c", 19 | "src/addr.c", 20 | "src/ns.c", 21 | "src/map.c", 22 | "src/hash.c", 23 | "src/waiter.c", 24 | "src/util.c", 25 | ], 26 | cflags: ["-Wno-error"], 27 | local_include_dirs: ["lib"], 28 | } 29 | 30 | cc_binary { 31 | name: "qrtr-cfg", 32 | vendor: true, 33 | srcs: [ 34 | "lib/logging.c", 35 | "src/addr.c", 36 | "src/cfg.c", 37 | ], 38 | cflags: ["-Wno-error"], 39 | local_include_dirs: ["lib"], 40 | } 41 | 42 | cc_binary { 43 | name: "qrtr-lookup", 44 | vendor: true, 45 | srcs: [ 46 | "lib/logging.c", 47 | "src/lookup.c", 48 | "src/util.c", 49 | ], 50 | cflags: ["-Wno-error"], 51 | local_include_dirs: ["lib"], 52 | } 53 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Sony Mobile Communications Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the organization nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /ci/alpine.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2021 Canonical Ltd. 5 | # Copyright (c) 2023 Linaro Ltd 6 | # Author: Krzysztof Kozlowski 7 | # 8 | # 9 | 10 | set -ex 11 | 12 | PKGS_CC="gcc" 13 | case $CC in 14 | clang*) 15 | PKGS_CC="clang" 16 | ;; 17 | esac 18 | 19 | apk add \ 20 | linux-headers \ 21 | meson \ 22 | musl-dev \ 23 | libc-dev \ 24 | $PKGS_CC 25 | 26 | echo "Install finished: $0" 27 | -------------------------------------------------------------------------------- /ci/archlinux.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2021 Canonical Ltd. 5 | # Copyright (c) 2023 Linaro Ltd 6 | # Author: Krzysztof Kozlowski 7 | # 8 | # 9 | 10 | set -ex 11 | 12 | PKGS_CC="gcc" 13 | case $CC in 14 | clang*) 15 | PKGS_CC="clang" 16 | ;; 17 | esac 18 | 19 | pacman -Syu --noconfirm \ 20 | systemd-libs \ 21 | pkgconf \ 22 | meson \ 23 | $PKGS_CC 24 | 25 | echo "Install finished: $0" 26 | -------------------------------------------------------------------------------- /ci/debian.cross-compile.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2018-2020 Petr Vorel 5 | # Copyright (c) 2021 Canonical Ltd. 6 | # Copyright (c) 2023 Linaro Ltd 7 | # Author: Krzysztof Kozlowski 8 | # 9 | # 10 | 11 | set -ex 12 | 13 | if [ -z "$ARCH" ]; then 14 | echo "missing \$ARCH!" >&2 15 | exit 1 16 | fi 17 | 18 | dpkg --add-architecture $ARCH 19 | apt update 20 | 21 | apt install -y --no-install-recommends \ 22 | libc6-dev:${ARCH} \ 23 | gcc-`dpkg-architecture -a ${ARCH} -q DEB_TARGET_GNU_TYPE` 24 | 25 | # Thanks debian 26 | apt install -y --no-install-recommends systemd-dev:${ARCH} -a ${ARCH} || true 27 | 28 | echo "Install finished: $0" 29 | -------------------------------------------------------------------------------- /ci/debian.i386.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2018-2020 Petr Vorel 5 | # Copyright (c) 2021 Canonical Ltd. 6 | # Copyright (c) 2023 Linaro Ltd 7 | # Author: Krzysztof Kozlowski 8 | # 9 | # 10 | 11 | set -ex 12 | 13 | dpkg --add-architecture i386 14 | apt update 15 | 16 | # gcc-multilib are also needed for clang 32-bit builds 17 | PKGS_CC="gcc-multilib" 18 | 19 | apt install -y --no-install-recommends \ 20 | linux-libc-dev:i386 21 | 22 | apt install -y --no-install-recommends \ 23 | $PKGS_CC 24 | 25 | echo "Install finished: $0" 26 | -------------------------------------------------------------------------------- /ci/debian.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2021 Canonical Ltd. 5 | # Copyright (c) 2023 Linaro Ltd 6 | # Author: Krzysztof Kozlowski 7 | # 8 | # 9 | 10 | set -ex 11 | 12 | apt update 13 | 14 | # Some distros (e.g. Ubuntu Hirsute) might pull tzdata which asks questions 15 | export DEBIAN_FRONTEND=noninteractive DEBCONF_NONINTERACTIVE_SEEN=true 16 | 17 | # Choose some random place in Europe 18 | echo "tzdata tzdata/Areas select Europe 19 | tzdata tzdata/Zones/Europe select Berlin 20 | " > /tmp/tzdata-preseed.txt 21 | debconf-set-selections /tmp/tzdata-preseed.txt 22 | 23 | PKGS_CC="build-essential" 24 | case $CC in 25 | clang*) 26 | PKGS_CC="clang" 27 | ;; 28 | esac 29 | 30 | apt install -y --no-install-recommends \ 31 | pkg-config \ 32 | meson \ 33 | systemd \ 34 | libc6-dev \ 35 | $PKGS_CC 36 | 37 | # Thanks debian 38 | apt install -y --no-install-recommends systemd-dev || true 39 | 40 | echo "Install finished: $0" 41 | -------------------------------------------------------------------------------- /ci/fedora.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # SPDX-License-Identifier: GPL-2.0 3 | # 4 | # Copyright (c) 2021 Canonical Ltd. 5 | # Copyright (c) 2023 Linaro Ltd 6 | # Author: Krzysztof Kozlowski 7 | # 8 | # 9 | 10 | set -ex 11 | 12 | PKGS_CC="gcc" 13 | case $CC in 14 | clang*) 15 | PKGS_CC="clang" 16 | ;; 17 | esac 18 | 19 | dnf -y install \ 20 | meson \ 21 | pkg-config \ 22 | libudev-devel \ 23 | systemd-devel \ 24 | systemd-libs \ 25 | $PKGS_CC 26 | 27 | echo "Install finished: $0" 28 | -------------------------------------------------------------------------------- /ci/ubuntu.cross-compile.sh: -------------------------------------------------------------------------------- 1 | debian.cross-compile.sh -------------------------------------------------------------------------------- /ci/ubuntu.sh: -------------------------------------------------------------------------------- 1 | debian.sh -------------------------------------------------------------------------------- /include/libqrtr.h: -------------------------------------------------------------------------------- 1 | #ifndef _QRTR_LIB_H_ 2 | #define _QRTR_LIB_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #ifdef __cplusplus 11 | extern "C" { 12 | #endif 13 | 14 | #ifndef offsetof 15 | #define offsetof(type, md) ((size_t)&((type *)0)->md) 16 | #endif 17 | 18 | #ifndef container_of 19 | #define container_of(ptr, type, member) \ 20 | ((type *)((char *)(ptr) - offsetof(type, member))) 21 | #endif 22 | 23 | #ifndef AF_QIPCRTR 24 | #define AF_QIPCRTR 42 25 | #endif 26 | 27 | struct sockaddr_qrtr; 28 | 29 | struct qrtr_packet { 30 | int type; 31 | 32 | unsigned int node; 33 | unsigned int port; 34 | 35 | unsigned int service; 36 | unsigned int instance; 37 | unsigned int version; 38 | 39 | void *data; 40 | size_t data_len; 41 | }; 42 | 43 | #define DEFINE_QRTR_PACKET(pkt, size) \ 44 | char pkt ## _buf[size]; \ 45 | struct qrtr_packet pkt = { .data = pkt ##_buf, \ 46 | .data_len = sizeof(pkt ##_buf), } 47 | 48 | #define QMI_REQUEST 0 49 | #define QMI_RESPONSE 2 50 | #define QMI_INDICATION 4 51 | 52 | #define QMI_COMMON_TLV_TYPE 0 53 | 54 | enum qmi_elem_type { 55 | QMI_EOTI, 56 | QMI_OPT_FLAG, 57 | QMI_DATA_LEN, 58 | QMI_UNSIGNED_1_BYTE, 59 | QMI_UNSIGNED_2_BYTE, 60 | QMI_UNSIGNED_4_BYTE, 61 | QMI_UNSIGNED_8_BYTE, 62 | QMI_SIGNED_1_BYTE_ENUM, 63 | QMI_SIGNED_2_BYTE_ENUM, 64 | QMI_SIGNED_4_BYTE_ENUM, 65 | QMI_STRUCT, 66 | QMI_STRING, 67 | }; 68 | 69 | enum qmi_array_type { 70 | NO_ARRAY, 71 | STATIC_ARRAY, 72 | VAR_LEN_ARRAY, 73 | }; 74 | 75 | /** 76 | * struct qmi_elem_info - describes how to encode a single QMI element 77 | * @data_type: Data type of this element. 78 | * @elem_len: Array length of this element, if an array. 79 | * @elem_size: Size of a single instance of this data type. 80 | * @array_type: Array type of this element. 81 | * @tlv_type: QMI message specific type to identify which element 82 | * is present in an incoming message. 83 | * @offset: Specifies the offset of the first instance of this 84 | * element in the data structure. 85 | * @ei_array: Null-terminated array of @qmi_elem_info to describe nested 86 | * structures. 87 | */ 88 | struct qmi_elem_info { 89 | enum qmi_elem_type data_type; 90 | uint32_t elem_len; 91 | uint32_t elem_size; 92 | enum qmi_array_type array_type; 93 | uint8_t tlv_type; 94 | size_t offset; 95 | struct qmi_elem_info *ei_array; 96 | }; 97 | 98 | #define QMI_RESULT_SUCCESS_V01 0 99 | #define QMI_RESULT_FAILURE_V01 1 100 | 101 | #define QMI_ERR_NONE_V01 0 102 | #define QMI_ERR_MALFORMED_MSG_V01 1 103 | #define QMI_ERR_NO_MEMORY_V01 2 104 | #define QMI_ERR_INTERNAL_V01 3 105 | #define QMI_ERR_CLIENT_IDS_EXHAUSTED_V01 5 106 | #define QMI_ERR_INVALID_ID_V01 41 107 | #define QMI_ERR_ENCODING_V01 58 108 | #define QMI_ERR_INCOMPATIBLE_STATE_V01 90 109 | #define QMI_ERR_NOT_SUPPORTED_V01 94 110 | 111 | /** 112 | * qmi_response_type_v01 - common response header (decoded) 113 | * @result: result of the transaction 114 | * @error: error value, when @result is QMI_RESULT_FAILURE_V01 115 | */ 116 | struct qmi_response_type_v01 { 117 | uint16_t result; 118 | uint16_t error; 119 | }; 120 | 121 | extern struct qmi_elem_info qmi_response_type_v01_ei[]; 122 | 123 | int qrtr_open(int rport); 124 | void qrtr_close(int sock); 125 | 126 | int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz); 127 | int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port); 128 | int qrtr_recv(int sock, void *buf, unsigned int bsz); 129 | 130 | int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance); 131 | int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance); 132 | 133 | int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance); 134 | int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance); 135 | 136 | int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance); 137 | int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance); 138 | 139 | int qrtr_poll(int sock, unsigned int ms); 140 | 141 | int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len, 142 | const struct sockaddr_qrtr *sq); 143 | 144 | int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id); 145 | int qmi_decode_message(void *c_struct, unsigned int *txn, 146 | const struct qrtr_packet *pkt, 147 | int type, int id, struct qmi_elem_info *ei); 148 | ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id, 149 | int txn_id, const void *c_struct, 150 | struct qmi_elem_info *ei); 151 | 152 | /* Initial kernel header didn't expose these */ 153 | #ifndef QRTR_NODE_BCAST 154 | 155 | #define QRTR_NODE_BCAST 0xffffffffu 156 | #define QRTR_PORT_CTRL 0xfffffffeu 157 | 158 | enum qrtr_pkt_type { 159 | QRTR_TYPE_DATA = 1, 160 | QRTR_TYPE_HELLO = 2, 161 | QRTR_TYPE_BYE = 3, 162 | QRTR_TYPE_NEW_SERVER = 4, 163 | QRTR_TYPE_DEL_SERVER = 5, 164 | QRTR_TYPE_DEL_CLIENT = 6, 165 | QRTR_TYPE_RESUME_TX = 7, 166 | QRTR_TYPE_EXIT = 8, 167 | QRTR_TYPE_PING = 9, 168 | QRTR_TYPE_NEW_LOOKUP = 10, 169 | QRTR_TYPE_DEL_LOOKUP = 11, 170 | }; 171 | 172 | struct qrtr_ctrl_pkt { 173 | __le32 cmd; 174 | 175 | union { 176 | struct { 177 | __le32 service; 178 | __le32 instance; 179 | __le32 node; 180 | __le32 port; 181 | } server; 182 | 183 | struct { 184 | __le32 node; 185 | __le32 port; 186 | } client; 187 | }; 188 | } __attribute__((packed)); 189 | 190 | #endif 191 | 192 | #ifdef __cplusplus 193 | } /* extern "C" */ 194 | #endif 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /include/logging.h: -------------------------------------------------------------------------------- 1 | #ifndef _QRTR_LOGGING_H_ 2 | #define _QRTR_LOGGING_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined(__GNUC__) || defined(__clang__) 9 | #define __PRINTF__(fmt, args) __attribute__((format(__printf__, fmt, args))) 10 | #else 11 | #define __PRINTF__(fmt, args) 12 | #endif 13 | 14 | void qlog_setup(const char *tag, bool use_syslog); 15 | void qlog_set_min_priority(int priority); 16 | 17 | void qlog(int priority, const char *format, ...) __PRINTF__(2, 3); 18 | 19 | #define LOGD(fmt, ...) qlog(LOG_DEBUG, fmt, ##__VA_ARGS__) 20 | 21 | #define LOGW(fmt, ...) qlog(LOG_WARNING, fmt, ##__VA_ARGS__) 22 | #define PLOGW(fmt, ...) \ 23 | qlog(LOG_WARNING, fmt ": %s", ##__VA_ARGS__, strerror(errno)) 24 | 25 | #define LOGE(fmt, ...) qlog(LOG_ERR, fmt, ##__VA_ARGS__) 26 | #define PLOGE(fmt, ...) qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno)) 27 | #define LOGE_AND_EXIT(fmt, ...) do { \ 28 | qlog(LOG_ERR, fmt, ##__VA_ARGS__); \ 29 | exit(1); \ 30 | } while(0) 31 | #define PLOGE_AND_EXIT(fmt, ...) do { \ 32 | qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno)); \ 33 | exit(1); \ 34 | } while(0) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/meson.build: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-3-Clause 2 | 3 | install_headers('libqrtr.h') 4 | -------------------------------------------------------------------------------- /include/ns.h: -------------------------------------------------------------------------------- 1 | #ifndef __NS_H_ 2 | #define __NS_H_ 3 | 4 | #include 5 | #include 6 | 7 | static inline __le32 cpu_to_le32(uint32_t x) { return htole32(x); } 8 | static inline uint32_t le32_to_cpu(__le32 x) { return le32toh(x); } 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /lib/logging.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #define QLOG_BUF_SIZE 512 7 | 8 | static const char default_tag[] = "libqrtr"; 9 | static const char *current_tag = default_tag; 10 | static int min_priority = LOG_INFO; 11 | 12 | static bool logging_to_syslog = false; 13 | 14 | void qlog_setup(const char *tag, bool use_syslog) 15 | { 16 | current_tag = tag; 17 | logging_to_syslog = use_syslog; 18 | 19 | openlog(tag, LOG_PID, LOG_USER); 20 | } 21 | 22 | void qlog_set_min_priority(int priority) 23 | { 24 | if (priority < LOG_EMERG || priority > LOG_DEBUG) 25 | return; 26 | 27 | min_priority = priority; 28 | } 29 | 30 | static const char *get_priority_string(int priority) 31 | { 32 | switch (priority) { 33 | case LOG_EMERG: 34 | return "EMERG"; 35 | case LOG_ALERT: 36 | return "ALERT"; 37 | case LOG_CRIT: 38 | return "CRIT"; 39 | case LOG_ERR: 40 | return "ERROR"; 41 | case LOG_WARNING: 42 | return "WARNING"; 43 | case LOG_NOTICE: 44 | return "NOTICE"; 45 | case LOG_INFO: 46 | return "INFO"; 47 | case LOG_DEBUG: 48 | return "DEBUG"; 49 | } 50 | return ""; 51 | } 52 | 53 | void qlog(int priority, const char *format, ...) 54 | { 55 | va_list ap; 56 | 57 | if (priority > min_priority) 58 | return; 59 | 60 | va_start(ap, format); 61 | 62 | if (logging_to_syslog) { 63 | vsyslog(priority, format, ap); 64 | } else { 65 | char buf[QLOG_BUF_SIZE]; 66 | vsnprintf(buf, QLOG_BUF_SIZE, format, ap); 67 | 68 | fprintf(stderr, "%s %s: %s\n", 69 | get_priority_string(priority), current_tag, buf); 70 | } 71 | 72 | va_end(ap); 73 | } 74 | -------------------------------------------------------------------------------- /lib/meson.build: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-3-Clause 2 | 3 | pkg = import('pkgconfig') 4 | 5 | libqrtr_srcs = ['logging.c', 'qmi.c', 'qrtr.c'] 6 | libqrtr = shared_library('qrtr', 7 | libqrtr_srcs, 8 | version: meson.project_version(), 9 | include_directories : inc, 10 | install: true) 11 | 12 | pkg.generate(libqrtr) 13 | -------------------------------------------------------------------------------- /lib/qmi.c: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2017, The Linux Foundation. All rights reserved. 2 | * Copyright (C) 2017-2018 Linaro Ltd. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above 11 | * copyright notice, this list of conditions and the following 12 | * disclaimer in the documentation and/or other materials provided 13 | * with the distribution. 14 | * * Neither the name of The Linux Foundation nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 19 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 20 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 22 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 25 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 28 | * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "logging.h" 38 | 39 | /** 40 | * qmi_header - wireformat header of QMI messages 41 | * @type: type of message 42 | * @txn_id: transaction id 43 | * @msg_id: message id 44 | * @msg_len: length of message payload following header 45 | */ 46 | struct qmi_header { 47 | uint8_t type; 48 | uint16_t txn_id; 49 | uint16_t msg_id; 50 | uint16_t msg_len; 51 | } __attribute__((packed)); 52 | 53 | 54 | #define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \ 55 | *p_dst++ = type; \ 56 | *p_dst++ = ((uint8_t)((length) & 0xFF)); \ 57 | *p_dst++ = ((uint8_t)(((length) >> 8) & 0xFF)); \ 58 | } while (0) 59 | 60 | #define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \ 61 | *p_type = (uint8_t)*p_src++; \ 62 | *p_length = (uint8_t)*p_src++; \ 63 | *p_length |= ((uint8_t)*p_src) << 8; \ 64 | } while (0) 65 | 66 | #define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \ 67 | do { \ 68 | memcpy(p_dst, p_src, size); \ 69 | p_dst = (uint8_t *)p_dst + size; \ 70 | p_src = (uint8_t *)p_src + size; \ 71 | } while (0) 72 | 73 | #define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \ 74 | do { \ 75 | memcpy(p_dst, p_src, size); \ 76 | p_dst = (uint8_t *)p_dst + size; \ 77 | p_src = (uint8_t *)p_src + size; \ 78 | } while (0) 79 | 80 | #define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \ 81 | encoded_bytes, tlv_len, encode_tlv, rc) \ 82 | do { \ 83 | buf_dst = (uint8_t *)buf_dst + rc; \ 84 | encoded_bytes += rc; \ 85 | tlv_len += rc; \ 86 | temp_si = temp_si + 1; \ 87 | encode_tlv = 1; \ 88 | } while (0) 89 | 90 | #define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \ 91 | do { \ 92 | buf_src = (uint8_t *)buf_src + rc; \ 93 | decoded_bytes += rc; \ 94 | } while (0) 95 | 96 | #define TLV_LEN_SIZE sizeof(uint16_t) 97 | #define TLV_TYPE_SIZE sizeof(uint8_t) 98 | #define OPTIONAL_TLV_TYPE_START 0x10 99 | 100 | static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf, 101 | const void *in_c_struct, uint32_t out_buf_len, 102 | int enc_level); 103 | 104 | static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct, 105 | const void *in_buf, uint32_t in_buf_len, int dec_level); 106 | 107 | /** 108 | * skip_to_next_elem() - Skip to next element in the structure to be encoded 109 | * @ei_array: Struct info describing the element to be skipped. 110 | * @level: Depth level of encoding/decoding to identify nested structures. 111 | * 112 | * This function is used while encoding optional elements. If the flag 113 | * corresponding to an optional element is not set, then encoding the 114 | * optional element can be skipped. This function can be used to perform 115 | * that operation. 116 | * 117 | * Return: struct info of the next element that can be encoded. 118 | */ 119 | static struct qmi_elem_info *skip_to_next_elem(struct qmi_elem_info *ei_array, 120 | int level) 121 | { 122 | struct qmi_elem_info *temp_ei = ei_array; 123 | uint8_t tlv_type; 124 | 125 | if (level > 1) { 126 | temp_ei = temp_ei + 1; 127 | } else { 128 | do { 129 | tlv_type = temp_ei->tlv_type; 130 | temp_ei = temp_ei + 1; 131 | } while (tlv_type == temp_ei->tlv_type); 132 | } 133 | 134 | return temp_ei; 135 | } 136 | 137 | /** 138 | * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message 139 | * @ei_array: Struct info array describing the structure. 140 | * @level: Level to identify the depth of the nested structures. 141 | * 142 | * Return: Expected minimum length of the QMI message or 0 on error. 143 | */ 144 | static int qmi_calc_min_msg_len(struct qmi_elem_info *ei_array, 145 | int level) 146 | { 147 | int min_msg_len = 0; 148 | struct qmi_elem_info *temp_ei = ei_array; 149 | 150 | if (!ei_array) 151 | return min_msg_len; 152 | 153 | while (temp_ei->data_type != QMI_EOTI) { 154 | /* Optional elements do not count in minimum length */ 155 | if (temp_ei->data_type == QMI_OPT_FLAG) { 156 | temp_ei = skip_to_next_elem(temp_ei, level); 157 | continue; 158 | } 159 | 160 | if (temp_ei->data_type == QMI_DATA_LEN) { 161 | min_msg_len += (temp_ei->elem_size == sizeof(uint8_t) ? 162 | sizeof(uint8_t) : sizeof(uint16_t)); 163 | temp_ei++; 164 | continue; 165 | } else if (temp_ei->data_type == QMI_STRUCT) { 166 | min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array, 167 | (level + 1)); 168 | temp_ei++; 169 | } else if (temp_ei->data_type == QMI_STRING) { 170 | if (level > 1) 171 | min_msg_len += temp_ei->elem_len <= 256 ? 172 | sizeof(uint8_t) : sizeof(uint16_t); 173 | min_msg_len += temp_ei->elem_len * temp_ei->elem_size; 174 | temp_ei++; 175 | } else { 176 | min_msg_len += (temp_ei->elem_len * temp_ei->elem_size); 177 | temp_ei++; 178 | } 179 | 180 | /* 181 | * Type & Length info. not prepended for elements in the 182 | * nested structure. 183 | */ 184 | if (level == 1) 185 | min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE); 186 | } 187 | 188 | return min_msg_len; 189 | } 190 | 191 | /** 192 | * qmi_encode_basic_elem() - Encodes elements of basic/primary data type 193 | * @buf_dst: Buffer to store the encoded information. 194 | * @buf_src: Buffer containing the elements to be encoded. 195 | * @elem_len: Number of elements, in the buf_src, to be encoded. 196 | * @elem_size: Size of a single instance of the element to be encoded. 197 | * 198 | * This function encodes the "elem_len" number of data elements, each of 199 | * size "elem_size" bytes from the source buffer "buf_src" and stores the 200 | * encoded information in the destination buffer "buf_dst". The elements are 201 | * of primary data type which include uint8_t - u64 or similar. This 202 | * function returns the number of bytes of encoded information. 203 | * 204 | * Return: The number of bytes of encoded information. 205 | */ 206 | static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src, 207 | uint32_t elem_len, uint32_t elem_size) 208 | { 209 | uint32_t i, rc = 0; 210 | 211 | for (i = 0; i < elem_len; i++) { 212 | QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size); 213 | rc += elem_size; 214 | } 215 | 216 | return rc; 217 | } 218 | 219 | /** 220 | * qmi_encode_struct_elem() - Encodes elements of struct data type 221 | * @ei_array: Struct info array descibing the struct element. 222 | * @buf_dst: Buffer to store the encoded information. 223 | * @buf_src: Buffer containing the elements to be encoded. 224 | * @elem_len: Number of elements, in the buf_src, to be encoded. 225 | * @out_buf_len: Available space in the encode buffer. 226 | * @enc_level: Depth of the nested structure from the main structure. 227 | * 228 | * This function encodes the "elem_len" number of struct elements, each of 229 | * size "ei_array->elem_size" bytes from the source buffer "buf_src" and 230 | * stores the encoded information in the destination buffer "buf_dst". The 231 | * elements are of struct data type which includes any C structure. This 232 | * function returns the number of bytes of encoded information. 233 | * 234 | * Return: The number of bytes of encoded information on success or negative 235 | * errno on error. 236 | */ 237 | static int qmi_encode_struct_elem(struct qmi_elem_info *ei_array, 238 | void *buf_dst, const void *buf_src, 239 | uint32_t elem_len, uint32_t out_buf_len, 240 | int enc_level) 241 | { 242 | int i, rc, encoded_bytes = 0; 243 | struct qmi_elem_info *temp_ei = ei_array; 244 | 245 | for (i = 0; i < elem_len; i++) { 246 | rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src, 247 | out_buf_len - encoded_bytes, enc_level); 248 | if (rc < 0) { 249 | LOGW("%s: STRUCT Encode failure\n", __func__); 250 | return rc; 251 | } 252 | buf_dst = (void*)((char*)buf_dst + rc); 253 | buf_src = (void*)((char*)buf_src + temp_ei->elem_size); 254 | encoded_bytes += rc; 255 | } 256 | 257 | return encoded_bytes; 258 | } 259 | 260 | /** 261 | * qmi_encode_string_elem() - Encodes elements of string data type 262 | * @ei_array: Struct info array descibing the string element. 263 | * @buf_dst: Buffer to store the encoded information. 264 | * @buf_src: Buffer containing the elements to be encoded. 265 | * @out_buf_len: Available space in the encode buffer. 266 | * @enc_level: Depth of the string element from the main structure. 267 | * 268 | * This function encodes a string element of maximum length "ei_array->elem_len" 269 | * bytes from the source buffer "buf_src" and stores the encoded information in 270 | * the destination buffer "buf_dst". This function returns the number of bytes 271 | * of encoded information. 272 | * 273 | * Return: The number of bytes of encoded information on success or negative 274 | * errno on error. 275 | */ 276 | static int qmi_encode_string_elem(struct qmi_elem_info *ei_array, 277 | void *buf_dst, const void *buf_src, 278 | uint32_t out_buf_len, int enc_level) 279 | { 280 | int rc; 281 | int encoded_bytes = 0; 282 | struct qmi_elem_info *temp_ei = ei_array; 283 | uint32_t string_len = 0; 284 | uint32_t string_len_sz = 0; 285 | 286 | string_len = strlen(buf_src); 287 | string_len_sz = temp_ei->elem_len <= 256 ? 288 | sizeof(uint8_t) : sizeof(uint16_t); 289 | if (string_len > temp_ei->elem_len) { 290 | LOGW("%s: String to be encoded is longer - %u > %u\n", 291 | __func__, string_len, temp_ei->elem_len); 292 | return -EINVAL; 293 | } 294 | 295 | if (enc_level == 1) { 296 | if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE > 297 | out_buf_len) { 298 | LOGW("%s: Output len %u > Out Buf len %u\n", 299 | __func__, string_len, out_buf_len); 300 | return -EINVAL; 301 | } 302 | } else { 303 | if (string_len + string_len_sz > out_buf_len) { 304 | LOGW("%s: Output len %u > Out Buf len %u\n", 305 | __func__, string_len, out_buf_len); 306 | return -EINVAL; 307 | } 308 | rc = qmi_encode_basic_elem(buf_dst, &string_len, 309 | 1, string_len_sz); 310 | encoded_bytes += rc; 311 | } 312 | 313 | rc = qmi_encode_basic_elem((void*)((char*)buf_dst + encoded_bytes), buf_src, 314 | string_len, temp_ei->elem_size); 315 | encoded_bytes += rc; 316 | 317 | return encoded_bytes; 318 | } 319 | 320 | /** 321 | * qmi_encode() - Core Encode Function 322 | * @ei_array: Struct info array describing the structure to be encoded. 323 | * @out_buf: Buffer to hold the encoded QMI message. 324 | * @in_c_struct: Pointer to the C structure to be encoded. 325 | * @out_buf_len: Available space in the encode buffer. 326 | * @enc_level: Encode level to indicate the depth of the nested structure, 327 | * within the main structure, being encoded. 328 | * 329 | * Return: The number of bytes of encoded information on success or negative 330 | * errno on error. 331 | */ 332 | static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf, 333 | const void *in_c_struct, uint32_t out_buf_len, 334 | int enc_level) 335 | { 336 | struct qmi_elem_info *temp_ei = ei_array; 337 | uint8_t opt_flag_value = 0; 338 | uint32_t data_len_value = 0, data_len_sz; 339 | uint8_t *buf_dst = (uint8_t *)out_buf; 340 | uint8_t *tlv_pointer; 341 | uint32_t tlv_len; 342 | uint8_t tlv_type; 343 | uint32_t encoded_bytes = 0; 344 | const void *buf_src; 345 | int encode_tlv = 0; 346 | int rc; 347 | 348 | if (!ei_array) 349 | return 0; 350 | 351 | tlv_pointer = buf_dst; 352 | tlv_len = 0; 353 | if (enc_level == 1) 354 | buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE); 355 | 356 | while (temp_ei->data_type != QMI_EOTI) { 357 | buf_src = (void*)((char*)in_c_struct + temp_ei->offset); 358 | tlv_type = temp_ei->tlv_type; 359 | 360 | if (temp_ei->array_type == NO_ARRAY) { 361 | data_len_value = 1; 362 | } else if (temp_ei->array_type == STATIC_ARRAY) { 363 | data_len_value = temp_ei->elem_len; 364 | } else if (data_len_value <= 0 || 365 | temp_ei->elem_len < data_len_value) { 366 | LOGW("%s: Invalid data length\n", __func__); 367 | return -EINVAL; 368 | } 369 | 370 | switch (temp_ei->data_type) { 371 | case QMI_OPT_FLAG: 372 | rc = qmi_encode_basic_elem(&opt_flag_value, buf_src, 373 | 1, sizeof(uint8_t)); 374 | if (opt_flag_value) 375 | temp_ei = temp_ei + 1; 376 | else 377 | temp_ei = skip_to_next_elem(temp_ei, enc_level); 378 | break; 379 | 380 | case QMI_DATA_LEN: 381 | memcpy(&data_len_value, buf_src, temp_ei->elem_size); 382 | data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ? 383 | sizeof(uint8_t) : sizeof(uint16_t); 384 | /* Check to avoid out of range buffer access */ 385 | if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE + 386 | TLV_TYPE_SIZE) > out_buf_len) { 387 | LOGW("%s: Too Small Buffer @DATA_LEN\n", 388 | __func__); 389 | return -EINVAL; 390 | } 391 | rc = qmi_encode_basic_elem(buf_dst, &data_len_value, 392 | 1, data_len_sz); 393 | UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, 394 | encoded_bytes, tlv_len, 395 | encode_tlv, rc); 396 | if (!data_len_value) 397 | temp_ei = skip_to_next_elem(temp_ei, enc_level); 398 | else 399 | encode_tlv = 0; 400 | break; 401 | 402 | case QMI_UNSIGNED_1_BYTE: 403 | case QMI_UNSIGNED_2_BYTE: 404 | case QMI_UNSIGNED_4_BYTE: 405 | case QMI_UNSIGNED_8_BYTE: 406 | case QMI_SIGNED_1_BYTE_ENUM: 407 | case QMI_SIGNED_2_BYTE_ENUM: 408 | case QMI_SIGNED_4_BYTE_ENUM: 409 | /* Check to avoid out of range buffer access */ 410 | if (((data_len_value * temp_ei->elem_size) + 411 | encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) > 412 | out_buf_len) { 413 | LOGW("%s: Too Small Buffer @data_type:%u\n", 414 | __func__, temp_ei->data_type); 415 | return -EINVAL; 416 | } 417 | rc = qmi_encode_basic_elem(buf_dst, buf_src, 418 | data_len_value, 419 | temp_ei->elem_size); 420 | UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, 421 | encoded_bytes, tlv_len, 422 | encode_tlv, rc); 423 | break; 424 | 425 | case QMI_STRUCT: 426 | rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src, 427 | data_len_value, 428 | out_buf_len - encoded_bytes, 429 | enc_level + 1); 430 | if (rc < 0) 431 | return rc; 432 | UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, 433 | encoded_bytes, tlv_len, 434 | encode_tlv, rc); 435 | break; 436 | 437 | case QMI_STRING: 438 | rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src, 439 | out_buf_len - encoded_bytes, 440 | enc_level); 441 | if (rc < 0) 442 | return rc; 443 | UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst, 444 | encoded_bytes, tlv_len, 445 | encode_tlv, rc); 446 | break; 447 | default: 448 | LOGW("%s: Unrecognized data type\n", __func__); 449 | return -EINVAL; 450 | } 451 | 452 | if (encode_tlv && enc_level == 1) { 453 | QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer); 454 | encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE); 455 | tlv_pointer = buf_dst; 456 | tlv_len = 0; 457 | buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE; 458 | encode_tlv = 0; 459 | } 460 | } 461 | 462 | return encoded_bytes; 463 | } 464 | 465 | /** 466 | * qmi_decode_basic_elem() - Decodes elements of basic/primary data type 467 | * @buf_dst: Buffer to store the decoded element. 468 | * @buf_src: Buffer containing the elements in QMI wire format. 469 | * @elem_len: Number of elements to be decoded. 470 | * @elem_size: Size of a single instance of the element to be decoded. 471 | * 472 | * This function decodes the "elem_len" number of elements in QMI wire format, 473 | * each of size "elem_size" bytes from the source buffer "buf_src" and stores 474 | * the decoded elements in the destination buffer "buf_dst". The elements are 475 | * of primary data type which include uint8_t - u64 or similar. This 476 | * function returns the number of bytes of decoded information. 477 | * 478 | * Return: The total size of the decoded data elements, in bytes. 479 | */ 480 | static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src, 481 | uint32_t elem_len, uint32_t elem_size) 482 | { 483 | uint32_t i, rc = 0; 484 | 485 | for (i = 0; i < elem_len; i++) { 486 | QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size); 487 | rc += elem_size; 488 | } 489 | 490 | return rc; 491 | } 492 | 493 | /** 494 | * qmi_decode_struct_elem() - Decodes elements of struct data type 495 | * @ei_array: Struct info array descibing the struct element. 496 | * @buf_dst: Buffer to store the decoded element. 497 | * @buf_src: Buffer containing the elements in QMI wire format. 498 | * @elem_len: Number of elements to be decoded. 499 | * @tlv_len: Total size of the encoded inforation corresponding to 500 | * this struct element. 501 | * @dec_level: Depth of the nested structure from the main structure. 502 | * 503 | * This function decodes the "elem_len" number of elements in QMI wire format, 504 | * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src" 505 | * and stores the decoded elements in the destination buffer "buf_dst". The 506 | * elements are of struct data type which includes any C structure. This 507 | * function returns the number of bytes of decoded information. 508 | * 509 | * Return: The total size of the decoded data elements on success, negative 510 | * errno on error. 511 | */ 512 | static int qmi_decode_struct_elem(struct qmi_elem_info *ei_array, 513 | void *buf_dst, const void *buf_src, 514 | uint32_t elem_len, uint32_t tlv_len, 515 | int dec_level) 516 | { 517 | int i, rc, decoded_bytes = 0; 518 | struct qmi_elem_info *temp_ei = ei_array; 519 | 520 | for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) { 521 | rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src, 522 | tlv_len - decoded_bytes, dec_level); 523 | if (rc < 0) 524 | return rc; 525 | buf_src = (void*)((char*)buf_src + rc); 526 | buf_dst = (void*)((char*)buf_dst + temp_ei->elem_size); 527 | decoded_bytes += rc; 528 | } 529 | 530 | if ((dec_level <= 2 && decoded_bytes != tlv_len) || 531 | (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) { 532 | LOGW("%s: Fault in decoding: dl(%d), db(%d), tl(%u), i(%d), el(%u)\n", 533 | __func__, dec_level, decoded_bytes, tlv_len, 534 | i, elem_len); 535 | return -EFAULT; 536 | } 537 | 538 | return decoded_bytes; 539 | } 540 | 541 | /** 542 | * qmi_decode_string_elem() - Decodes elements of string data type 543 | * @ei_array: Struct info array descibing the string element. 544 | * @buf_dst: Buffer to store the decoded element. 545 | * @buf_src: Buffer containing the elements in QMI wire format. 546 | * @tlv_len: Total size of the encoded inforation corresponding to 547 | * this string element. 548 | * @dec_level: Depth of the string element from the main structure. 549 | * 550 | * This function decodes the string element of maximum length 551 | * "ei_array->elem_len" from the source buffer "buf_src" and puts it into 552 | * the destination buffer "buf_dst". This function returns number of bytes 553 | * decoded from the input buffer. 554 | * 555 | * Return: The total size of the decoded data elements on success, negative 556 | * errno on error. 557 | */ 558 | static int qmi_decode_string_elem(struct qmi_elem_info *ei_array, 559 | void *buf_dst, const void *buf_src, 560 | uint32_t tlv_len, int dec_level) 561 | { 562 | int rc; 563 | int decoded_bytes = 0; 564 | uint32_t string_len = 0; 565 | uint32_t string_len_sz = 0; 566 | struct qmi_elem_info *temp_ei = ei_array; 567 | 568 | if (dec_level == 1) { 569 | string_len = tlv_len; 570 | } else { 571 | string_len_sz = temp_ei->elem_len <= 256 ? 572 | sizeof(uint8_t) : sizeof(uint16_t); 573 | rc = qmi_decode_basic_elem(&string_len, buf_src, 574 | 1, string_len_sz); 575 | decoded_bytes += rc; 576 | } 577 | 578 | if (string_len > temp_ei->elem_len) { 579 | LOGW("%s: String len %u > Max Len %u\n", 580 | __func__, string_len, temp_ei->elem_len); 581 | return -EINVAL; 582 | } else if (string_len > tlv_len) { 583 | LOGW("%s: String len %u > Input Buffer Len %u\n", 584 | __func__, string_len, tlv_len); 585 | return -EFAULT; 586 | } 587 | 588 | rc = qmi_decode_basic_elem(buf_dst, (void*)((char*)buf_src + decoded_bytes), 589 | string_len, temp_ei->elem_size); 590 | *((char *)buf_dst + string_len) = '\0'; 591 | decoded_bytes += rc; 592 | 593 | return decoded_bytes; 594 | } 595 | 596 | /** 597 | * find_ei() - Find element info corresponding to TLV Type 598 | * @ei_array: Struct info array of the message being decoded. 599 | * @type: TLV Type of the element being searched. 600 | * 601 | * Every element that got encoded in the QMI message will have a type 602 | * information associated with it. While decoding the QMI message, 603 | * this function is used to find the struct info regarding the element 604 | * that corresponds to the type being decoded. 605 | * 606 | * Return: Pointer to struct info, if found 607 | */ 608 | static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array, 609 | uint32_t type) 610 | { 611 | struct qmi_elem_info *temp_ei = ei_array; 612 | 613 | while (temp_ei->data_type != QMI_EOTI) { 614 | if (temp_ei->tlv_type == (uint8_t)type) 615 | return temp_ei; 616 | temp_ei = temp_ei + 1; 617 | } 618 | 619 | return NULL; 620 | } 621 | 622 | /** 623 | * qmi_decode() - Core Decode Function 624 | * @ei_array: Struct info array describing the structure to be decoded. 625 | * @out_c_struct: Buffer to hold the decoded C struct 626 | * @in_buf: Buffer containing the QMI message to be decoded 627 | * @in_buf_len: Length of the QMI message to be decoded 628 | * @dec_level: Decode level to indicate the depth of the nested structure, 629 | * within the main structure, being decoded 630 | * 631 | * Return: The number of bytes of decoded information on success, negative 632 | * errno on error. 633 | */ 634 | static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct, 635 | const void *in_buf, uint32_t in_buf_len, 636 | int dec_level) 637 | { 638 | struct qmi_elem_info *temp_ei = ei_array; 639 | uint8_t opt_flag_value = 1; 640 | uint32_t data_len_value = 0, data_len_sz = 0; 641 | uint8_t *buf_dst = out_c_struct; 642 | const uint8_t *tlv_pointer; 643 | uint32_t tlv_len = 0; 644 | uint32_t tlv_type; 645 | uint32_t decoded_bytes = 0; 646 | const void *buf_src = in_buf; 647 | int rc; 648 | 649 | while (decoded_bytes < in_buf_len) { 650 | if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI) 651 | return decoded_bytes; 652 | 653 | if (dec_level == 1) { 654 | tlv_pointer = buf_src; 655 | QMI_ENCDEC_DECODE_TLV(&tlv_type, 656 | &tlv_len, tlv_pointer); 657 | buf_src = (void*)((char*)buf_src + (TLV_TYPE_SIZE + TLV_LEN_SIZE)); 658 | decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE); 659 | temp_ei = find_ei(ei_array, tlv_type); 660 | if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) { 661 | LOGW("%s: Inval element info\n", __func__); 662 | return -EINVAL; 663 | } else if (!temp_ei) { 664 | UPDATE_DECODE_VARIABLES(buf_src, 665 | decoded_bytes, tlv_len); 666 | continue; 667 | } 668 | } else { 669 | /* 670 | * No length information for elements in nested 671 | * structures. So use remaining decodable buffer space. 672 | */ 673 | tlv_len = in_buf_len - decoded_bytes; 674 | } 675 | 676 | buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); 677 | if (temp_ei->data_type == QMI_OPT_FLAG) { 678 | memcpy(buf_dst, &opt_flag_value, sizeof(uint8_t)); 679 | temp_ei = temp_ei + 1; 680 | buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); 681 | } 682 | 683 | if (temp_ei->data_type == QMI_DATA_LEN) { 684 | data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ? 685 | sizeof(uint8_t) : sizeof(uint16_t); 686 | rc = qmi_decode_basic_elem(&data_len_value, buf_src, 687 | 1, data_len_sz); 688 | memcpy(buf_dst, &data_len_value, sizeof(uint32_t)); 689 | temp_ei = temp_ei + 1; 690 | buf_dst = (void*)((char*)out_c_struct + temp_ei->offset); 691 | tlv_len -= data_len_sz; 692 | UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); 693 | } 694 | 695 | if (temp_ei->array_type == NO_ARRAY) { 696 | data_len_value = 1; 697 | } else if (temp_ei->array_type == STATIC_ARRAY) { 698 | data_len_value = temp_ei->elem_len; 699 | } else if (data_len_value > temp_ei->elem_len) { 700 | LOGW("%s: Data len %u > max spec %u\n", 701 | __func__, data_len_value, temp_ei->elem_len); 702 | return -EINVAL; 703 | } 704 | 705 | switch (temp_ei->data_type) { 706 | case QMI_UNSIGNED_1_BYTE: 707 | case QMI_UNSIGNED_2_BYTE: 708 | case QMI_UNSIGNED_4_BYTE: 709 | case QMI_UNSIGNED_8_BYTE: 710 | case QMI_SIGNED_1_BYTE_ENUM: 711 | case QMI_SIGNED_2_BYTE_ENUM: 712 | case QMI_SIGNED_4_BYTE_ENUM: 713 | rc = qmi_decode_basic_elem(buf_dst, buf_src, 714 | data_len_value, 715 | temp_ei->elem_size); 716 | UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); 717 | break; 718 | 719 | case QMI_STRUCT: 720 | rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src, 721 | data_len_value, tlv_len, 722 | dec_level + 1); 723 | if (rc < 0) 724 | return rc; 725 | UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); 726 | break; 727 | 728 | case QMI_STRING: 729 | rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src, 730 | tlv_len, dec_level); 731 | if (rc < 0) 732 | return rc; 733 | UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc); 734 | break; 735 | 736 | default: 737 | LOGW("%s: Unrecognized data type\n", __func__); 738 | return -EINVAL; 739 | } 740 | temp_ei = temp_ei + 1; 741 | } 742 | 743 | return decoded_bytes; 744 | } 745 | 746 | /** 747 | * qmi_encode_message() - Encode C structure as QMI encoded message 748 | * @type: Type of QMI message 749 | * @msg_id: Message ID of the message 750 | * @len: Passed as max length of the message, updated to actual size 751 | * @txn_id: Transaction ID 752 | * @ei: QMI message descriptor 753 | * @c_struct: Reference to structure to encode 754 | * 755 | * Return: Buffer with encoded message, or negative ERR_PTR() on error 756 | */ 757 | ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id, 758 | int txn_id, const void *c_struct, 759 | struct qmi_elem_info *ei) 760 | { 761 | struct qmi_header *hdr = pkt->data; 762 | ssize_t msglen = 0; 763 | int ret; 764 | 765 | /* Check the possibility of a zero length QMI message */ 766 | if (!c_struct) { 767 | ret = qmi_calc_min_msg_len(ei, 1); 768 | if (ret) { 769 | LOGW("%s: Calc. len %d != 0, but NULL c_struct\n", 770 | __func__, ret); 771 | return -EINVAL; 772 | } 773 | } 774 | 775 | if (pkt->data_len < sizeof(*hdr)) 776 | return -EMSGSIZE; 777 | 778 | /* Encode message, if we have a message */ 779 | if (c_struct) { 780 | msglen = qmi_encode(ei, (void*)((char*)pkt->data + sizeof(*hdr)), c_struct, 781 | pkt->data_len - sizeof(*hdr), 1); 782 | if (msglen < 0) 783 | return msglen; 784 | } 785 | 786 | hdr->type = type; 787 | hdr->txn_id = txn_id; 788 | hdr->msg_id = msg_id; 789 | hdr->msg_len = msglen; 790 | 791 | pkt->type = QRTR_TYPE_DATA; 792 | pkt->data_len = sizeof(*hdr) + msglen; 793 | 794 | return pkt->data_len; 795 | } 796 | 797 | int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id) 798 | { 799 | const struct qmi_header *qmi = pkt->data; 800 | 801 | if (qmi->msg_len != pkt->data_len - sizeof(*qmi)) { 802 | LOGW("[RMTFS] Invalid length of incoming qmi request\n"); 803 | return -EINVAL; 804 | } 805 | 806 | *msg_id = qmi->msg_id; 807 | 808 | return 0; 809 | } 810 | 811 | /** 812 | * qmi_decode_message() - Decode QMI encoded message to C structure 813 | * @buf: Buffer with encoded message 814 | * @len: Amount of data in @buf 815 | * @ei: QMI message descriptor 816 | * @c_struct: Reference to structure to decode into 817 | * 818 | * Return: The number of bytes of decoded information on success, negative 819 | * errno on error. 820 | */ 821 | int qmi_decode_message(void *c_struct, unsigned int *txn, 822 | const struct qrtr_packet *pkt, 823 | int type, int id, struct qmi_elem_info *ei) 824 | { 825 | const struct qmi_header *hdr = pkt->data; 826 | 827 | if (!ei) 828 | return -EINVAL; 829 | 830 | if (!c_struct || !pkt->data || !pkt->data_len) 831 | return -EINVAL; 832 | 833 | if (hdr->type != type) 834 | return -EINVAL; 835 | 836 | if (hdr->msg_id != id) 837 | return -EINVAL; 838 | 839 | if (txn) 840 | *txn = hdr->txn_id; 841 | 842 | return qmi_decode(ei, c_struct, (void*)((char*)pkt->data + sizeof(*hdr)), pkt->data_len - sizeof(*hdr), 1); 843 | } 844 | 845 | /* Common header in all QMI responses */ 846 | struct qmi_elem_info qmi_response_type_v01_ei[] = { 847 | { 848 | .data_type = QMI_SIGNED_2_BYTE_ENUM, 849 | .elem_len = 1, 850 | .elem_size = sizeof(uint16_t), 851 | .array_type = NO_ARRAY, 852 | .tlv_type = QMI_COMMON_TLV_TYPE, 853 | .offset = offsetof(struct qmi_response_type_v01, result), 854 | .ei_array = NULL, 855 | }, 856 | { 857 | .data_type = QMI_SIGNED_2_BYTE_ENUM, 858 | .elem_len = 1, 859 | .elem_size = sizeof(uint16_t), 860 | .array_type = NO_ARRAY, 861 | .tlv_type = QMI_COMMON_TLV_TYPE, 862 | .offset = offsetof(struct qmi_response_type_v01, error), 863 | .ei_array = NULL, 864 | }, 865 | { 866 | .data_type = QMI_EOTI, 867 | .elem_len = 0, 868 | .elem_size = 0, 869 | .array_type = NO_ARRAY, 870 | .tlv_type = QMI_COMMON_TLV_TYPE, 871 | .offset = 0, 872 | .ei_array = NULL, 873 | }, 874 | }; 875 | -------------------------------------------------------------------------------- /lib/qrtr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "logging.h" 14 | #include "ns.h" 15 | 16 | static int qrtr_getname(int sock, struct sockaddr_qrtr *sq) 17 | { 18 | socklen_t sl = sizeof(*sq); 19 | int rc; 20 | 21 | rc = getsockname(sock, (void *)sq, &sl); 22 | if (rc) { 23 | PLOGE("getsockname()"); 24 | return -1; 25 | } 26 | 27 | if (sq->sq_family != AF_QIPCRTR || sl != sizeof(*sq)) 28 | return -1; 29 | 30 | return 0; 31 | } 32 | 33 | int qrtr_open(int rport) 34 | { 35 | struct timeval tv; 36 | int sock; 37 | int rc; 38 | 39 | sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); 40 | if (sock < 0) { 41 | PLOGE("socket(AF_QIPCRTR)"); 42 | return -1; 43 | } 44 | 45 | tv.tv_sec = 1; 46 | tv.tv_usec = 0; 47 | 48 | rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); 49 | if (rc) { 50 | PLOGE("setsockopt(SO_RCVTIMEO)"); 51 | goto err; 52 | } 53 | 54 | if (rport != 0) { 55 | struct sockaddr_qrtr sq = {}; 56 | 57 | sq.sq_family = AF_QIPCRTR; 58 | sq.sq_node = 1; 59 | sq.sq_port = rport; 60 | 61 | rc = bind(sock, (void *)&sq, sizeof(sq)); 62 | if (rc < 0) { 63 | PLOGE("bind(%d)", rport); 64 | goto err; 65 | } 66 | } 67 | 68 | return sock; 69 | err: 70 | close(sock); 71 | return -1; 72 | } 73 | 74 | void qrtr_close(int sock) 75 | { 76 | close(sock); 77 | } 78 | 79 | int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz) 80 | { 81 | struct sockaddr_qrtr sq = {}; 82 | int rc; 83 | 84 | sq.sq_family = AF_QIPCRTR; 85 | sq.sq_node = node; 86 | sq.sq_port = port; 87 | 88 | rc = sendto(sock, data, sz, 0, (void *)&sq, sizeof(sq)); 89 | if (rc < 0) { 90 | PLOGE("sendto()"); 91 | return -1; 92 | } 93 | 94 | return 0; 95 | } 96 | 97 | int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance) 98 | { 99 | struct qrtr_ctrl_pkt pkt; 100 | struct sockaddr_qrtr sq; 101 | 102 | if (qrtr_getname(sock, &sq)) 103 | return -1; 104 | 105 | memset(&pkt, 0, sizeof(pkt)); 106 | 107 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); 108 | pkt.server.service = cpu_to_le32(service); 109 | pkt.server.instance = cpu_to_le32(instance << 8 | version); 110 | 111 | return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); 112 | } 113 | 114 | int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance) 115 | { 116 | struct qrtr_ctrl_pkt pkt; 117 | struct sockaddr_qrtr sq; 118 | 119 | if (qrtr_getname(sock, &sq)) 120 | return -1; 121 | 122 | memset(&pkt, 0, sizeof(pkt)); 123 | 124 | pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); 125 | pkt.server.service = cpu_to_le32(service); 126 | pkt.server.instance = cpu_to_le32(instance << 8 | version); 127 | pkt.server.node = cpu_to_le32(sq.sq_node); 128 | pkt.server.port = cpu_to_le32(sq.sq_port); 129 | 130 | return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); 131 | } 132 | 133 | int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance) 134 | { 135 | return qrtr_new_server(sock, service, version, instance); 136 | } 137 | 138 | int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance) 139 | { 140 | return qrtr_remove_server(sock, service, version, instance); 141 | } 142 | 143 | int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance) 144 | { 145 | struct qrtr_ctrl_pkt pkt; 146 | struct sockaddr_qrtr sq; 147 | 148 | if (qrtr_getname(sock, &sq)) 149 | return -1; 150 | 151 | memset(&pkt, 0, sizeof(pkt)); 152 | 153 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP); 154 | pkt.server.service = cpu_to_le32(service); 155 | pkt.server.instance = cpu_to_le32(instance << 8 | version); 156 | 157 | return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); 158 | } 159 | 160 | int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance) 161 | { 162 | struct qrtr_ctrl_pkt pkt; 163 | struct sockaddr_qrtr sq; 164 | 165 | if (qrtr_getname(sock, &sq)) 166 | return -1; 167 | 168 | memset(&pkt, 0, sizeof(pkt)); 169 | 170 | pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_LOOKUP); 171 | pkt.server.service = cpu_to_le32(service); 172 | pkt.server.instance = cpu_to_le32(instance << 8 | version); 173 | pkt.server.node = cpu_to_le32(sq.sq_node); 174 | pkt.server.port = cpu_to_le32(sq.sq_port); 175 | 176 | return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt)); 177 | } 178 | 179 | int qrtr_poll(int sock, unsigned int ms) 180 | { 181 | struct pollfd fds; 182 | 183 | fds.fd = sock; 184 | fds.revents = 0; 185 | fds.events = POLLIN | POLLERR; 186 | 187 | return poll(&fds, 1, ms); 188 | } 189 | 190 | int qrtr_recv(int sock, void *buf, unsigned int bsz) 191 | { 192 | int rc; 193 | 194 | rc = recv(sock, buf, bsz, 0); 195 | if (rc < 0) 196 | PLOGE("recv()"); 197 | return rc; 198 | } 199 | 200 | int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port) 201 | { 202 | struct sockaddr_qrtr sq; 203 | socklen_t sl; 204 | int rc; 205 | 206 | sl = sizeof(sq); 207 | rc = recvfrom(sock, buf, bsz, 0, (void *)&sq, &sl); 208 | if (rc < 0) { 209 | PLOGE("recvfrom()"); 210 | return rc; 211 | } 212 | if (node) 213 | *node = sq.sq_node; 214 | if (port) 215 | *port = sq.sq_port; 216 | return rc; 217 | } 218 | 219 | int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len, 220 | const struct sockaddr_qrtr *sq) 221 | { 222 | const struct qrtr_ctrl_pkt *ctrl = buf; 223 | 224 | if (sq->sq_port == QRTR_PORT_CTRL){ 225 | if (len < sizeof(*ctrl)) 226 | return -EMSGSIZE; 227 | 228 | dest->type = le32_to_cpu(ctrl->cmd); 229 | switch (dest->type) { 230 | case QRTR_TYPE_BYE: 231 | dest->node = le32_to_cpu(ctrl->client.node); 232 | break; 233 | case QRTR_TYPE_DEL_CLIENT: 234 | dest->node = le32_to_cpu(ctrl->client.node); 235 | dest->port = le32_to_cpu(ctrl->client.port); 236 | break; 237 | case QRTR_TYPE_NEW_SERVER: 238 | case QRTR_TYPE_DEL_SERVER: 239 | dest->node = le32_to_cpu(ctrl->server.node); 240 | dest->port = le32_to_cpu(ctrl->server.port); 241 | dest->service = le32_to_cpu(ctrl->server.service); 242 | dest->version = le32_to_cpu(ctrl->server.instance) & 0xff; 243 | dest->instance = le32_to_cpu(ctrl->server.instance) >> 8; 244 | break; 245 | default: 246 | dest->type = 0; 247 | } 248 | } else { 249 | dest->type = QRTR_TYPE_DATA; 250 | dest->node = sq->sq_node; 251 | dest->port = sq->sq_port; 252 | 253 | dest->data = buf; 254 | dest->data_len = len; 255 | } 256 | 257 | return 0; 258 | } 259 | -------------------------------------------------------------------------------- /meson.build: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-3-Clause 2 | 3 | project('qrtr', 4 | 'c', 5 | version: '1.1', 6 | license : [ 'BSD-3-Clause'], 7 | default_options : [ 8 | 'warning_level=1', 9 | 'buildtype=release', 10 | ]) 11 | 12 | prefix = get_option('prefix') 13 | with_qrtr_ns = get_option('qrtr-ns') 14 | 15 | install_systemd_unit = get_option('systemd-service') 16 | systemd = dependency('systemd', required : install_systemd_unit) 17 | if systemd.found() 18 | systemd_system_unit_dir = get_option('systemd-unit-prefix') 19 | if systemd_system_unit_dir == '' 20 | systemd_system_unit_dir = systemd.get_variable( 21 | pkgconfig : 'systemdsystemunitdir', 22 | pkgconfig_define: ['prefix', prefix]) 23 | else 24 | message('Could not resolve systemd dependencies, skipping unit file') 25 | install_systemd_unit = false 26 | endif 27 | endif 28 | 29 | inc = include_directories('include') 30 | subdir('lib') 31 | subdir('include') 32 | subdir('src') 33 | 34 | if systemd.found() and with_qrtr_ns.enabled() 35 | systemd_unit_conf = configuration_data() 36 | systemd_unit_conf.set('prefix', prefix) 37 | configure_file( 38 | input : 'qrtr-ns.service.in', 39 | output : 'qrtr-ns.service', 40 | configuration : systemd_unit_conf, 41 | install_dir : systemd_system_unit_dir) 42 | endif 43 | -------------------------------------------------------------------------------- /meson_options.txt: -------------------------------------------------------------------------------- 1 | option('qrtr-ns', 2 | type: 'feature', 3 | value: 'auto', 4 | description: 'Whether or not to build the qrtr-ns binary' 5 | ) 6 | 7 | option('systemd-unit-prefix', 8 | type: 'string', 9 | description: 'Directory for systemd system unit files' 10 | ) 11 | 12 | option('systemd-service', 13 | type: 'feature', 14 | value: 'auto', 15 | description: 'Whether or not the systemd service should be built' 16 | ) 17 | -------------------------------------------------------------------------------- /qrtr-ns.service.in: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=QIPCRTR Name Service 3 | 4 | [Service] 5 | ExecStart=@prefix@/bin/qrtr-ns -f 1 6 | Restart=always 7 | 8 | [Install] 9 | WantedBy=multi-user.target 10 | -------------------------------------------------------------------------------- /qrtr.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2.7 2 | 3 | import ctypes 4 | import collections 5 | 6 | from ctypes import CDLL, CFUNCTYPE, POINTER, cast, py_object 7 | from ctypes import c_char_p, c_void_p, c_int, pointer 8 | 9 | _qrtr = CDLL("./libqrtr.so") 10 | 11 | class qrtr: 12 | Result = collections.namedtuple('Result', ['service', 'instance', 'addr']) 13 | _cbtype = CFUNCTYPE(None, c_void_p, c_int, c_int, c_int, c_int) 14 | def __init__(self, port=0): 15 | self.sock = _qrtr.qrtr_open(port) 16 | if self.sock < 0: 17 | raise RuntimeError("unable to open qrtr socket") 18 | self.service = None 19 | self._qrtr = _qrtr 20 | 21 | def __del__(self): 22 | self._qrtr.qrtr_close(self.sock) 23 | 24 | def _lookup_list_add(self, ptr, srv, instance, node, port): 25 | res = qrtr.Result(srv, instance, (node, port)) 26 | cast(ptr, POINTER(py_object)).contents.value.append(res) 27 | 28 | def lookup(self, srv, instance=0, ifilter=0): 29 | results = [] 30 | err = _qrtr.qrtr_lookup(self.sock, srv, instance, ifilter, 31 | qrtr._cbtype(self._lookup_list_add), cast(pointer(py_object(results)), c_void_p)) 32 | if err: 33 | raise RuntimeError("query failed") 34 | return results 35 | 36 | def publish(self, service, version, instance): 37 | err = _qrtr.qrtr_publish(self.sock, service, version, instance) 38 | if err: 39 | raise RuntimeError("publish failed") 40 | self.service = (service, version, instance) 41 | 42 | def new_server(self, service, version, instance): 43 | err = _qrtr.qrtr_new_server(self.sock, service, version, instance) 44 | if err: 45 | raise RuntimeError("new_server failed") 46 | self.service = (service, version, instance) 47 | 48 | return self.service 49 | 50 | def remove_server(self, service): 51 | err = _qrtr.qrtr_remove_server(self.sock, *service) 52 | if err: 53 | raise RuntimeError("remove_server failed") 54 | self.service = None 55 | 56 | def new_lookup(self, service, version, instance): 57 | err = _qrtr.qrtr_new_lookup(self.sock, service, version, instance) 58 | if err: 59 | raise RuntimeError("new_lookup failed") 60 | return (service, version, instance) 61 | 62 | def remove_lookup(self, lookup): 63 | err = _qrtr.qrtr_remove_lookup(self.sock, *lookup) 64 | if err: 65 | raise RuntimeError("remove_lookup failed") 66 | 67 | def send(self, addr, data): 68 | node, port = addr 69 | n = _qrtr.qrtr_sendto(self.sock, node, port, c_char_p(data), len(data)) 70 | if n: 71 | raise RuntimeError("sendto failed") 72 | 73 | def recv(self, sz=65536): 74 | buf = ctypes.create_string_buffer(sz) 75 | n = _qrtr.qrtr_recv(self.sock, c_char_p(ctypes.addressof(buf)), sz) 76 | if n <= 0: 77 | raise RuntimeError("recv failed") 78 | return buf[0:n] 79 | 80 | def recvfrom(self, sz=65536): 81 | node = ctypes.c_int() 82 | port = ctypes.c_int() 83 | buf = ctypes.create_string_buffer(sz) 84 | n = _qrtr.qrtr_recvfrom(self.sock, c_char_p(ctypes.addressof(buf)), sz, 85 | ctypes.byref(node), ctypes.byref(port)) 86 | if n <= 0: 87 | raise RuntimeError("recvfrom failed") 88 | return (buf[0:n], (node.value, port.value)) 89 | 90 | def poll(self, tout=0): 91 | return _qrtr.qrtr_poll(self.sock, tout) 92 | 93 | if __name__ == "__main__": 94 | svcs = qrtr().lookup(15) # 15 is the test service 95 | print " service instance addr" 96 | for svc in svcs: 97 | print "% 8d % 8d %s" % (svc.service, svc.instance, svc.addr) 98 | -------------------------------------------------------------------------------- /src/addr.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "logging.h" 14 | 15 | void qrtr_set_address(uint32_t addr) 16 | { 17 | struct { 18 | struct nlmsghdr nh; 19 | struct ifaddrmsg ifa; 20 | char attrbuf[32]; 21 | } req; 22 | struct { 23 | struct nlmsghdr nh; 24 | struct nlmsgerr err; 25 | } resp; 26 | struct sockaddr_qrtr sq; 27 | struct rtattr *rta; 28 | socklen_t sl = sizeof(sq); 29 | int sock; 30 | int ret; 31 | 32 | /* Trigger loading of the qrtr kernel module */ 33 | sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); 34 | if (sock < 0) 35 | PLOGE_AND_EXIT("failed to create AF_QIPCRTR socket"); 36 | 37 | ret = getsockname(sock, (void*)&sq, &sl); 38 | if (ret < 0) 39 | PLOGE_AND_EXIT("getsockname()"); 40 | close(sock); 41 | 42 | /* Skip configuring the address, if it's same as current */ 43 | if (sl == sizeof(sq) && sq.sq_node == addr) 44 | return; 45 | 46 | sock = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); 47 | if (sock < 0) 48 | PLOGE_AND_EXIT("failed to create netlink socket"); 49 | 50 | memset(&req, 0, sizeof(req)); 51 | req.nh.nlmsg_len = NLMSG_SPACE(sizeof(struct ifaddrmsg)); 52 | req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; 53 | req.nh.nlmsg_type = RTM_NEWADDR; 54 | req.ifa.ifa_family = AF_QIPCRTR; 55 | 56 | rta = (struct rtattr *)(((char *) &req) + req.nh.nlmsg_len); 57 | rta->rta_type = IFA_LOCAL; 58 | rta->rta_len = RTA_LENGTH(sizeof(addr)); 59 | memcpy(RTA_DATA(rta), &addr, sizeof(addr)); 60 | 61 | req.nh.nlmsg_len += rta->rta_len; 62 | 63 | ret = send(sock, &req, req.nh.nlmsg_len, 0); 64 | if (ret < 0) 65 | PLOGE_AND_EXIT("failed to send netlink request"); 66 | 67 | ret = recv(sock, &resp, sizeof(resp), 0); 68 | if (ret < 0) 69 | PLOGE_AND_EXIT("failed to receive netlink response"); 70 | 71 | if (resp.nh.nlmsg_type == NLMSG_ERROR && resp.err.error != 0) { 72 | errno = -resp.err.error; 73 | PLOGE_AND_EXIT("failed to configure node id"); 74 | } 75 | 76 | close(sock); 77 | } 78 | -------------------------------------------------------------------------------- /src/addr.h: -------------------------------------------------------------------------------- 1 | #ifndef __ADDR_H_ 2 | #define __ADDR_H_ 3 | 4 | #include 5 | 6 | void qrtr_set_address(uint32_t addr); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /src/cfg.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "addr.h" 17 | #include "logging.h" 18 | 19 | static void usage(const char *progname) 20 | { 21 | fprintf(stderr, "%s \n", progname); 22 | exit(1); 23 | } 24 | 25 | int main(int argc, char **argv) 26 | { 27 | unsigned long addrul; 28 | uint32_t addr; 29 | char *ep; 30 | const char *progname = basename(argv[0]); 31 | 32 | qlog_setup(progname, false); 33 | 34 | if (argc != 2) 35 | usage(progname); 36 | 37 | addrul = strtoul(argv[1], &ep, 10); 38 | if (argv[1][0] == '\0' || *ep != '\0' || addrul >= UINT_MAX) 39 | usage(progname); 40 | addr = addrul; 41 | qrtr_set_address(addr); 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /src/hash.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "hash.h" 3 | 4 | unsigned int hash_mem(const void *data, unsigned int len) 5 | { 6 | unsigned int h; 7 | unsigned int i; 8 | 9 | h = len; 10 | 11 | for (i = 0; i < len; ++i) 12 | h = ((h >> 27) ^ (h << 5)) ^ ((const unsigned char *)data)[i]; 13 | 14 | return h; 15 | } 16 | 17 | unsigned int hash_string(const char *value) 18 | { 19 | return hash_mem(value, strlen(value)); 20 | } 21 | 22 | unsigned int hash_u32(uint32_t value) 23 | { 24 | return value * 2654435761UL; 25 | } 26 | 27 | unsigned int hash_u64(uint64_t value) 28 | { 29 | return hash_u32(value & 0xffffffff) ^ hash_u32(value >> 32); 30 | } 31 | 32 | unsigned int hash_pointer(void *value) 33 | { 34 | if (sizeof(value) == sizeof(uint64_t)) 35 | return hash_u64((long)value); 36 | return hash_u32((long)value); 37 | } 38 | -------------------------------------------------------------------------------- /src/hash.h: -------------------------------------------------------------------------------- 1 | #ifndef _HASH_H_ 2 | #define _HASH_H_ 3 | 4 | #include 5 | 6 | unsigned int hash_mem(const void *data, unsigned int len); 7 | unsigned int hash_string(const char *value); 8 | unsigned int hash_u32(uint32_t value); 9 | unsigned int hash_u64(uint64_t value); 10 | unsigned int hash_pointer(void *value); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /src/list.h: -------------------------------------------------------------------------------- 1 | #ifndef _LIST_H_ 2 | #define _LIST_H_ 3 | 4 | #include 5 | 6 | #ifndef offsetof 7 | #define offsetof(type, md) ((size_t)&((type *)0)->md) 8 | #endif 9 | 10 | #ifndef container_of 11 | #define container_of(ptr, type, member) \ 12 | ((type *)((char *)(ptr) - offsetof(type, member))) 13 | #endif 14 | 15 | struct list_item { 16 | struct list_item *next; 17 | struct list_item *prev; 18 | }; 19 | 20 | struct list { 21 | struct list_item *head; 22 | struct list_item *tail; 23 | }; 24 | 25 | #define LIST_INIT(name) { 0, 0 } 26 | 27 | #define LIST(name) \ 28 | struct list name = LIST_INIT(name) 29 | 30 | #define list_entry(ptr, type, member) \ 31 | container_of(ptr, type, member) 32 | 33 | static inline void list_init(struct list *list) 34 | { 35 | list->head = 0; 36 | list->tail = 0; 37 | } 38 | 39 | static inline void list_append(struct list *list, struct list_item *item) 40 | { 41 | item->next = 0; 42 | item->prev = list->tail; 43 | if (list->tail != 0) 44 | list->tail->next = item; 45 | else 46 | list->head = item; 47 | list->tail = item; 48 | } 49 | 50 | static inline void list_prepend(struct list *list, struct list_item *item) 51 | { 52 | item->prev = 0; 53 | item->next = list->head; 54 | if (list->head == 0) 55 | list->tail = item; 56 | list->head = item; 57 | } 58 | 59 | static inline void list_insert(struct list *list, struct list_item *after, struct list_item *item) 60 | { 61 | if (after == 0) { 62 | list_prepend(list, item); 63 | return; 64 | } 65 | item->prev = after; 66 | item->next = after->next; 67 | after->next = item; 68 | if (item->next) 69 | item->next->prev = item; 70 | if (list->tail == after) 71 | list->tail = item; 72 | } 73 | 74 | static inline void list_remove(struct list *list, struct list_item *item) 75 | { 76 | if (item->next) 77 | item->next->prev = item->prev; 78 | if (list->head == item) { 79 | list->head = item->next; 80 | if (list->head == 0) 81 | list->tail = 0; 82 | } else { 83 | item->prev->next = item->next; 84 | if (list->tail == item) 85 | list->tail = item->prev; 86 | } 87 | item->prev = item->next = 0; 88 | } 89 | 90 | static inline struct list_item *list_pop(struct list *list) 91 | { 92 | struct list_item *item; 93 | item = list->head; 94 | if (item == 0) 95 | return 0; 96 | list_remove(list, item); 97 | return item; 98 | } 99 | 100 | static inline struct list_item *list_last(struct list *list) 101 | { 102 | return list->tail; 103 | } 104 | 105 | static inline struct list_item *list_first(struct list *list) 106 | { 107 | return list->head; 108 | } 109 | 110 | 111 | static inline struct list_item *list_next(struct list_item *item) 112 | { 113 | return item->next; 114 | } 115 | 116 | #define list_push list_append 117 | 118 | #define list_for_each(_list, _iter) \ 119 | for (_iter = (_list)->head; (_iter) != 0; _iter = (_iter)->next) 120 | 121 | #define list_for_each_after(_node, _iter) \ 122 | for (_iter = (_node)->next; (_iter) != 0; _iter = (_iter)->next) 123 | 124 | #define list_for_each_safe(_list, _iter, _bkup) \ 125 | for (_iter = (_list)->head; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup)) 126 | 127 | #define list_for_each_safe_after(_node, _iter, _bkup) \ 128 | for (_iter = (_node)->next; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup)) 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /src/lookup.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "logging.h" 15 | #include "ns.h" 16 | #include "util.h" 17 | 18 | #define DIAG_SERVICE 4097 19 | 20 | static const struct { 21 | unsigned int service; 22 | unsigned int ifilter; 23 | const char *name; 24 | } common_names[] = { 25 | { 0, 0, "Control service" }, 26 | { 1, 0, "Wireless Data Service" }, 27 | { 2, 0, "Device Management Service" }, 28 | { 3, 0, "Network Access Service" }, 29 | { 4, 0, "Quality Of Service service" }, 30 | { 5, 0, "Wireless Messaging Service" }, 31 | { 6, 0, "Position Determination Service" }, 32 | { 7, 0, "Authentication service" }, 33 | { 8, 0, "AT service" }, 34 | { 9, 0, "Voice service" }, 35 | { 10, 0, "Card Application Toolkit service (v2)" }, 36 | { 11, 0, "User Identity Module service" }, 37 | { 12, 0, "Phonebook Management service" }, 38 | { 13, 0, "QCHAT service" }, 39 | { 14, 0, "Remote file system service" }, 40 | { 15, 0, "Test service" }, 41 | { 16, 0, "Location service (~ PDS v2)" }, 42 | { 17, 0, "Specific absorption rate service" }, 43 | { 18, 0, "IMS settings service" }, 44 | { 19, 0, "Analog to digital converter driver service" }, 45 | { 20, 0, "Core sound driver service" }, 46 | { 21, 0, "Modem embedded file system service" }, 47 | { 22, 0, "Time service" }, 48 | { 23, 0, "Thermal sensors service" }, 49 | { 24, 0, "Thermal mitigation device service" }, 50 | { 25, 0, "Service access proxy service" }, 51 | { 26, 0, "Wireless data administrative service" }, 52 | { 27, 0, "TSYNC control service" }, 53 | { 28, 0, "Remote file system access service" }, 54 | { 29, 0, "Circuit switched videotelephony service" }, 55 | { 30, 0, "Qualcomm mobile access point service" }, 56 | { 31, 0, "IMS presence service" }, 57 | { 32, 0, "IMS videotelephony service" }, 58 | { 33, 0, "IMS application service" }, 59 | { 34, 0, "Coexistence service" }, 60 | { 36, 0, "Persistent device configuration service" }, 61 | { 38, 0, "Simultaneous transmit service" }, 62 | { 39, 0, "Bearer independent transport service" }, 63 | { 40, 0, "IMS RTP service" }, 64 | { 41, 0, "RF radiated performance enhancement service" }, 65 | { 42, 0, "Data system determination service" }, 66 | { 43, 0, "Subsystem control service" }, 67 | { 47, 0, "Data Port Mapper service" }, 68 | { 48, 0, "QMI DFS service" }, 69 | { 49, 0, "IPA control service" }, 70 | { 50, 0, "UIMRMT service" }, 71 | { 51, 0, "CoreSight remote tracing service" }, 72 | { 52, 0, "Dynamic Heap Memory Sharing" }, 73 | { 55, 0, "QMI-SLIM service" }, 74 | { 56, 0, "LOWI (Location) service" }, 75 | { 57, 0, "WLPS service" }, 76 | { 64, 0, "Service registry locator service" }, 77 | { 66, 0, "Service registry notification service" }, 78 | { 69, 0, "ATH10k WLAN firmware service" }, 79 | { 70, 0, "LTE service" }, 80 | { 71, 0, "UIMHTTP service" }, 81 | { 75, 0, "QDMA service" }, 82 | { 76, 0, "SSGCCS service" }, 83 | { 77, 0, "IMS QMI Priv service" }, 84 | { 224, 0, "Card Application Toolkit service (v1)" }, 85 | { 225, 0, "Remote Management Service" }, 86 | { 226, 0, "Open Mobile Alliance device management service" }, 87 | { 231, 0, "Vendor-specific service" }, 88 | { 235, 0, "Modem service" }, 89 | { 256, 0, "Sensor Manager service" }, 90 | { 271, 0, "Sensor Registry service" }, 91 | { 312, 0, "QBT1000 Ultrasonic Fingerprint Sensor service" }, 92 | { 400, 0, "Snapdragon Sensor Core service" }, 93 | { 769, 0, "SLIMbus control service" }, 94 | { 770, 0, "IMS data service" }, 95 | { 771, 0, "Peripheral Access Control Manager service" }, 96 | { 4096, 0, "TFTP" }, 97 | { DIAG_SERVICE, 0, "DIAG service" }, 98 | }; 99 | 100 | static const char *diag_instance_base_str(unsigned int instance_base) 101 | { 102 | switch (instance_base) { 103 | case 0: return "MODEM"; 104 | case 1: return "LPASS"; 105 | case 2: return "WCNSS"; 106 | case 3: return "SENSORS"; 107 | case 4: return "CDSP"; 108 | case 5: return "WDSP"; 109 | default: return ""; 110 | } 111 | } 112 | 113 | static const char *diag_instance_str(unsigned int instance) 114 | { 115 | switch (instance) { 116 | case 0: return "CNTL"; 117 | case 1: return "CMD"; 118 | case 2: return "DATA"; 119 | case 3: return "DCI_CMD"; 120 | case 4: return "DCI"; 121 | default: return ""; 122 | } 123 | } 124 | 125 | static int get_diag_instance_info(char *str, size_t size, unsigned int instance) 126 | { 127 | return snprintf(str, size, "%s:%s", 128 | diag_instance_base_str(instance >> 6), 129 | diag_instance_str(instance & 0x3f)); 130 | } 131 | 132 | static unsigned int read_num_le(const char *str, int *rcp) 133 | { 134 | unsigned int ret; 135 | char *e; 136 | 137 | if (*rcp) 138 | return 0; 139 | 140 | errno = 0; 141 | ret = strtoul(str, &e, 0); 142 | *rcp = -(errno || *e); 143 | 144 | return cpu_to_le32(ret); 145 | } 146 | 147 | int main(int argc, char **argv) 148 | { 149 | struct qrtr_ctrl_pkt pkt; 150 | struct sockaddr_qrtr sq; 151 | unsigned int instance; 152 | unsigned int service; 153 | unsigned int version; 154 | unsigned int node; 155 | unsigned int port; 156 | socklen_t sl = sizeof(sq); 157 | struct timeval tv; 158 | int sock; 159 | int len; 160 | int rc; 161 | const char *progname = basename(argv[0]); 162 | 163 | qlog_setup(progname, false); 164 | 165 | rc = 0; 166 | memset(&pkt, 0, sizeof(pkt)); 167 | 168 | switch (argc) { 169 | default: 170 | rc = -1; 171 | break; 172 | case 3: pkt.server.instance = read_num_le(argv[2], &rc); 173 | case 2: pkt.server.service = read_num_le(argv[1], &rc); 174 | case 1: break; 175 | } 176 | if (rc) { 177 | fprintf(stderr, "Usage: %s [ [ []]]\n", progname); 178 | exit(1); 179 | } 180 | 181 | sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); 182 | if (sock < 0) 183 | PLOGE_AND_EXIT("sock(AF_QIPCRTR)"); 184 | 185 | rc = getsockname(sock, (void *)&sq, &sl); 186 | if (rc || sq.sq_family != AF_QIPCRTR || sl != sizeof(sq)) 187 | PLOGE_AND_EXIT("getsockname()"); 188 | 189 | sq.sq_port = QRTR_PORT_CTRL; 190 | 191 | tv.tv_sec = 1; 192 | tv.tv_usec = 0; 193 | 194 | pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP); 195 | 196 | rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); 197 | if (rc) 198 | PLOGE_AND_EXIT("setsockopt(SO_RCVTIMEO)"); 199 | 200 | rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq)); 201 | if (rc < 0) 202 | PLOGE_AND_EXIT("sendto()"); 203 | 204 | printf(" Service Version Instance Node Port\n"); 205 | 206 | while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) { 207 | unsigned int type = le32_to_cpu(pkt.cmd); 208 | const char *name = NULL; 209 | unsigned int i; 210 | 211 | if (len < sizeof(pkt) || type != QRTR_TYPE_NEW_SERVER) { 212 | PLOGW("invalid/short packet"); 213 | continue; 214 | } 215 | 216 | if (!pkt.server.service && !pkt.server.instance && 217 | !pkt.server.node && !pkt.server.port) 218 | break; 219 | 220 | service = le32_to_cpu(pkt.server.service); 221 | version = le32_to_cpu(pkt.server.instance) & 0xff; 222 | instance = le32_to_cpu(pkt.server.instance) >> 8; 223 | node = le32_to_cpu(pkt.server.node); 224 | port = le32_to_cpu(pkt.server.port); 225 | 226 | for (i = 0; i < sizeof(common_names)/sizeof(common_names[0]); ++i) { 227 | if (service != common_names[i].service) 228 | continue; 229 | if (instance && 230 | (instance & common_names[i].ifilter) != common_names[i].ifilter) 231 | continue; 232 | name = common_names[i].name; 233 | } 234 | if (!name) 235 | name = ""; 236 | 237 | if (service == DIAG_SERVICE) { 238 | char buf[24]; 239 | instance = le32_to_cpu(pkt.server.instance); 240 | get_diag_instance_info(buf, sizeof(buf), instance); 241 | printf("%9u %s %8u %4u %5u %s (%s)\n", 242 | service, "N/A", instance, node, port, name, buf); 243 | } else { 244 | printf("%9u %7u %8u %4u %5u %s\n", 245 | service, version, instance, node, port, name); 246 | } 247 | } 248 | 249 | if (len < 0) 250 | PLOGE_AND_EXIT("recv()"); 251 | 252 | close(sock); 253 | 254 | return 0; 255 | } 256 | -------------------------------------------------------------------------------- /src/map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008-2009, Courtney Cavin 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * 7 | * - Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * 10 | * - Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 18 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | * POSSIBILITY OF SUCH DAMAGE. 25 | */ 26 | 27 | #include 28 | #include "map.h" 29 | 30 | struct map_entry { 31 | struct map_item *item; 32 | }; 33 | 34 | /* Marker for deleted items */ 35 | static struct map_item deleted; 36 | 37 | void map_destroy(struct map *map) 38 | { 39 | free(map->data); 40 | } 41 | 42 | void map_clear(struct map *map, void (*release)(struct map_item *)) 43 | { 44 | int i; 45 | 46 | for (i = 0; i < map->size; ++i){ 47 | if (!map->data[i].item) 48 | continue; 49 | if (map->data[i].item != &deleted) 50 | (* release)(map->data[i].item); 51 | map->data[i].item = NULL; 52 | } 53 | map->count = 0; 54 | } 55 | 56 | int map_create(struct map *map) 57 | { 58 | map->size = 0; 59 | map->data = 0; 60 | map->count = 0; 61 | return 0; 62 | } 63 | 64 | static int map_hash(struct map *map, unsigned int key) 65 | { 66 | struct map_entry *e; 67 | int idx, i; 68 | 69 | if (map->count == map->size) 70 | return -1; 71 | 72 | idx = key % map->size; 73 | 74 | for (i = 0; i < map->size; ++i) { 75 | e = &map->data[idx]; 76 | if (!e->item || e->item == &deleted) { 77 | ++map->count; 78 | return idx; 79 | } 80 | if (e->item->key == key) 81 | return idx; 82 | 83 | idx = (idx + 1) % map->size; 84 | } 85 | 86 | return -2; 87 | } 88 | 89 | static int map_rehash(struct map *map); 90 | 91 | int map_reput(struct map *map, unsigned int key, struct map_item *value, 92 | struct map_item **old) 93 | { 94 | int rc; 95 | 96 | while ((rc = map_hash(map, key)) < 0) { 97 | if ((rc = map_rehash(map)) < 0) 98 | return rc; 99 | } 100 | 101 | if (old) { 102 | if (map->data[rc].item == &deleted) 103 | *old = NULL; 104 | else 105 | *old = map->data[rc].item; 106 | } 107 | map->data[rc].item = value; 108 | if (value) 109 | map->data[rc].item->key = key; 110 | 111 | return 0; 112 | } 113 | 114 | int map_put(struct map *map, unsigned int key, struct map_item *value) 115 | { 116 | return map_reput(map, key, value, NULL); 117 | } 118 | 119 | static int map_rehash(struct map *map) 120 | { 121 | struct map_entry *oldt, *newt; 122 | int o_size, i; 123 | int rc; 124 | 125 | newt = calloc(sizeof(struct map_entry), map->size + 256); 126 | if (!newt) 127 | return -1; 128 | 129 | oldt = map->data; 130 | map->data = newt; 131 | 132 | o_size = map->size; 133 | map->size += 256; 134 | map->count = 0; 135 | 136 | for (i = 0; i < o_size; ++i){ 137 | if (!oldt[i].item || oldt[i].item == &deleted) 138 | continue; 139 | rc = map_put(map, oldt[i].item->key, oldt[i].item); 140 | if (rc < 0) 141 | return rc; 142 | } 143 | 144 | free(oldt); 145 | 146 | return 0; 147 | } 148 | 149 | static struct map_entry *map_find(const struct map *map, unsigned int key) 150 | { 151 | struct map_entry *e; 152 | int idx, i; 153 | 154 | if (map->size == 0) 155 | return NULL; 156 | 157 | idx = key % map->size; 158 | 159 | for (i = 0; i < map->size; ++i) { 160 | e = &map->data[idx]; 161 | idx = (idx + 1) % map->size; 162 | 163 | if (!e->item) 164 | break; 165 | if (e->item == &deleted) 166 | continue; 167 | if (e->item->key == key) 168 | return e; 169 | } 170 | return NULL; 171 | } 172 | 173 | int map_contains(const struct map *map, unsigned int key) 174 | { 175 | return (map_find(map, key) == NULL) ? 0 : 1; 176 | } 177 | 178 | struct map_item *map_get(const struct map *map, unsigned int key) 179 | { 180 | struct map_entry *e; 181 | 182 | e = map_find(map, key); 183 | if (e == NULL) 184 | return NULL; 185 | return e->item; 186 | } 187 | 188 | int map_remove(struct map *map, unsigned int key) 189 | { 190 | struct map_entry *e; 191 | 192 | e = map_find(map, key); 193 | if (e) { 194 | e->item = &deleted; 195 | --map->count; 196 | } 197 | return !e; 198 | } 199 | 200 | unsigned int map_length(struct map *map) 201 | { 202 | return map ? map->count : 0; 203 | } 204 | 205 | static struct map_entry *map_iter_from(const struct map *map, unsigned int start) 206 | { 207 | unsigned int i = start; 208 | 209 | for (; i < map->size; ++i) { 210 | if (map->data[i].item && map->data[i].item != &deleted) 211 | return &map->data[i]; 212 | } 213 | return NULL; 214 | } 215 | 216 | struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter) 217 | { 218 | if (iter == NULL) 219 | return NULL; 220 | 221 | return map_iter_from(map, (iter - map->data) + 1); 222 | } 223 | 224 | struct map_entry *map_iter_first(const struct map *map) 225 | { 226 | return map_iter_from(map, 0); 227 | } 228 | 229 | 230 | struct map_item *map_iter_item(struct map_entry *iter) 231 | { 232 | return iter->item; 233 | } 234 | -------------------------------------------------------------------------------- /src/map.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAP_H_ 2 | #define _MAP_H_ 3 | 4 | struct map_item { 5 | unsigned int key; 6 | }; 7 | 8 | struct map_entry; 9 | 10 | struct map { 11 | unsigned int size; 12 | unsigned int count; 13 | struct map_entry *data; 14 | }; 15 | 16 | int map_create(struct map *map); 17 | void map_destroy(struct map *map); 18 | void map_clear(struct map *map, void (*release)(struct map_item *)); 19 | 20 | int map_put(struct map *map, unsigned int key, struct map_item *v); 21 | int map_reput(struct map *map, unsigned int key, struct map_item *v, 22 | struct map_item **old); 23 | int map_contains(const struct map *map, unsigned int key); 24 | struct map_item *map_get(const struct map *map, unsigned int key); 25 | int map_remove(struct map *map, unsigned int key); 26 | unsigned int map_length(struct map *map); 27 | 28 | struct map_entry *map_iter_first(const struct map *map); 29 | struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter); 30 | struct map_item *map_iter_item(struct map_entry *iter); 31 | 32 | #define map_for_each(map, iter) \ 33 | for (iter = map_iter_first(map); iter; iter = map_iter_next(map, iter)) 34 | 35 | #define map_iter_data(iter, type, member) \ 36 | container_of(map_iter_item(iter), type, member) 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/meson.build: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: BSD-3-Clause 2 | 3 | cfg_srcs = ['addr.c', 4 | 'cfg.c', 5 | 'hash.c'] 6 | executable('qrtr-cfg', 7 | cfg_srcs, 8 | link_with : libqrtr, 9 | include_directories : inc, 10 | install : true) 11 | 12 | if with_qrtr_ns.enabled() 13 | ns_srcs = ['addr.c', 14 | 'hash.c', 15 | 'map.c', 16 | 'ns.c', 17 | 'util.c', 18 | 'waiter.c'] 19 | executable('qrtr-ns', 20 | ns_srcs, 21 | link_with : libqrtr, 22 | include_directories : inc, 23 | install : true) 24 | endif 25 | 26 | executable('qrtr-lookup', 27 | 'lookup.c', 28 | link_with : libqrtr, 29 | include_directories : inc, 30 | install : true) -------------------------------------------------------------------------------- /src/ns.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "addr.h" 19 | #include "hash.h" 20 | #include "list.h" 21 | #include "map.h" 22 | #include "ns.h" 23 | #include "util.h" 24 | #include "waiter.h" 25 | 26 | #include "logging.h" 27 | 28 | static const char *ctrl_pkt_strings[] = { 29 | [QRTR_TYPE_HELLO] = "hello", 30 | [QRTR_TYPE_BYE] = "bye", 31 | [QRTR_TYPE_NEW_SERVER] = "new-server", 32 | [QRTR_TYPE_DEL_SERVER] = "del-server", 33 | [QRTR_TYPE_DEL_CLIENT] = "del-client", 34 | [QRTR_TYPE_RESUME_TX] = "resume-tx", 35 | [QRTR_TYPE_EXIT] = "exit", 36 | [QRTR_TYPE_PING] = "ping", 37 | [QRTR_TYPE_NEW_LOOKUP] = "new-lookup", 38 | [QRTR_TYPE_DEL_LOOKUP] = "del-lookup", 39 | }; 40 | 41 | #define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0])) 42 | 43 | struct context { 44 | int sock; 45 | 46 | int local_node; 47 | 48 | struct sockaddr_qrtr bcast_sq; 49 | 50 | struct list lookups; 51 | }; 52 | 53 | struct server_filter { 54 | unsigned int service; 55 | unsigned int instance; 56 | unsigned int ifilter; 57 | }; 58 | 59 | struct lookup { 60 | unsigned int service; 61 | unsigned int instance; 62 | 63 | struct sockaddr_qrtr sq; 64 | struct list_item li; 65 | }; 66 | 67 | struct server { 68 | unsigned int service; 69 | unsigned int instance; 70 | 71 | unsigned int node; 72 | unsigned int port; 73 | struct map_item mi; 74 | struct list_item qli; 75 | }; 76 | 77 | struct node { 78 | unsigned int id; 79 | 80 | struct map_item mi; 81 | struct map services; 82 | }; 83 | 84 | static struct map nodes; 85 | 86 | static void server_mi_free(struct map_item *mi); 87 | 88 | static struct node *node_get(unsigned int node_id) 89 | { 90 | struct map_item *mi; 91 | struct node *node; 92 | int rc; 93 | 94 | mi = map_get(&nodes, hash_u32(node_id)); 95 | if (mi) 96 | return container_of(mi, struct node, mi); 97 | 98 | node = calloc(1, sizeof(*node)); 99 | if (!node) 100 | return NULL; 101 | 102 | node->id = node_id; 103 | 104 | rc = map_create(&node->services); 105 | if (rc) 106 | LOGE_AND_EXIT("unable to create map"); 107 | 108 | rc = map_put(&nodes, hash_u32(node_id), &node->mi); 109 | if (rc) { 110 | map_destroy(&node->services); 111 | free(node); 112 | return NULL; 113 | } 114 | 115 | return node; 116 | } 117 | 118 | static int server_match(const struct server *srv, const struct server_filter *f) 119 | { 120 | unsigned int ifilter = f->ifilter; 121 | 122 | if (f->service != 0 && srv->service != f->service) 123 | return 0; 124 | if (!ifilter && f->instance) 125 | ifilter = ~0; 126 | return (srv->instance & ifilter) == f->instance; 127 | } 128 | 129 | static int server_query(const struct server_filter *f, struct list *list) 130 | { 131 | struct map_entry *node_me; 132 | struct map_entry *me; 133 | struct node *node; 134 | int count = 0; 135 | 136 | list_init(list); 137 | map_for_each(&nodes, node_me) { 138 | node = map_iter_data(node_me, struct node, mi); 139 | 140 | map_for_each(&node->services, me) { 141 | struct server *srv; 142 | 143 | srv = map_iter_data(me, struct server, mi); 144 | if (!server_match(srv, f)) 145 | continue; 146 | 147 | list_append(list, &srv->qli); 148 | ++count; 149 | } 150 | } 151 | 152 | return count; 153 | } 154 | 155 | static int service_announce_new(struct context *ctx, 156 | struct sockaddr_qrtr *dest, 157 | struct server *srv) 158 | { 159 | struct qrtr_ctrl_pkt cmsg; 160 | int rc; 161 | 162 | LOGD("advertising new server [%u:%x]@[%u:%u]\n", 163 | srv->service, srv->instance, srv->node, srv->port); 164 | 165 | cmsg.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER); 166 | cmsg.server.service = cpu_to_le32(srv->service); 167 | cmsg.server.instance = cpu_to_le32(srv->instance); 168 | cmsg.server.node = cpu_to_le32(srv->node); 169 | cmsg.server.port = cpu_to_le32(srv->port); 170 | 171 | rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0, 172 | (struct sockaddr *)dest, sizeof(*dest)); 173 | if (rc < 0) 174 | PLOGW("sendto()"); 175 | 176 | return rc; 177 | } 178 | 179 | static int service_announce_del(struct context *ctx, 180 | struct sockaddr_qrtr *dest, 181 | struct server *srv) 182 | { 183 | struct qrtr_ctrl_pkt cmsg; 184 | int rc; 185 | 186 | LOGD("advertising removal of server [%u:%x]@[%u:%u]\n", 187 | srv->service, srv->instance, srv->node, srv->port); 188 | 189 | cmsg.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER); 190 | cmsg.server.service = cpu_to_le32(srv->service); 191 | cmsg.server.instance = cpu_to_le32(srv->instance); 192 | cmsg.server.node = cpu_to_le32(srv->node); 193 | cmsg.server.port = cpu_to_le32(srv->port); 194 | 195 | rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0, 196 | (struct sockaddr *)dest, sizeof(*dest)); 197 | if (rc < 0) 198 | PLOGW("sendto()"); 199 | 200 | return rc; 201 | } 202 | 203 | static int lookup_notify(struct context *ctx, struct sockaddr_qrtr *to, 204 | struct server *srv, bool new) 205 | { 206 | struct qrtr_ctrl_pkt pkt = {}; 207 | int rc; 208 | 209 | pkt.cmd = new ? QRTR_TYPE_NEW_SERVER : QRTR_TYPE_DEL_SERVER; 210 | if (srv) { 211 | pkt.server.service = cpu_to_le32(srv->service); 212 | pkt.server.instance = cpu_to_le32(srv->instance); 213 | pkt.server.node = cpu_to_le32(srv->node); 214 | pkt.server.port = cpu_to_le32(srv->port); 215 | } 216 | 217 | rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, 218 | (struct sockaddr *)to, sizeof(*to)); 219 | if (rc < 0) 220 | PLOGW("send lookup result failed"); 221 | return rc; 222 | } 223 | 224 | static int annouce_servers(struct context *ctx, struct sockaddr_qrtr *sq) 225 | { 226 | struct map_entry *me; 227 | struct server *srv; 228 | struct node *node; 229 | int rc; 230 | 231 | node = node_get(ctx->local_node); 232 | if (!node) 233 | return 0; 234 | 235 | map_for_each(&node->services, me) { 236 | srv = map_iter_data(me, struct server, mi); 237 | 238 | rc = service_announce_new(ctx, sq, srv); 239 | if (rc < 0) 240 | return rc; 241 | } 242 | 243 | return 0; 244 | } 245 | 246 | static struct server *server_add(unsigned int service, unsigned int instance, 247 | unsigned int node_id, unsigned int port) 248 | { 249 | struct map_item *mi; 250 | struct server *srv; 251 | struct node *node; 252 | int rc; 253 | 254 | if (!service || !port) 255 | return NULL; 256 | 257 | srv = calloc(1, sizeof(*srv)); 258 | if (srv == NULL) 259 | return NULL; 260 | 261 | srv->service = service; 262 | srv->instance = instance; 263 | srv->node = node_id; 264 | srv->port = port; 265 | 266 | node = node_get(node_id); 267 | if (!node) 268 | goto err; 269 | 270 | rc = map_reput(&node->services, hash_u32(port), &srv->mi, &mi); 271 | if (rc) 272 | goto err; 273 | 274 | LOGD("add server [%u:%x]@[%u:%u]\n", srv->service, srv->instance, 275 | srv->node, srv->port); 276 | 277 | if (mi) { /* we replaced someone */ 278 | struct server *old = container_of(mi, struct server, mi); 279 | free(old); 280 | } 281 | 282 | return srv; 283 | 284 | err: 285 | free(srv); 286 | return NULL; 287 | } 288 | 289 | static int server_del(struct context *ctx, struct node *node, unsigned int port) 290 | { 291 | struct lookup *lookup; 292 | struct list_item *li; 293 | struct map_item *mi; 294 | struct server *srv; 295 | 296 | mi = map_get(&node->services, hash_u32(port)); 297 | if (!mi) 298 | return -ENOENT; 299 | 300 | srv = container_of(mi, struct server, mi); 301 | map_remove(&node->services, srv->mi.key); 302 | 303 | /* Broadcast the removal of local services */ 304 | if (srv->node == ctx->local_node) 305 | service_announce_del(ctx, &ctx->bcast_sq, srv); 306 | 307 | /* Announce the service's disappearance to observers */ 308 | list_for_each(&ctx->lookups, li) { 309 | lookup = container_of(li, struct lookup, li); 310 | if (lookup->service && lookup->service != srv->service) 311 | continue; 312 | if (lookup->instance && lookup->instance != srv->instance) 313 | continue; 314 | 315 | lookup_notify(ctx, &lookup->sq, srv, false); 316 | } 317 | 318 | free(srv); 319 | 320 | return 0; 321 | } 322 | 323 | static int ctrl_cmd_hello(struct context *ctx, struct sockaddr_qrtr *sq, 324 | const void *buf, size_t len) 325 | { 326 | int rc; 327 | 328 | rc = sendto(ctx->sock, buf, len, 0, (void *)sq, sizeof(*sq)); 329 | if (rc > 0) 330 | rc = annouce_servers(ctx, sq); 331 | 332 | return rc; 333 | } 334 | 335 | static int ctrl_cmd_bye(struct context *ctx, struct sockaddr_qrtr *from) 336 | { 337 | struct qrtr_ctrl_pkt pkt; 338 | struct sockaddr_qrtr sq; 339 | struct node *local_node; 340 | struct map_entry *me; 341 | struct server *srv; 342 | struct node *node; 343 | int rc; 344 | 345 | node = node_get(from->sq_node); 346 | if (!node) 347 | return 0; 348 | 349 | map_for_each(&node->services, me) { 350 | srv = map_iter_data(me, struct server, mi); 351 | 352 | server_del(ctx, node, srv->port); 353 | } 354 | 355 | /* Advertise the removal of this client to all local services */ 356 | local_node = node_get(ctx->local_node); 357 | if (!local_node) 358 | return 0; 359 | 360 | memset(&pkt, 0, sizeof(pkt)); 361 | pkt.cmd = QRTR_TYPE_BYE; 362 | pkt.client.node = from->sq_node; 363 | 364 | map_for_each(&local_node->services, me) { 365 | srv = map_iter_data(me, struct server, mi); 366 | 367 | sq.sq_family = AF_QIPCRTR; 368 | sq.sq_node = srv->node; 369 | sq.sq_port = srv->port; 370 | 371 | rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, 372 | (struct sockaddr *)&sq, sizeof(sq)); 373 | if (rc < 0) 374 | PLOGW("bye propagation failed"); 375 | } 376 | 377 | return 0; 378 | } 379 | 380 | static int ctrl_cmd_del_client(struct context *ctx, struct sockaddr_qrtr *from, 381 | unsigned node_id, unsigned port) 382 | { 383 | struct qrtr_ctrl_pkt pkt; 384 | struct sockaddr_qrtr sq; 385 | struct node *local_node; 386 | struct list_item *tmp; 387 | struct lookup *lookup; 388 | struct list_item *li; 389 | struct map_entry *me; 390 | struct server *srv; 391 | struct node *node; 392 | int rc; 393 | 394 | /* Don't accept spoofed messages */ 395 | if (from->sq_node != node_id) 396 | return -EINVAL; 397 | 398 | /* Local DEL_CLIENT messages comes from the port being closed */ 399 | if (from->sq_node == ctx->local_node && from->sq_port != port) 400 | return -EINVAL; 401 | 402 | /* Remove any lookups by this client */ 403 | list_for_each_safe(&ctx->lookups, li, tmp) { 404 | lookup = container_of(li, struct lookup, li); 405 | if (lookup->sq.sq_node != node_id) 406 | continue; 407 | if (lookup->sq.sq_port != port) 408 | continue; 409 | 410 | list_remove(&ctx->lookups, &lookup->li); 411 | free(lookup); 412 | } 413 | 414 | /* Remove the server belonging to this port*/ 415 | node = node_get(node_id); 416 | if (node) 417 | server_del(ctx, node, port); 418 | 419 | /* Advertise the removal of this client to all local services */ 420 | local_node = node_get(ctx->local_node); 421 | if (!local_node) 422 | return 0; 423 | 424 | pkt.cmd = QRTR_TYPE_DEL_CLIENT; 425 | pkt.client.node = node_id; 426 | pkt.client.port = port; 427 | 428 | map_for_each(&local_node->services, me) { 429 | srv = map_iter_data(me, struct server, mi); 430 | 431 | sq.sq_family = AF_QIPCRTR; 432 | sq.sq_node = srv->node; 433 | sq.sq_port = srv->port; 434 | 435 | rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, 436 | (struct sockaddr *)&sq, sizeof(sq)); 437 | if (rc < 0) 438 | PLOGW("del_client propagation failed"); 439 | } 440 | 441 | return 0; 442 | } 443 | 444 | static int ctrl_cmd_new_server(struct context *ctx, struct sockaddr_qrtr *from, 445 | unsigned int service, unsigned int instance, 446 | unsigned int node_id, unsigned int port) 447 | { 448 | struct lookup *lookup; 449 | struct list_item *li; 450 | struct server *srv; 451 | int rc = 0; 452 | 453 | /* Ignore specified node and port for local servers*/ 454 | if (from->sq_node == ctx->local_node) { 455 | node_id = from->sq_node; 456 | port = from->sq_port; 457 | } 458 | 459 | /* Don't accept spoofed messages */ 460 | if (from->sq_node != node_id) 461 | return -EINVAL; 462 | 463 | srv = server_add(service, instance, node_id, port); 464 | if (!srv) 465 | return -EINVAL; 466 | 467 | if (srv->node == ctx->local_node) 468 | rc = service_announce_new(ctx, &ctx->bcast_sq, srv); 469 | 470 | list_for_each(&ctx->lookups, li) { 471 | lookup = container_of(li, struct lookup, li); 472 | if (lookup->service && lookup->service != service) 473 | continue; 474 | if (lookup->instance && lookup->instance != instance) 475 | continue; 476 | 477 | lookup_notify(ctx, &lookup->sq, srv, true); 478 | } 479 | 480 | return rc; 481 | } 482 | 483 | static int ctrl_cmd_del_server(struct context *ctx, struct sockaddr_qrtr *from, 484 | unsigned int service, unsigned int instance, 485 | unsigned int node_id, unsigned int port) 486 | { 487 | struct node *node; 488 | 489 | /* Ignore specified node and port for local servers*/ 490 | if (from->sq_node == ctx->local_node) { 491 | node_id = from->sq_node; 492 | port = from->sq_port; 493 | } 494 | 495 | /* Don't accept spoofed messages */ 496 | if (from->sq_node != node_id) 497 | return -EINVAL; 498 | 499 | /* Local servers may only unregister themselves */ 500 | if (from->sq_node == ctx->local_node && from->sq_port != port) 501 | return -EINVAL; 502 | 503 | node = node_get(node_id); 504 | if (!node) 505 | return -ENOENT; 506 | 507 | return server_del(ctx, node, port); 508 | } 509 | 510 | static int ctrl_cmd_new_lookup(struct context *ctx, struct sockaddr_qrtr *from, 511 | unsigned int service, unsigned int instance) 512 | { 513 | struct server_filter filter; 514 | struct list reply_list; 515 | struct lookup *lookup; 516 | struct list_item *li; 517 | struct server *srv; 518 | 519 | /* Accept only local observers */ 520 | if (from->sq_node != ctx->local_node) 521 | return -EINVAL; 522 | 523 | lookup = calloc(1, sizeof(*lookup)); 524 | if (!lookup) 525 | return -EINVAL; 526 | 527 | lookup->sq = *from; 528 | lookup->service = service; 529 | lookup->instance = instance; 530 | list_append(&ctx->lookups, &lookup->li); 531 | 532 | memset(&filter, 0, sizeof(filter)); 533 | filter.service = service; 534 | filter.instance = instance; 535 | 536 | server_query(&filter, &reply_list); 537 | list_for_each(&reply_list, li) { 538 | srv = container_of(li, struct server, qli); 539 | 540 | lookup_notify(ctx, from, srv, true); 541 | } 542 | 543 | lookup_notify(ctx, from, NULL, true); 544 | 545 | return 0; 546 | } 547 | 548 | static int ctrl_cmd_del_lookup(struct context *ctx, struct sockaddr_qrtr *from, 549 | unsigned int service, unsigned int instance) 550 | { 551 | struct lookup *lookup; 552 | struct list_item *tmp; 553 | struct list_item *li; 554 | 555 | list_for_each_safe(&ctx->lookups, li, tmp) { 556 | lookup = container_of(li, struct lookup, li); 557 | if (lookup->sq.sq_node != from->sq_node) 558 | continue; 559 | if (lookup->sq.sq_port != from->sq_port) 560 | continue; 561 | if (lookup->service != service) 562 | continue; 563 | if (lookup->instance && lookup->instance != instance) 564 | continue; 565 | 566 | list_remove(&ctx->lookups, &lookup->li); 567 | free(lookup); 568 | } 569 | 570 | return 0; 571 | } 572 | 573 | static void ctrl_port_fn(void *vcontext, struct waiter_ticket *tkt) 574 | { 575 | struct context *ctx = vcontext; 576 | struct sockaddr_qrtr sq; 577 | int sock = ctx->sock; 578 | struct qrtr_ctrl_pkt *msg; 579 | unsigned int cmd; 580 | char buf[4096]; 581 | socklen_t sl; 582 | ssize_t len; 583 | int rc; 584 | 585 | sl = sizeof(sq); 586 | len = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl); 587 | if (len <= 0) { 588 | PLOGW("recvfrom()"); 589 | close(sock); 590 | ctx->sock = -1; 591 | goto out; 592 | } 593 | msg = (void *)buf; 594 | 595 | if (len < 4) { 596 | LOGW("short packet from %u:%u", sq.sq_node, sq.sq_port); 597 | goto out; 598 | } 599 | 600 | cmd = le32_to_cpu(msg->cmd); 601 | if (cmd < ARRAY_SIZE(ctrl_pkt_strings) && ctrl_pkt_strings[cmd]) 602 | LOGD("%s from %u:%u\n", ctrl_pkt_strings[cmd], sq.sq_node, sq.sq_port); 603 | else 604 | LOGD("UNK (%08x) from %u:%u\n", cmd, sq.sq_node, sq.sq_port); 605 | 606 | rc = 0; 607 | switch (cmd) { 608 | case QRTR_TYPE_HELLO: 609 | rc = ctrl_cmd_hello(ctx, &sq, buf, len); 610 | break; 611 | case QRTR_TYPE_BYE: 612 | rc = ctrl_cmd_bye(ctx, &sq); 613 | break; 614 | case QRTR_TYPE_DEL_CLIENT: 615 | rc = ctrl_cmd_del_client(ctx, &sq, 616 | le32_to_cpu(msg->client.node), 617 | le32_to_cpu(msg->client.port)); 618 | break; 619 | case QRTR_TYPE_NEW_SERVER: 620 | rc = ctrl_cmd_new_server(ctx, &sq, 621 | le32_to_cpu(msg->server.service), 622 | le32_to_cpu(msg->server.instance), 623 | le32_to_cpu(msg->server.node), 624 | le32_to_cpu(msg->server.port)); 625 | break; 626 | case QRTR_TYPE_DEL_SERVER: 627 | rc = ctrl_cmd_del_server(ctx, &sq, 628 | le32_to_cpu(msg->server.service), 629 | le32_to_cpu(msg->server.instance), 630 | le32_to_cpu(msg->server.node), 631 | le32_to_cpu(msg->server.port)); 632 | break; 633 | case QRTR_TYPE_EXIT: 634 | case QRTR_TYPE_PING: 635 | case QRTR_TYPE_RESUME_TX: 636 | break; 637 | case QRTR_TYPE_NEW_LOOKUP: 638 | rc = ctrl_cmd_new_lookup(ctx, &sq, 639 | le32_to_cpu(msg->server.service), 640 | le32_to_cpu(msg->server.instance)); 641 | break; 642 | case QRTR_TYPE_DEL_LOOKUP: 643 | rc = ctrl_cmd_del_lookup(ctx, &sq, 644 | le32_to_cpu(msg->server.service), 645 | le32_to_cpu(msg->server.instance)); 646 | break; 647 | } 648 | 649 | if (rc < 0) 650 | LOGW("failed while handling packet from %u:%u", 651 | sq.sq_node, sq.sq_port); 652 | out: 653 | waiter_ticket_clear(tkt); 654 | } 655 | 656 | static int say_hello(struct context *ctx) 657 | { 658 | struct qrtr_ctrl_pkt pkt; 659 | int rc; 660 | 661 | memset(&pkt, 0, sizeof(pkt)); 662 | pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO); 663 | 664 | rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0, 665 | (struct sockaddr *)&ctx->bcast_sq, sizeof(ctx->bcast_sq)); 666 | if (rc < 0) 667 | return rc; 668 | 669 | return 0; 670 | } 671 | 672 | static void server_mi_free(struct map_item *mi) 673 | { 674 | free(container_of(mi, struct server, mi)); 675 | } 676 | 677 | static void node_mi_free(struct map_item *mi) 678 | { 679 | struct node *node = container_of(mi, struct node, mi); 680 | 681 | map_clear(&node->services, server_mi_free); 682 | map_destroy(&node->services); 683 | 684 | free(node); 685 | } 686 | 687 | static void go_dormant(int sock) 688 | { 689 | close(sock); 690 | 691 | for (;;) 692 | sleep(UINT_MAX); 693 | } 694 | 695 | static void usage(const char *progname) 696 | { 697 | fprintf(stderr, "%s [-f] [-s] []\n", progname); 698 | exit(1); 699 | } 700 | 701 | int main(int argc, char **argv) 702 | { 703 | struct waiter_ticket *tkt; 704 | struct sockaddr_qrtr sq; 705 | struct context ctx; 706 | unsigned long addr = (unsigned long)-1; 707 | struct waiter *w; 708 | socklen_t sl = sizeof(sq); 709 | bool foreground = false; 710 | bool use_syslog = false; 711 | bool verbose_log = false; 712 | char *ep; 713 | int opt; 714 | int rc; 715 | const char *progname = basename(argv[0]); 716 | 717 | while ((opt = getopt(argc, argv, "fsv")) != -1) { 718 | switch (opt) { 719 | case 'f': 720 | foreground = true; 721 | break; 722 | case 's': 723 | use_syslog = true; 724 | break; 725 | case 'v': 726 | verbose_log = true; 727 | break; 728 | default: 729 | usage(progname); 730 | } 731 | } 732 | 733 | qlog_setup(progname, use_syslog); 734 | if (verbose_log) 735 | qlog_set_min_priority(LOG_DEBUG); 736 | 737 | if (optind < argc) { 738 | addr = strtoul(argv[optind], &ep, 10); 739 | if (argv[1][0] == '\0' || *ep != '\0' || addr >= UINT_MAX) 740 | usage(progname); 741 | 742 | qrtr_set_address(addr); 743 | optind++; 744 | } 745 | 746 | if (optind != argc) 747 | usage(progname); 748 | 749 | w = waiter_create(); 750 | if (w == NULL) 751 | LOGE_AND_EXIT("unable to create waiter"); 752 | 753 | list_init(&ctx.lookups); 754 | 755 | rc = map_create(&nodes); 756 | if (rc) 757 | LOGE_AND_EXIT("unable to create node map"); 758 | 759 | ctx.sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0); 760 | if (ctx.sock < 0) 761 | PLOGE_AND_EXIT("unable to create control socket"); 762 | 763 | rc = getsockname(ctx.sock, (void*)&sq, &sl); 764 | if (rc < 0) 765 | PLOGE_AND_EXIT("getsockname()"); 766 | sq.sq_port = QRTR_PORT_CTRL; 767 | ctx.local_node = sq.sq_node; 768 | 769 | rc = bind(ctx.sock, (void *)&sq, sizeof(sq)); 770 | if (rc < 0) { 771 | if (errno == EADDRINUSE) { 772 | PLOGE("nameserver already running, going dormant"); 773 | go_dormant(ctx.sock); 774 | } 775 | 776 | PLOGE_AND_EXIT("bind control socket"); 777 | } 778 | 779 | ctx.bcast_sq.sq_family = AF_QIPCRTR; 780 | ctx.bcast_sq.sq_node = QRTR_NODE_BCAST; 781 | ctx.bcast_sq.sq_port = QRTR_PORT_CTRL; 782 | 783 | rc = say_hello(&ctx); 784 | if (rc) 785 | PLOGE_AND_EXIT("unable to say hello"); 786 | 787 | /* If we're going to background, fork and exit parent */ 788 | if (!foreground && fork() != 0) { 789 | close(ctx.sock); 790 | exit(0); 791 | } 792 | 793 | tkt = waiter_add_fd(w, ctx.sock); 794 | waiter_ticket_callback(tkt, ctrl_port_fn, &ctx); 795 | 796 | while (ctx.sock >= 0) 797 | waiter_wait(w); 798 | 799 | puts("exiting cleanly"); 800 | 801 | waiter_destroy(w); 802 | 803 | map_clear(&nodes, node_mi_free); 804 | map_destroy(&nodes); 805 | 806 | return 0; 807 | } 808 | -------------------------------------------------------------------------------- /src/util.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "util.h" 7 | 8 | uint64_t time_ms(void) 9 | { 10 | struct timeval tv; 11 | gettimeofday(&tv, NULL); 12 | return (uint64_t)tv.tv_sec*1000 + tv.tv_usec/1000; 13 | } 14 | 15 | void util_sleep(int ms) 16 | { 17 | usleep(ms * 1000); 18 | } 19 | -------------------------------------------------------------------------------- /src/util.h: -------------------------------------------------------------------------------- 1 | #ifndef __UTIL_H_ 2 | #define __UTIL_H_ 3 | 4 | #include 5 | 6 | uint64_t time_ms(void); 7 | void util_sleep(int ms); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /src/waiter.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013-2014, Sony Mobile Communications Inc. 3 | * Copyright (c) 2014, Courtney Cavin 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * - Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * - Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * - Neither the name of the organization nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "list.h" 38 | #include "waiter.h" 39 | #include "util.h" 40 | 41 | struct pollset { 42 | int nfds; 43 | int cause; 44 | }; 45 | 46 | static struct pollset *pollset_create(int count) 47 | { 48 | struct pollset *ps; 49 | 50 | ps = calloc(1, sizeof(*ps) + sizeof(struct pollfd) * count); 51 | if (ps == NULL) 52 | return NULL; 53 | 54 | return ps; 55 | } 56 | 57 | static void pollset_destroy(struct pollset *ps) 58 | { 59 | free(ps); 60 | } 61 | 62 | static void pollset_reset(struct pollset *ps) 63 | { 64 | ps->nfds = 0; 65 | } 66 | 67 | static void pollset_add_fd(struct pollset *ps, int fd) 68 | { 69 | struct pollfd *pfd = (struct pollfd *)(ps + 1); 70 | pfd[ps->nfds].fd = fd; 71 | pfd[ps->nfds].events = POLLERR | POLLIN; 72 | ps->nfds++; 73 | } 74 | 75 | static int pollset_wait(struct pollset *ps, int ms) 76 | { 77 | struct pollfd *pfd = (struct pollfd *)(ps + 1); 78 | int rc; 79 | int i; 80 | 81 | rc = poll(pfd, ps->nfds, ms); 82 | if (rc <= 0) 83 | return rc; 84 | 85 | ps->cause = -1; 86 | for (i = 0; i < ps->nfds; ++i) { 87 | if (pfd[i].revents & (POLLERR | POLLIN)) { 88 | ps->cause = i; 89 | break; 90 | } 91 | } 92 | return rc; 93 | 94 | } 95 | 96 | static int pollset_cause_fd(struct pollset *ps, int fd) 97 | { 98 | struct pollfd *pfd = (struct pollfd *)(ps + 1); 99 | return (ps->cause >= 0 && pfd[ps->cause].fd == fd); 100 | } 101 | 102 | enum waiter_type { 103 | WATCH_TYPE_NULL, 104 | WATCH_TYPE_FD, 105 | WATCH_TYPE_TIMEOUT, 106 | }; 107 | 108 | struct waiter_ticket { 109 | enum waiter_type type; 110 | union { 111 | int filedes; 112 | unsigned int event; 113 | unsigned int interval; 114 | }; 115 | struct { 116 | void (* fn)(void *data, struct waiter_ticket *); 117 | void *data; 118 | } callback; 119 | 120 | uint64_t start; 121 | int updated; 122 | struct waiter *waiter; 123 | struct list_item list_item; 124 | }; 125 | 126 | struct waiter { 127 | struct list tickets; 128 | struct pollset *pollset; 129 | int count; 130 | }; 131 | 132 | struct waiter *waiter_create(void) 133 | { 134 | struct waiter *w; 135 | 136 | w = calloc(1, sizeof(*w)); 137 | if (w == NULL) 138 | return NULL; 139 | 140 | list_init(&w->tickets); 141 | return w; 142 | } 143 | 144 | void waiter_destroy(struct waiter *w) 145 | { 146 | struct waiter_ticket *ticket; 147 | struct list_item *safe; 148 | struct list_item *node; 149 | 150 | list_for_each_safe(&w->tickets, node, safe) { 151 | ticket = list_entry(node, struct waiter_ticket, list_item); 152 | free(ticket); 153 | } 154 | 155 | if (w->pollset) 156 | pollset_destroy(w->pollset); 157 | free(w); 158 | } 159 | 160 | void waiter_synchronize(struct waiter *w) 161 | { 162 | struct waiter_ticket *oticket; 163 | struct waiter_ticket *ticket; 164 | struct list_item *node; 165 | 166 | list_for_each(&w->tickets, node) { 167 | struct list_item *onode; 168 | ticket = list_entry(node, struct waiter_ticket, list_item); 169 | 170 | if (ticket->type != WATCH_TYPE_TIMEOUT) 171 | continue; 172 | 173 | list_for_each_after(node, onode) { 174 | oticket = list_entry(onode, struct waiter_ticket, list_item); 175 | if (oticket->type != WATCH_TYPE_TIMEOUT) 176 | continue; 177 | 178 | if (oticket->interval == ticket->interval) { 179 | oticket->start = ticket->start; 180 | break; 181 | } 182 | } 183 | } 184 | } 185 | 186 | void waiter_wait(struct waiter *w) 187 | { 188 | struct pollset *ps = w->pollset; 189 | struct waiter_ticket *ticket; 190 | struct list_item *node; 191 | uint64_t term_time; 192 | uint64_t now; 193 | int rc; 194 | 195 | pollset_reset(ps); 196 | 197 | term_time = (uint64_t)-1; 198 | list_for_each(&w->tickets, node) { 199 | ticket = list_entry(node, struct waiter_ticket, list_item); 200 | switch (ticket->type) { 201 | case WATCH_TYPE_TIMEOUT: 202 | if (ticket->start + ticket->interval < term_time) 203 | term_time = ticket->start + ticket->interval; 204 | break; 205 | case WATCH_TYPE_FD: 206 | pollset_add_fd(ps, ticket->filedes); 207 | break; 208 | case WATCH_TYPE_NULL: 209 | break; 210 | } 211 | } 212 | 213 | if (term_time == (uint64_t)-1) { /* wait forever */ 214 | rc = pollset_wait(ps, -1); 215 | } else { 216 | now = time_ms(); 217 | if (now >= term_time) { /* already past timeout, skip poll */ 218 | rc = 0; 219 | } else { 220 | uint64_t delta; 221 | 222 | delta = term_time - now; 223 | if (delta > ((1u << 31) - 1)) 224 | delta = ((1u << 31) - 1); 225 | rc = pollset_wait(ps, (int)delta); 226 | } 227 | } 228 | 229 | if (rc < 0) 230 | return; 231 | 232 | now = time_ms(); 233 | list_for_each(&w->tickets, node) { 234 | int fresh = 0; 235 | 236 | ticket = list_entry(node, struct waiter_ticket, list_item); 237 | switch (ticket->type) { 238 | case WATCH_TYPE_TIMEOUT: 239 | if (now >= ticket->start + ticket->interval) { 240 | ticket->start = now; 241 | fresh = !ticket->updated; 242 | } 243 | break; 244 | case WATCH_TYPE_FD: 245 | if (rc == 0) /* timed-out */ 246 | break; 247 | if (pollset_cause_fd(ps, ticket->filedes)) 248 | fresh = !ticket->updated; 249 | break; 250 | case WATCH_TYPE_NULL: 251 | break; 252 | } 253 | if (fresh) { 254 | ticket->updated = 1; 255 | if (ticket->callback.fn) 256 | (* ticket->callback.fn)( 257 | ticket->callback.data, 258 | ticket 259 | ); 260 | } 261 | } 262 | } 263 | 264 | int waiter_wait_timeout(struct waiter *w, unsigned int ms) 265 | { 266 | struct waiter_ticket ticket; 267 | int rc; 268 | 269 | memset(&ticket, 0, sizeof(ticket)); 270 | waiter_ticket_set_timeout(&ticket, ms); 271 | list_append(&w->tickets, &ticket.list_item); 272 | w->count++; 273 | 274 | waiter_wait(w); 275 | rc = waiter_ticket_check(&ticket); 276 | 277 | list_remove(&w->tickets, &ticket.list_item); 278 | w->count--; 279 | 280 | return -!rc; 281 | } 282 | 283 | void waiter_ticket_set_null(struct waiter_ticket *ticket) 284 | { 285 | ticket->type = WATCH_TYPE_NULL; 286 | } 287 | 288 | void waiter_ticket_set_fd(struct waiter_ticket *ticket, int fd) 289 | { 290 | ticket->type = WATCH_TYPE_FD; 291 | ticket->filedes = fd; 292 | } 293 | 294 | void waiter_ticket_set_timeout(struct waiter_ticket *ticket, unsigned int ms) 295 | { 296 | ticket->type = WATCH_TYPE_TIMEOUT; 297 | ticket->interval = ms; 298 | ticket->start = time_ms(); 299 | } 300 | 301 | struct waiter_ticket *waiter_add_null(struct waiter *w) 302 | { 303 | struct waiter_ticket *ticket; 304 | 305 | ticket = calloc(1, sizeof(*ticket)); 306 | if (ticket == NULL) 307 | return NULL; 308 | ticket->waiter = w; 309 | 310 | list_append(&w->tickets, &ticket->list_item); 311 | if ((w->count % 32) == 0) { 312 | if (w->pollset) 313 | pollset_destroy(w->pollset); 314 | w->pollset = pollset_create(w->count + 33); 315 | if (w->pollset == NULL) 316 | return NULL; 317 | } 318 | w->count++; 319 | 320 | waiter_ticket_set_null(ticket); 321 | 322 | return ticket; 323 | } 324 | 325 | struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd) 326 | { 327 | struct waiter_ticket *ticket; 328 | 329 | ticket = waiter_add_null(w); 330 | if (ticket == NULL) 331 | return NULL; 332 | 333 | waiter_ticket_set_fd(ticket, fd); 334 | 335 | return ticket; 336 | } 337 | 338 | struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms) 339 | { 340 | struct waiter_ticket *ticket; 341 | 342 | ticket = waiter_add_null(w); 343 | if (ticket == NULL) 344 | return NULL; 345 | 346 | waiter_ticket_set_timeout(ticket, ms); 347 | 348 | return ticket; 349 | } 350 | 351 | void waiter_ticket_delete(struct waiter_ticket *ticket) 352 | { 353 | struct waiter *w = ticket->waiter; 354 | list_remove(&w->tickets, &ticket->list_item); 355 | w->count--; 356 | free(ticket); 357 | } 358 | 359 | void waiter_ticket_callback(struct waiter_ticket *ticket, waiter_ticket_cb_t cb_fn, void *data) 360 | { 361 | ticket->callback.fn = cb_fn; 362 | ticket->callback.data = data; 363 | } 364 | 365 | int waiter_ticket_check(const struct waiter_ticket *ticket) 366 | { 367 | return -(ticket->updated == 0); 368 | } 369 | 370 | int waiter_ticket_clear(struct waiter_ticket *ticket) 371 | { 372 | int ret; 373 | 374 | ret = waiter_ticket_check(ticket); 375 | ticket->updated = 0; 376 | 377 | return ret; 378 | } 379 | -------------------------------------------------------------------------------- /src/waiter.h: -------------------------------------------------------------------------------- 1 | #ifndef _WAITER_H_ 2 | #define _WAITER_H_ 3 | 4 | /** Waiter type. */ 5 | struct waiter; 6 | 7 | /** Create a new waiter. 8 | * @return Newly created waiter on success, NULL on failure. 9 | */ 10 | struct waiter *waiter_create(void); 11 | 12 | /** Destroy existing waiter. 13 | * @param w waiter to destroy. 14 | */ 15 | void waiter_destroy(struct waiter *w); 16 | 17 | /** Wait for next ticket. 18 | * @param w waiter. 19 | */ 20 | void waiter_wait(struct waiter *w); 21 | 22 | /** Wait for next ticket or timeout. 23 | * @param w waiter. 24 | * @param ms timeout in milliseconds. 25 | * @return 0 on ticket; !0 on timeout. 26 | */ 27 | int waiter_wait_timeout(struct waiter *w, unsigned int ms); 28 | 29 | /** Synchronize timer-based tickets. 30 | * @param w waiter. 31 | */ 32 | void waiter_synchronize(struct waiter *w); 33 | 34 | /** Waiter ticket type. */ 35 | struct waiter_ticket; 36 | 37 | /** Add a null wait ticket to pool. 38 | * @param w waiter 39 | * @return wait ticket on success; NULL on failure. 40 | */ 41 | struct waiter_ticket *waiter_add_null(struct waiter *w); 42 | 43 | /** Add a file descriptor to the pool. 44 | * @param w waiter. 45 | * @param fd file descriptor. 46 | * @return wait ticket on success; NULL on failure. 47 | */ 48 | struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd); 49 | 50 | /** Add a timeout to the pool. 51 | * @param w waiter. 52 | * @param ms duration of timeout in milliseconds. 53 | * @return wait ticket on success; NULL on failure. 54 | */ 55 | struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms); 56 | 57 | /** Set ticket type to null. 58 | * @param tkt wait ticket. 59 | */ 60 | void waiter_ticket_set_null(struct waiter_ticket *tkt); 61 | 62 | /** Set ticket type to file descriptor. 63 | * @param tkt wait ticket. 64 | * @param fd file descriptor. 65 | */ 66 | void waiter_ticket_set_fd(struct waiter_ticket *tkt, int fd); 67 | 68 | /** Set ticket type to timeout. 69 | * @param tkt wait ticket. 70 | * @param ms timeout in milliseconds. 71 | */ 72 | void waiter_ticket_set_timeout(struct waiter_ticket *tkt, unsigned int ms); 73 | 74 | /** Destroy ticket. 75 | * @param tkt wait ticket. 76 | */ 77 | void waiter_ticket_delete(struct waiter_ticket *tkt); 78 | 79 | /** Check to see if ticket has triggered. 80 | * @param tkt wait ticket. 81 | * @return 0 if triggered, !0 otherwise. 82 | */ 83 | int waiter_ticket_check(const struct waiter_ticket *tkt); 84 | 85 | 86 | /** Clear ticket trigger status. 87 | * @param tkt wait ticket. 88 | * @return 0 if triggered, !0 otherwise. 89 | */ 90 | int waiter_ticket_clear(struct waiter_ticket *tkt); 91 | 92 | /** Wait ticket callback function type. */ 93 | typedef void (* waiter_ticket_cb_t)(void *, struct waiter_ticket *); 94 | 95 | /** Register callback function for ticket trigger. 96 | * @param tkt wait ticket. 97 | * @param cb_fn callback function. 98 | * @param data private data to pass to callback function. 99 | */ 100 | void waiter_ticket_callback(struct waiter_ticket *tkt, 101 | waiter_ticket_cb_t cb_fn, void *data); 102 | 103 | #endif 104 | --------------------------------------------------------------------------------