db845c: qcom: Add userspace tools to talk to dsp and modem
Add Qcom userspace tools and their respective sepolicy rules.
Userspace tools are downloaded from following github:
To trigger loading of wlan firmware on SDM845
git clone https://github.com/andersson/pd-mapper
Userspace reference for net/qrtr in the Linux kernel
git clone https://github.com/andersson/qrtr
Qualcomm Remote Filesystem Service Implementation
git clone https://github.com/andersson/rmtfs
Trivial File Transfer Protocol server over AF_QIPCRTR
git clone https://github.com/andersson/tqftpserv
Change-Id: Ic466af6fef010a9b71c90e38205f49a876b001e2
Signed-off-by: Amit Pundir <amit.pundir@linaro.org>
Signed-off-by: John Stultz <john.stultz@linaro.org>
diff --git a/qcom/qrtr/Android.bp b/qcom/qrtr/Android.bp
new file mode 100644
index 0000000..9544a75
--- /dev/null
+++ b/qcom/qrtr/Android.bp
@@ -0,0 +1,49 @@
+cc_library {
+ name: "libqrtr",
+ vendor: true,
+ srcs: [
+ "lib/logging.c",
+ "lib/qrtr.c",
+ "lib/qmi.c",
+ ],
+ cflags: ["-fPIC"],
+ export_include_dirs: ["lib"],
+ local_include_dirs: ["src"],
+}
+
+cc_binary {
+ name: "qrtr-ns",
+ vendor: true,
+ srcs: [
+ "lib/logging.c",
+ "src/addr.c",
+ "src/ns.c",
+ "src/map.c",
+ "src/hash.c",
+ "src/waiter.c",
+ "src/util.c",
+ ],
+ local_include_dirs: ["lib"],
+}
+
+cc_binary {
+ name: "qrtr-cfg",
+ vendor: true,
+ srcs: [
+ "lib/logging.c",
+ "src/addr.c",
+ "src/cfg.c",
+ ],
+ local_include_dirs: ["lib"],
+}
+
+cc_binary {
+ name: "qrtr-lookup",
+ vendor: true,
+ srcs: [
+ "lib/logging.c",
+ "src/lookup.c",
+ "src/util.c",
+ ],
+ local_include_dirs: ["lib"],
+}
diff --git a/qcom/qrtr/LICENSE b/qcom/qrtr/LICENSE
new file mode 100644
index 0000000..afbafbe
--- /dev/null
+++ b/qcom/qrtr/LICENSE
@@ -0,0 +1,24 @@
+Copyright (c) 2015, Sony Mobile Communications Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ * Neither the name of the organization nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/qcom/qrtr/Makefile b/qcom/qrtr/Makefile
new file mode 100644
index 0000000..f814aa2
--- /dev/null
+++ b/qcom/qrtr/Makefile
@@ -0,0 +1,150 @@
+proj := qrtr
+proj-major := 1
+proj-minor := 0
+proj-version := $(proj-major).$(proj-minor)
+
+CFLAGS := -Wall -g
+LDFLAGS :=
+
+prefix := /usr/local
+bindir := $(prefix)/bin
+libdir := $(prefix)/lib
+includedir := $(prefix)/include
+servicedir := $(prefix)/lib/systemd/system
+
+ifneq ($(CROSS_COMPILE),)
+CC := $(CROSS_COMPILE)gcc
+endif
+SFLAGS := -I$(shell $(CC) -print-file-name=include) -Wno-non-pointer-null
+
+$(proj)-cfg-srcs := \
+ lib/logging.c \
+ src/addr.c \
+ src/cfg.c \
+
+$(proj)-cfg-cflags := -Ilib
+
+$(proj)-ns-srcs := \
+ lib/logging.c \
+ src/addr.c \
+ src/ns.c \
+ src/map.c \
+ src/hash.c \
+ src/waiter.c \
+ src/util.c \
+
+$(proj)-ns-cflags := -Ilib
+
+$(proj)-lookup-srcs := \
+ lib/logging.c \
+ src/lookup.c \
+ src/util.c \
+
+$(proj)-lookup-cflags := -Ilib
+
+lib$(proj).so-srcs := \
+ lib/logging.c \
+ lib/qrtr.c \
+ lib/qmi.c
+
+lib$(proj).so-cflags := -fPIC -Isrc
+
+targets := $(proj)-ns $(proj)-cfg $(proj)-lookup lib$(proj).so
+
+out := out
+src_to_obj = $(patsubst %.c,$(out)/obj/%.o,$(1))
+src_to_dep = $(patsubst %.c,$(out)/dep/%.d,$(1))
+
+all-srcs :=
+all-objs :=
+all-deps :=
+all-clean := $(out)
+all-install :=
+
+all: $(targets)
+
+$(out)/obj/%.o: %.c
+ifneq ($C,)
+ @echo "CHECK $<"
+ @sparse $< $(patsubst -iquote=%,-I%,$(CFLAGS)) $(SFLAGS)
+endif
+ @echo "CC $<"
+ @$(CC) -MM -MF $(call src_to_dep,$<) -MP -MT "$@ $(call src_to_dep,$<)" $(CFLAGS) $(_CFLAGS) $<
+ @$(CC) -o $@ -c $< $(CFLAGS) $(_CFLAGS)
+
+define add-inc-target
+$(DESTDIR)$(includedir)/$2: $1/$2
+ @echo "INSTALL $$<"
+ @install -D -m 755 $$< $$@
+
+all-install += $(DESTDIR)$(includedir)/$2
+endef
+
+define add-target-deps
+all-srcs += $($1-srcs)
+all-objs += $(call src_to_obj,$($1-srcs))
+all-deps += $(call src_to_dep,$($1-srcs))
+all-clean += $1
+$(call src_to_obj,$($1-srcs)): _CFLAGS := $($1-cflags)
+endef
+
+define add-bin-target
+
+$(call add-target-deps,$1)
+
+$1: $(call src_to_obj,$($1-srcs))
+ @echo "LD $$@"
+ $$(CC) -o $$@ $$(filter %.o,$$^) $(LDFLAGS)
+
+$(DESTDIR)$(bindir)/$1: $1
+ @echo "INSTALL $$<"
+ @install -D -m 755 $$< $$@
+
+all-install += $(DESTDIR)$(bindir)/$1
+endef
+
+define add-lib-target
+
+$(call add-target-deps,$1)
+
+$1: $(call src_to_obj,$($1-srcs))
+ @echo "LD $$@"
+ $$(CC) -o $$@ $$(filter %.o,$$^) $(LDFLAGS) -shared -Wl,-soname,$1.$(proj-major)
+
+$(DESTDIR)$(libdir)/$1.$(proj-version): $1
+ @echo "INSTALL $$<"
+ @install -D -m 755 $$< $$@
+ @ln -sf $1.$(proj-version) $(DESTDIR)$(libdir)/$1.$(proj-major)
+ @ln -sf $1.$(proj-major) $(DESTDIR)$(libdir)/$1
+
+all-install += $(DESTDIR)$(libdir)/$1.$(proj-version)
+endef
+
+define add-systemd-service-target
+$1: $1.in
+ sed 's+QRTR_NS_PATH+$(bindir)+g' $$< > $$@
+
+$(DESTDIR)$(servicedir)/$1: $1
+ @echo "INSTALL $$<"
+ @install -D -m 755 $$< $$@
+
+all-install += $(DESTDIR)$(servicedir)/$1
+endef
+
+$(foreach v,$(filter-out %.so,$(targets)),$(eval $(call add-bin-target,$v)))
+$(foreach v,$(filter %.so,$(targets)),$(eval $(call add-lib-target,$v)))
+$(eval $(call add-inc-target,lib,libqrtr.h))
+$(eval $(call add-systemd-service-target,qrtr-ns.service))
+
+install: $(all-install)
+
+clean:
+ @echo CLEAN
+ @$(RM) -r $(all-clean)
+
+$(call src_to_obj,$(all-srcs)): Makefile
+
+ifneq ("$(MAKECMDGOALS)","clean")
+cmd-goal-1 := $(shell mkdir -p $(sort $(dir $(all-objs) $(all-deps))))
+-include $(all-deps)
+endif
diff --git a/qcom/qrtr/lib/libqrtr.h b/qcom/qrtr/lib/libqrtr.h
new file mode 100644
index 0000000..87433ed
--- /dev/null
+++ b/qcom/qrtr/lib/libqrtr.h
@@ -0,0 +1,195 @@
+#ifndef _QRTR_LIB_H_
+#define _QRTR_LIB_H_
+
+#include <linux/qrtr.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef offsetof
+#define offsetof(type, md) ((unsigned long)&((type *)0)->md)
+#endif
+
+#ifndef container_of
+#define container_of(ptr, type, member) \
+ ((type *)((char *)(ptr) - offsetof(type, member)))
+#endif
+
+#ifndef AF_QIPCRTR
+#define AF_QIPCRTR 42
+#endif
+
+struct sockaddr_qrtr;
+
+struct qrtr_packet {
+ int type;
+
+ unsigned int node;
+ unsigned int port;
+
+ unsigned int service;
+ unsigned int instance;
+ unsigned int version;
+
+ void *data;
+ size_t data_len;
+};
+
+#define DEFINE_QRTR_PACKET(pkt, size) \
+ char pkt ## _buf[size]; \
+ struct qrtr_packet pkt = { .data = pkt ##_buf, \
+ .data_len = sizeof(pkt ##_buf), }
+
+#define QMI_REQUEST 0
+#define QMI_RESPONSE 2
+#define QMI_INDICATION 4
+
+#define QMI_COMMON_TLV_TYPE 0
+
+enum qmi_elem_type {
+ QMI_EOTI,
+ QMI_OPT_FLAG,
+ QMI_DATA_LEN,
+ QMI_UNSIGNED_1_BYTE,
+ QMI_UNSIGNED_2_BYTE,
+ QMI_UNSIGNED_4_BYTE,
+ QMI_UNSIGNED_8_BYTE,
+ QMI_SIGNED_1_BYTE_ENUM,
+ QMI_SIGNED_2_BYTE_ENUM,
+ QMI_SIGNED_4_BYTE_ENUM,
+ QMI_STRUCT,
+ QMI_STRING,
+};
+
+enum qmi_array_type {
+ NO_ARRAY,
+ STATIC_ARRAY,
+ VAR_LEN_ARRAY,
+};
+
+/**
+ * struct qmi_elem_info - describes how to encode a single QMI element
+ * @data_type: Data type of this element.
+ * @elem_len: Array length of this element, if an array.
+ * @elem_size: Size of a single instance of this data type.
+ * @array_type: Array type of this element.
+ * @tlv_type: QMI message specific type to identify which element
+ * is present in an incoming message.
+ * @offset: Specifies the offset of the first instance of this
+ * element in the data structure.
+ * @ei_array: Null-terminated array of @qmi_elem_info to describe nested
+ * structures.
+ */
+struct qmi_elem_info {
+ enum qmi_elem_type data_type;
+ uint32_t elem_len;
+ uint32_t elem_size;
+ enum qmi_array_type array_type;
+ uint8_t tlv_type;
+ uint32_t offset;
+ struct qmi_elem_info *ei_array;
+};
+
+#define QMI_RESULT_SUCCESS_V01 0
+#define QMI_RESULT_FAILURE_V01 1
+
+#define QMI_ERR_NONE_V01 0
+#define QMI_ERR_MALFORMED_MSG_V01 1
+#define QMI_ERR_NO_MEMORY_V01 2
+#define QMI_ERR_INTERNAL_V01 3
+#define QMI_ERR_CLIENT_IDS_EXHAUSTED_V01 5
+#define QMI_ERR_INVALID_ID_V01 41
+#define QMI_ERR_ENCODING_V01 58
+#define QMI_ERR_INCOMPATIBLE_STATE_V01 90
+#define QMI_ERR_NOT_SUPPORTED_V01 94
+
+/**
+ * qmi_response_type_v01 - common response header (decoded)
+ * @result: result of the transaction
+ * @error: error value, when @result is QMI_RESULT_FAILURE_V01
+ */
+struct qmi_response_type_v01 {
+ uint16_t result;
+ uint16_t error;
+};
+
+extern struct qmi_elem_info qmi_response_type_v01_ei[];
+
+int qrtr_open(int rport);
+void qrtr_close(int sock);
+
+int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz);
+int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port);
+int qrtr_recv(int sock, void *buf, unsigned int bsz);
+
+int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance);
+int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance);
+
+int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance);
+int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance);
+
+int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance);
+int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance);
+
+int qrtr_poll(int sock, unsigned int ms);
+
+int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len,
+ const struct sockaddr_qrtr *sq);
+
+int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id);
+int qmi_decode_message(void *c_struct, unsigned int *txn,
+ const struct qrtr_packet *pkt,
+ int type, int id, struct qmi_elem_info *ei);
+ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id,
+ int txn_id, const void *c_struct,
+ struct qmi_elem_info *ei);
+
+/* Initial kernel header didn't expose these */
+#ifndef QRTR_NODE_BCAST
+
+#define QRTR_NODE_BCAST 0xffffffffu
+#define QRTR_PORT_CTRL 0xfffffffeu
+
+enum qrtr_pkt_type {
+ QRTR_TYPE_DATA = 1,
+ QRTR_TYPE_HELLO = 2,
+ QRTR_TYPE_BYE = 3,
+ QRTR_TYPE_NEW_SERVER = 4,
+ QRTR_TYPE_DEL_SERVER = 5,
+ QRTR_TYPE_DEL_CLIENT = 6,
+ QRTR_TYPE_RESUME_TX = 7,
+ QRTR_TYPE_EXIT = 8,
+ QRTR_TYPE_PING = 9,
+ QRTR_TYPE_NEW_LOOKUP = 10,
+ QRTR_TYPE_DEL_LOOKUP = 11,
+};
+
+struct qrtr_ctrl_pkt {
+ __le32 cmd;
+
+ union {
+ struct {
+ __le32 service;
+ __le32 instance;
+ __le32 node;
+ __le32 port;
+ } server;
+
+ struct {
+ __le32 node;
+ __le32 port;
+ } client;
+ };
+} __attribute__((packed));
+
+#endif
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif
diff --git a/qcom/qrtr/lib/logging.c b/qcom/qrtr/lib/logging.c
new file mode 100644
index 0000000..0b0f103
--- /dev/null
+++ b/qcom/qrtr/lib/logging.c
@@ -0,0 +1,73 @@
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <syslog.h>
+
+#define QLOG_BUF_SIZE 512
+
+static const char default_tag[] = "libqrtr";
+static const char *current_tag = default_tag;
+static int min_priority = LOG_INFO;
+
+static bool logging_to_syslog = false;
+
+void qlog_setup(const char *tag, bool use_syslog)
+{
+ current_tag = tag;
+ logging_to_syslog = use_syslog;
+
+ openlog(tag, LOG_PID, LOG_USER);
+}
+
+void qlog_set_min_priority(int priority)
+{
+ if (priority < LOG_EMERG || priority > LOG_DEBUG)
+ return;
+
+ min_priority = priority;
+}
+
+static const char *get_priority_string(int priority)
+{
+ switch (priority) {
+ case LOG_EMERG:
+ return "EMERG";
+ case LOG_ALERT:
+ return "ALERT";
+ case LOG_CRIT:
+ return "CRIT";
+ case LOG_ERR:
+ return "ERROR";
+ case LOG_WARNING:
+ return "WARNING";
+ case LOG_NOTICE:
+ return "NOTICE";
+ case LOG_INFO:
+ return "INFO";
+ case LOG_DEBUG:
+ return "DEBUG";
+ }
+ return "";
+}
+
+void qlog(int priority, const char *format, ...)
+{
+ va_list ap;
+
+ if (priority > min_priority)
+ return;
+
+ va_start(ap, format);
+
+ if (logging_to_syslog) {
+ vsyslog(priority, format, ap);
+ } else {
+ char buf[QLOG_BUF_SIZE];
+ vsnprintf(buf, QLOG_BUF_SIZE, format, ap);
+
+ fprintf(stderr, "%s %s: %s\n",
+ get_priority_string(priority), current_tag, buf);
+ }
+
+ va_end(ap);
+}
diff --git a/qcom/qrtr/lib/logging.h b/qcom/qrtr/lib/logging.h
new file mode 100644
index 0000000..8372a8f
--- /dev/null
+++ b/qcom/qrtr/lib/logging.h
@@ -0,0 +1,36 @@
+#ifndef _QRTR_LOGGING_H_
+#define _QRTR_LOGGING_H_
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <syslog.h>
+
+#if defined(__GNUC__) || defined(__clang__)
+#define __PRINTF__(fmt, args) __attribute__((format(__printf__, fmt, args)))
+#else
+#define __PRINTF__(fmt, args)
+#endif
+
+void qlog_setup(const char *tag, bool use_syslog);
+void qlog_set_min_priority(int priority);
+
+void qlog(int priority, const char *format, ...) __PRINTF__(2, 3);
+
+#define LOGD(fmt, ...) qlog(LOG_DEBUG, fmt, ##__VA_ARGS__)
+
+#define LOGW(fmt, ...) qlog(LOG_WARNING, fmt, ##__VA_ARGS__)
+#define PLOGW(fmt, ...) \
+ qlog(LOG_WARNING, fmt ": %s", ##__VA_ARGS__, strerror(errno))
+
+#define LOGE(fmt, ...) qlog(LOG_ERR, fmt, ##__VA_ARGS__)
+#define PLOGE(fmt, ...) qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno))
+#define LOGE_AND_EXIT(fmt, ...) do { \
+ qlog(LOG_ERR, fmt, ##__VA_ARGS__); \
+ exit(1); \
+ } while(0)
+#define PLOGE_AND_EXIT(fmt, ...) do { \
+ qlog(LOG_ERR, fmt ": %s", ##__VA_ARGS__, strerror(errno)); \
+ exit(1); \
+ } while(0)
+
+#endif
diff --git a/qcom/qrtr/lib/qmi.c b/qcom/qrtr/lib/qmi.c
new file mode 100644
index 0000000..88f81d8
--- /dev/null
+++ b/qcom/qrtr/lib/qmi.c
@@ -0,0 +1,874 @@
+/* Copyright (c) 2017, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2017-2018 Linaro Ltd.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of The Linux Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <errno.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+
+/**
+ * qmi_header - wireformat header of QMI messages
+ * @type: type of message
+ * @txn_id: transaction id
+ * @msg_id: message id
+ * @msg_len: length of message payload following header
+ */
+struct qmi_header {
+ uint8_t type;
+ uint16_t txn_id;
+ uint16_t msg_id;
+ uint16_t msg_len;
+} __attribute__((packed));
+
+
+#define QMI_ENCDEC_ENCODE_TLV(type, length, p_dst) do { \
+ *p_dst++ = type; \
+ *p_dst++ = ((uint8_t)((length) & 0xFF)); \
+ *p_dst++ = ((uint8_t)(((length) >> 8) & 0xFF)); \
+} while (0)
+
+#define QMI_ENCDEC_DECODE_TLV(p_type, p_length, p_src) do { \
+ *p_type = (uint8_t)*p_src++; \
+ *p_length = (uint8_t)*p_src++; \
+ *p_length |= ((uint8_t)*p_src) << 8; \
+} while (0)
+
+#define QMI_ENCDEC_ENCODE_N_BYTES(p_dst, p_src, size) \
+do { \
+ memcpy(p_dst, p_src, size); \
+ p_dst = (uint8_t *)p_dst + size; \
+ p_src = (uint8_t *)p_src + size; \
+} while (0)
+
+#define QMI_ENCDEC_DECODE_N_BYTES(p_dst, p_src, size) \
+do { \
+ memcpy(p_dst, p_src, size); \
+ p_dst = (uint8_t *)p_dst + size; \
+ p_src = (uint8_t *)p_src + size; \
+} while (0)
+
+#define UPDATE_ENCODE_VARIABLES(temp_si, buf_dst, \
+ encoded_bytes, tlv_len, encode_tlv, rc) \
+do { \
+ buf_dst = (uint8_t *)buf_dst + rc; \
+ encoded_bytes += rc; \
+ tlv_len += rc; \
+ temp_si = temp_si + 1; \
+ encode_tlv = 1; \
+} while (0)
+
+#define UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc) \
+do { \
+ buf_src = (uint8_t *)buf_src + rc; \
+ decoded_bytes += rc; \
+} while (0)
+
+#define TLV_LEN_SIZE sizeof(uint16_t)
+#define TLV_TYPE_SIZE sizeof(uint8_t)
+#define OPTIONAL_TLV_TYPE_START 0x10
+
+static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
+ const void *in_c_struct, uint32_t out_buf_len,
+ int enc_level);
+
+static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct,
+ const void *in_buf, uint32_t in_buf_len, int dec_level);
+
+/**
+ * skip_to_next_elem() - Skip to next element in the structure to be encoded
+ * @ei_array: Struct info describing the element to be skipped.
+ * @level: Depth level of encoding/decoding to identify nested structures.
+ *
+ * This function is used while encoding optional elements. If the flag
+ * corresponding to an optional element is not set, then encoding the
+ * optional element can be skipped. This function can be used to perform
+ * that operation.
+ *
+ * Return: struct info of the next element that can be encoded.
+ */
+static struct qmi_elem_info *skip_to_next_elem(struct qmi_elem_info *ei_array,
+ int level)
+{
+ struct qmi_elem_info *temp_ei = ei_array;
+ uint8_t tlv_type;
+
+ if (level > 1) {
+ temp_ei = temp_ei + 1;
+ } else {
+ do {
+ tlv_type = temp_ei->tlv_type;
+ temp_ei = temp_ei + 1;
+ } while (tlv_type == temp_ei->tlv_type);
+ }
+
+ return temp_ei;
+}
+
+/**
+ * qmi_calc_min_msg_len() - Calculate the minimum length of a QMI message
+ * @ei_array: Struct info array describing the structure.
+ * @level: Level to identify the depth of the nested structures.
+ *
+ * Return: Expected minimum length of the QMI message or 0 on error.
+ */
+static int qmi_calc_min_msg_len(struct qmi_elem_info *ei_array,
+ int level)
+{
+ int min_msg_len = 0;
+ struct qmi_elem_info *temp_ei = ei_array;
+
+ if (!ei_array)
+ return min_msg_len;
+
+ while (temp_ei->data_type != QMI_EOTI) {
+ /* Optional elements do not count in minimum length */
+ if (temp_ei->data_type == QMI_OPT_FLAG) {
+ temp_ei = skip_to_next_elem(temp_ei, level);
+ continue;
+ }
+
+ if (temp_ei->data_type == QMI_DATA_LEN) {
+ min_msg_len += (temp_ei->elem_size == sizeof(uint8_t) ?
+ sizeof(uint8_t) : sizeof(uint16_t));
+ temp_ei++;
+ continue;
+ } else if (temp_ei->data_type == QMI_STRUCT) {
+ min_msg_len += qmi_calc_min_msg_len(temp_ei->ei_array,
+ (level + 1));
+ temp_ei++;
+ } else if (temp_ei->data_type == QMI_STRING) {
+ if (level > 1)
+ min_msg_len += temp_ei->elem_len <= 256 ?
+ sizeof(uint8_t) : sizeof(uint16_t);
+ min_msg_len += temp_ei->elem_len * temp_ei->elem_size;
+ temp_ei++;
+ } else {
+ min_msg_len += (temp_ei->elem_len * temp_ei->elem_size);
+ temp_ei++;
+ }
+
+ /*
+ * Type & Length info. not prepended for elements in the
+ * nested structure.
+ */
+ if (level == 1)
+ min_msg_len += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+ }
+
+ return min_msg_len;
+}
+
+/**
+ * qmi_encode_basic_elem() - Encodes elements of basic/primary data type
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @elem_len: Number of elements, in the buf_src, to be encoded.
+ * @elem_size: Size of a single instance of the element to be encoded.
+ *
+ * This function encodes the "elem_len" number of data elements, each of
+ * size "elem_size" bytes from the source buffer "buf_src" and stores the
+ * encoded information in the destination buffer "buf_dst". The elements are
+ * of primary data type which include uint8_t - u64 or similar. This
+ * function returns the number of bytes of encoded information.
+ *
+ * Return: The number of bytes of encoded information.
+ */
+static int qmi_encode_basic_elem(void *buf_dst, const void *buf_src,
+ uint32_t elem_len, uint32_t elem_size)
+{
+ uint32_t i, rc = 0;
+
+ for (i = 0; i < elem_len; i++) {
+ QMI_ENCDEC_ENCODE_N_BYTES(buf_dst, buf_src, elem_size);
+ rc += elem_size;
+ }
+
+ return rc;
+}
+
+/**
+ * qmi_encode_struct_elem() - Encodes elements of struct data type
+ * @ei_array: Struct info array descibing the struct element.
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @elem_len: Number of elements, in the buf_src, to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Depth of the nested structure from the main structure.
+ *
+ * This function encodes the "elem_len" number of struct elements, each of
+ * size "ei_array->elem_size" bytes from the source buffer "buf_src" and
+ * stores the encoded information in the destination buffer "buf_dst". The
+ * elements are of struct data type which includes any C structure. This
+ * function returns the number of bytes of encoded information.
+ *
+ * Return: The number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode_struct_elem(struct qmi_elem_info *ei_array,
+ void *buf_dst, const void *buf_src,
+ uint32_t elem_len, uint32_t out_buf_len,
+ int enc_level)
+{
+ int i, rc, encoded_bytes = 0;
+ struct qmi_elem_info *temp_ei = ei_array;
+
+ for (i = 0; i < elem_len; i++) {
+ rc = qmi_encode(temp_ei->ei_array, buf_dst, buf_src,
+ out_buf_len - encoded_bytes, enc_level);
+ if (rc < 0) {
+ LOGW("%s: STRUCT Encode failure\n", __func__);
+ return rc;
+ }
+ buf_dst = buf_dst + rc;
+ buf_src = buf_src + temp_ei->elem_size;
+ encoded_bytes += rc;
+ }
+
+ return encoded_bytes;
+}
+
+/**
+ * qmi_encode_string_elem() - Encodes elements of string data type
+ * @ei_array: Struct info array descibing the string element.
+ * @buf_dst: Buffer to store the encoded information.
+ * @buf_src: Buffer containing the elements to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Depth of the string element from the main structure.
+ *
+ * This function encodes a string element of maximum length "ei_array->elem_len"
+ * bytes from the source buffer "buf_src" and stores the encoded information in
+ * the destination buffer "buf_dst". This function returns the number of bytes
+ * of encoded information.
+ *
+ * Return: The number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode_string_elem(struct qmi_elem_info *ei_array,
+ void *buf_dst, const void *buf_src,
+ uint32_t out_buf_len, int enc_level)
+{
+ int rc;
+ int encoded_bytes = 0;
+ struct qmi_elem_info *temp_ei = ei_array;
+ uint32_t string_len = 0;
+ uint32_t string_len_sz = 0;
+
+ string_len = strlen(buf_src);
+ string_len_sz = temp_ei->elem_len <= 256 ?
+ sizeof(uint8_t) : sizeof(uint16_t);
+ if (string_len > temp_ei->elem_len) {
+ LOGW("%s: String to be encoded is longer - %d > %d\n",
+ __func__, string_len, temp_ei->elem_len);
+ return -EINVAL;
+ }
+
+ if (enc_level == 1) {
+ if (string_len + TLV_LEN_SIZE + TLV_TYPE_SIZE >
+ out_buf_len) {
+ LOGW("%s: Output len %d > Out Buf len %d\n",
+ __func__, string_len, out_buf_len);
+ return -EINVAL;
+ }
+ } else {
+ if (string_len + string_len_sz > out_buf_len) {
+ LOGW("%s: Output len %d > Out Buf len %d\n",
+ __func__, string_len, out_buf_len);
+ return -EINVAL;
+ }
+ rc = qmi_encode_basic_elem(buf_dst, &string_len,
+ 1, string_len_sz);
+ encoded_bytes += rc;
+ }
+
+ rc = qmi_encode_basic_elem(buf_dst + encoded_bytes, buf_src,
+ string_len, temp_ei->elem_size);
+ encoded_bytes += rc;
+
+ return encoded_bytes;
+}
+
+/**
+ * qmi_encode() - Core Encode Function
+ * @ei_array: Struct info array describing the structure to be encoded.
+ * @out_buf: Buffer to hold the encoded QMI message.
+ * @in_c_struct: Pointer to the C structure to be encoded.
+ * @out_buf_len: Available space in the encode buffer.
+ * @enc_level: Encode level to indicate the depth of the nested structure,
+ * within the main structure, being encoded.
+ *
+ * Return: The number of bytes of encoded information on success or negative
+ * errno on error.
+ */
+static int qmi_encode(struct qmi_elem_info *ei_array, void *out_buf,
+ const void *in_c_struct, uint32_t out_buf_len,
+ int enc_level)
+{
+ struct qmi_elem_info *temp_ei = ei_array;
+ uint8_t opt_flag_value = 0;
+ uint32_t data_len_value = 0, data_len_sz;
+ uint8_t *buf_dst = (uint8_t *)out_buf;
+ uint8_t *tlv_pointer;
+ uint32_t tlv_len;
+ uint8_t tlv_type;
+ uint32_t encoded_bytes = 0;
+ const void *buf_src;
+ int encode_tlv = 0;
+ int rc;
+
+ if (!ei_array)
+ return 0;
+
+ tlv_pointer = buf_dst;
+ tlv_len = 0;
+ if (enc_level == 1)
+ buf_dst = buf_dst + (TLV_LEN_SIZE + TLV_TYPE_SIZE);
+
+ while (temp_ei->data_type != QMI_EOTI) {
+ buf_src = in_c_struct + temp_ei->offset;
+ tlv_type = temp_ei->tlv_type;
+
+ if (temp_ei->array_type == NO_ARRAY) {
+ data_len_value = 1;
+ } else if (temp_ei->array_type == STATIC_ARRAY) {
+ data_len_value = temp_ei->elem_len;
+ } else if (data_len_value <= 0 ||
+ temp_ei->elem_len < data_len_value) {
+ LOGW("%s: Invalid data length\n", __func__);
+ return -EINVAL;
+ }
+
+ switch (temp_ei->data_type) {
+ case QMI_OPT_FLAG:
+ rc = qmi_encode_basic_elem(&opt_flag_value, buf_src,
+ 1, sizeof(uint8_t));
+ if (opt_flag_value)
+ temp_ei = temp_ei + 1;
+ else
+ temp_ei = skip_to_next_elem(temp_ei, enc_level);
+ break;
+
+ case QMI_DATA_LEN:
+ memcpy(&data_len_value, buf_src, temp_ei->elem_size);
+ data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
+ sizeof(uint8_t) : sizeof(uint16_t);
+ /* Check to avoid out of range buffer access */
+ if ((data_len_sz + encoded_bytes + TLV_LEN_SIZE +
+ TLV_TYPE_SIZE) > out_buf_len) {
+ LOGW("%s: Too Small Buffer @DATA_LEN\n",
+ __func__);
+ return -EINVAL;
+ }
+ rc = qmi_encode_basic_elem(buf_dst, &data_len_value,
+ 1, data_len_sz);
+ UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+ encoded_bytes, tlv_len,
+ encode_tlv, rc);
+ if (!data_len_value)
+ temp_ei = skip_to_next_elem(temp_ei, enc_level);
+ else
+ encode_tlv = 0;
+ break;
+
+ case QMI_UNSIGNED_1_BYTE:
+ case QMI_UNSIGNED_2_BYTE:
+ case QMI_UNSIGNED_4_BYTE:
+ case QMI_UNSIGNED_8_BYTE:
+ case QMI_SIGNED_1_BYTE_ENUM:
+ case QMI_SIGNED_2_BYTE_ENUM:
+ case QMI_SIGNED_4_BYTE_ENUM:
+ /* Check to avoid out of range buffer access */
+ if (((data_len_value * temp_ei->elem_size) +
+ encoded_bytes + TLV_LEN_SIZE + TLV_TYPE_SIZE) >
+ out_buf_len) {
+ LOGW("%s: Too Small Buffer @data_type:%d\n",
+ __func__, temp_ei->data_type);
+ return -EINVAL;
+ }
+ rc = qmi_encode_basic_elem(buf_dst, buf_src,
+ data_len_value,
+ temp_ei->elem_size);
+ UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+ encoded_bytes, tlv_len,
+ encode_tlv, rc);
+ break;
+
+ case QMI_STRUCT:
+ rc = qmi_encode_struct_elem(temp_ei, buf_dst, buf_src,
+ data_len_value,
+ out_buf_len - encoded_bytes,
+ enc_level + 1);
+ if (rc < 0)
+ return rc;
+ UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+ encoded_bytes, tlv_len,
+ encode_tlv, rc);
+ break;
+
+ case QMI_STRING:
+ rc = qmi_encode_string_elem(temp_ei, buf_dst, buf_src,
+ out_buf_len - encoded_bytes,
+ enc_level);
+ if (rc < 0)
+ return rc;
+ UPDATE_ENCODE_VARIABLES(temp_ei, buf_dst,
+ encoded_bytes, tlv_len,
+ encode_tlv, rc);
+ break;
+ default:
+ LOGW("%s: Unrecognized data type\n", __func__);
+ return -EINVAL;
+ }
+
+ if (encode_tlv && enc_level == 1) {
+ QMI_ENCDEC_ENCODE_TLV(tlv_type, tlv_len, tlv_pointer);
+ encoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+ tlv_pointer = buf_dst;
+ tlv_len = 0;
+ buf_dst = buf_dst + TLV_LEN_SIZE + TLV_TYPE_SIZE;
+ encode_tlv = 0;
+ }
+ }
+
+ return encoded_bytes;
+}
+
+/**
+ * qmi_decode_basic_elem() - Decodes elements of basic/primary data type
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @elem_len: Number of elements to be decoded.
+ * @elem_size: Size of a single instance of the element to be decoded.
+ *
+ * This function decodes the "elem_len" number of elements in QMI wire format,
+ * each of size "elem_size" bytes from the source buffer "buf_src" and stores
+ * the decoded elements in the destination buffer "buf_dst". The elements are
+ * of primary data type which include uint8_t - u64 or similar. This
+ * function returns the number of bytes of decoded information.
+ *
+ * Return: The total size of the decoded data elements, in bytes.
+ */
+static int qmi_decode_basic_elem(void *buf_dst, const void *buf_src,
+ uint32_t elem_len, uint32_t elem_size)
+{
+ uint32_t i, rc = 0;
+
+ for (i = 0; i < elem_len; i++) {
+ QMI_ENCDEC_DECODE_N_BYTES(buf_dst, buf_src, elem_size);
+ rc += elem_size;
+ }
+
+ return rc;
+}
+
+/**
+ * qmi_decode_struct_elem() - Decodes elements of struct data type
+ * @ei_array: Struct info array descibing the struct element.
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @elem_len: Number of elements to be decoded.
+ * @tlv_len: Total size of the encoded inforation corresponding to
+ * this struct element.
+ * @dec_level: Depth of the nested structure from the main structure.
+ *
+ * This function decodes the "elem_len" number of elements in QMI wire format,
+ * each of size "(tlv_len/elem_len)" bytes from the source buffer "buf_src"
+ * and stores the decoded elements in the destination buffer "buf_dst". The
+ * elements are of struct data type which includes any C structure. This
+ * function returns the number of bytes of decoded information.
+ *
+ * Return: The total size of the decoded data elements on success, negative
+ * errno on error.
+ */
+static int qmi_decode_struct_elem(struct qmi_elem_info *ei_array,
+ void *buf_dst, const void *buf_src,
+ uint32_t elem_len, uint32_t tlv_len,
+ int dec_level)
+{
+ int i, rc, decoded_bytes = 0;
+ struct qmi_elem_info *temp_ei = ei_array;
+
+ for (i = 0; i < elem_len && decoded_bytes < tlv_len; i++) {
+ rc = qmi_decode(temp_ei->ei_array, buf_dst, buf_src,
+ tlv_len - decoded_bytes, dec_level);
+ if (rc < 0)
+ return rc;
+ buf_src = buf_src + rc;
+ buf_dst = buf_dst + temp_ei->elem_size;
+ decoded_bytes += rc;
+ }
+
+ if ((dec_level <= 2 && decoded_bytes != tlv_len) ||
+ (dec_level > 2 && (i < elem_len || decoded_bytes > tlv_len))) {
+ LOGW("%s: Fault in decoding: dl(%d), db(%d), tl(%d), i(%d), el(%d)\n",
+ __func__, dec_level, decoded_bytes, tlv_len,
+ i, elem_len);
+ return -EFAULT;
+ }
+
+ return decoded_bytes;
+}
+
+/**
+ * qmi_decode_string_elem() - Decodes elements of string data type
+ * @ei_array: Struct info array descibing the string element.
+ * @buf_dst: Buffer to store the decoded element.
+ * @buf_src: Buffer containing the elements in QMI wire format.
+ * @tlv_len: Total size of the encoded inforation corresponding to
+ * this string element.
+ * @dec_level: Depth of the string element from the main structure.
+ *
+ * This function decodes the string element of maximum length
+ * "ei_array->elem_len" from the source buffer "buf_src" and puts it into
+ * the destination buffer "buf_dst". This function returns number of bytes
+ * decoded from the input buffer.
+ *
+ * Return: The total size of the decoded data elements on success, negative
+ * errno on error.
+ */
+static int qmi_decode_string_elem(struct qmi_elem_info *ei_array,
+ void *buf_dst, const void *buf_src,
+ uint32_t tlv_len, int dec_level)
+{
+ int rc;
+ int decoded_bytes = 0;
+ uint32_t string_len = 0;
+ uint32_t string_len_sz = 0;
+ struct qmi_elem_info *temp_ei = ei_array;
+
+ if (dec_level == 1) {
+ string_len = tlv_len;
+ } else {
+ string_len_sz = temp_ei->elem_len <= 256 ?
+ sizeof(uint8_t) : sizeof(uint16_t);
+ rc = qmi_decode_basic_elem(&string_len, buf_src,
+ 1, string_len_sz);
+ decoded_bytes += rc;
+ }
+
+ if (string_len > temp_ei->elem_len) {
+ LOGW("%s: String len %d > Max Len %d\n",
+ __func__, string_len, temp_ei->elem_len);
+ return -EINVAL;
+ } else if (string_len > tlv_len) {
+ LOGW("%s: String len %d > Input Buffer Len %d\n",
+ __func__, string_len, tlv_len);
+ return -EFAULT;
+ }
+
+ rc = qmi_decode_basic_elem(buf_dst, buf_src + decoded_bytes,
+ string_len, temp_ei->elem_size);
+ *((char *)buf_dst + string_len) = '\0';
+ decoded_bytes += rc;
+
+ return decoded_bytes;
+}
+
+/**
+ * find_ei() - Find element info corresponding to TLV Type
+ * @ei_array: Struct info array of the message being decoded.
+ * @type: TLV Type of the element being searched.
+ *
+ * Every element that got encoded in the QMI message will have a type
+ * information associated with it. While decoding the QMI message,
+ * this function is used to find the struct info regarding the element
+ * that corresponds to the type being decoded.
+ *
+ * Return: Pointer to struct info, if found
+ */
+static struct qmi_elem_info *find_ei(struct qmi_elem_info *ei_array,
+ uint32_t type)
+{
+ struct qmi_elem_info *temp_ei = ei_array;
+
+ while (temp_ei->data_type != QMI_EOTI) {
+ if (temp_ei->tlv_type == (uint8_t)type)
+ return temp_ei;
+ temp_ei = temp_ei + 1;
+ }
+
+ return NULL;
+}
+
+/**
+ * qmi_decode() - Core Decode Function
+ * @ei_array: Struct info array describing the structure to be decoded.
+ * @out_c_struct: Buffer to hold the decoded C struct
+ * @in_buf: Buffer containing the QMI message to be decoded
+ * @in_buf_len: Length of the QMI message to be decoded
+ * @dec_level: Decode level to indicate the depth of the nested structure,
+ * within the main structure, being decoded
+ *
+ * Return: The number of bytes of decoded information on success, negative
+ * errno on error.
+ */
+static int qmi_decode(struct qmi_elem_info *ei_array, void *out_c_struct,
+ const void *in_buf, uint32_t in_buf_len,
+ int dec_level)
+{
+ struct qmi_elem_info *temp_ei = ei_array;
+ uint8_t opt_flag_value = 1;
+ uint32_t data_len_value = 0, data_len_sz = 0;
+ uint8_t *buf_dst = out_c_struct;
+ const uint8_t *tlv_pointer;
+ uint32_t tlv_len = 0;
+ uint32_t tlv_type;
+ uint32_t decoded_bytes = 0;
+ const void *buf_src = in_buf;
+ int rc;
+
+ while (decoded_bytes < in_buf_len) {
+ if (dec_level >= 2 && temp_ei->data_type == QMI_EOTI)
+ return decoded_bytes;
+
+ if (dec_level == 1) {
+ tlv_pointer = buf_src;
+ QMI_ENCDEC_DECODE_TLV(&tlv_type,
+ &tlv_len, tlv_pointer);
+ buf_src += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+ decoded_bytes += (TLV_TYPE_SIZE + TLV_LEN_SIZE);
+ temp_ei = find_ei(ei_array, tlv_type);
+ if (!temp_ei && tlv_type < OPTIONAL_TLV_TYPE_START) {
+ LOGW("%s: Inval element info\n", __func__);
+ return -EINVAL;
+ } else if (!temp_ei) {
+ UPDATE_DECODE_VARIABLES(buf_src,
+ decoded_bytes, tlv_len);
+ continue;
+ }
+ } else {
+ /*
+ * No length information for elements in nested
+ * structures. So use remaining decodable buffer space.
+ */
+ tlv_len = in_buf_len - decoded_bytes;
+ }
+
+ buf_dst = out_c_struct + temp_ei->offset;
+ if (temp_ei->data_type == QMI_OPT_FLAG) {
+ memcpy(buf_dst, &opt_flag_value, sizeof(uint8_t));
+ temp_ei = temp_ei + 1;
+ buf_dst = out_c_struct + temp_ei->offset;
+ }
+
+ if (temp_ei->data_type == QMI_DATA_LEN) {
+ data_len_sz = temp_ei->elem_size == sizeof(uint8_t) ?
+ sizeof(uint8_t) : sizeof(uint16_t);
+ rc = qmi_decode_basic_elem(&data_len_value, buf_src,
+ 1, data_len_sz);
+ memcpy(buf_dst, &data_len_value, sizeof(uint32_t));
+ temp_ei = temp_ei + 1;
+ buf_dst = out_c_struct + temp_ei->offset;
+ tlv_len -= data_len_sz;
+ UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+ }
+
+ if (temp_ei->array_type == NO_ARRAY) {
+ data_len_value = 1;
+ } else if (temp_ei->array_type == STATIC_ARRAY) {
+ data_len_value = temp_ei->elem_len;
+ } else if (data_len_value > temp_ei->elem_len) {
+ LOGW("%s: Data len %d > max spec %d\n",
+ __func__, data_len_value, temp_ei->elem_len);
+ return -EINVAL;
+ }
+
+ switch (temp_ei->data_type) {
+ case QMI_UNSIGNED_1_BYTE:
+ case QMI_UNSIGNED_2_BYTE:
+ case QMI_UNSIGNED_4_BYTE:
+ case QMI_UNSIGNED_8_BYTE:
+ case QMI_SIGNED_1_BYTE_ENUM:
+ case QMI_SIGNED_2_BYTE_ENUM:
+ case QMI_SIGNED_4_BYTE_ENUM:
+ rc = qmi_decode_basic_elem(buf_dst, buf_src,
+ data_len_value,
+ temp_ei->elem_size);
+ UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+ break;
+
+ case QMI_STRUCT:
+ rc = qmi_decode_struct_elem(temp_ei, buf_dst, buf_src,
+ data_len_value, tlv_len,
+ dec_level + 1);
+ if (rc < 0)
+ return rc;
+ UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+ break;
+
+ case QMI_STRING:
+ rc = qmi_decode_string_elem(temp_ei, buf_dst, buf_src,
+ tlv_len, dec_level);
+ if (rc < 0)
+ return rc;
+ UPDATE_DECODE_VARIABLES(buf_src, decoded_bytes, rc);
+ break;
+
+ default:
+ LOGW("%s: Unrecognized data type\n", __func__);
+ return -EINVAL;
+ }
+ temp_ei = temp_ei + 1;
+ }
+
+ return decoded_bytes;
+}
+
+/**
+ * qmi_encode_message() - Encode C structure as QMI encoded message
+ * @type: Type of QMI message
+ * @msg_id: Message ID of the message
+ * @len: Passed as max length of the message, updated to actual size
+ * @txn_id: Transaction ID
+ * @ei: QMI message descriptor
+ * @c_struct: Reference to structure to encode
+ *
+ * Return: Buffer with encoded message, or negative ERR_PTR() on error
+ */
+ssize_t qmi_encode_message(struct qrtr_packet *pkt, int type, int msg_id,
+ int txn_id, const void *c_struct,
+ struct qmi_elem_info *ei)
+{
+ struct qmi_header *hdr = pkt->data;
+ ssize_t msglen = 0;
+ int ret;
+
+ /* Check the possibility of a zero length QMI message */
+ if (!c_struct) {
+ ret = qmi_calc_min_msg_len(ei, 1);
+ if (ret) {
+ LOGW("%s: Calc. len %d != 0, but NULL c_struct\n",
+ __func__, ret);
+ return -EINVAL;
+ }
+ }
+
+ if (pkt->data_len < sizeof(*hdr))
+ return -EMSGSIZE;
+
+ /* Encode message, if we have a message */
+ if (c_struct) {
+ msglen = qmi_encode(ei, pkt->data + sizeof(*hdr), c_struct,
+ pkt->data_len - sizeof(*hdr), 1);
+ if (msglen < 0)
+ return msglen;
+ }
+
+ hdr->type = type;
+ hdr->txn_id = txn_id;
+ hdr->msg_id = msg_id;
+ hdr->msg_len = msglen;
+
+ pkt->type = QRTR_TYPE_DATA;
+ pkt->data_len = sizeof(*hdr) + msglen;
+
+ return pkt->data_len;
+}
+
+int qmi_decode_header(const struct qrtr_packet *pkt, unsigned int *msg_id)
+{
+ const struct qmi_header *qmi = pkt->data;
+
+ if (qmi->msg_len != pkt->data_len - sizeof(*qmi)) {
+ LOGW("[RMTFS] Invalid length of incoming qmi request\n");
+ return -EINVAL;
+ }
+
+ *msg_id = qmi->msg_id;
+
+ return 0;
+}
+
+/**
+ * qmi_decode_message() - Decode QMI encoded message to C structure
+ * @buf: Buffer with encoded message
+ * @len: Amount of data in @buf
+ * @ei: QMI message descriptor
+ * @c_struct: Reference to structure to decode into
+ *
+ * Return: The number of bytes of decoded information on success, negative
+ * errno on error.
+ */
+int qmi_decode_message(void *c_struct, unsigned int *txn,
+ const struct qrtr_packet *pkt,
+ int type, int id, struct qmi_elem_info *ei)
+{
+ const struct qmi_header *hdr = pkt->data;
+
+ if (!ei)
+ return -EINVAL;
+
+ if (!c_struct || !pkt->data || !pkt->data_len)
+ return -EINVAL;
+
+ if (hdr->type != type)
+ return -EINVAL;
+
+ if (hdr->msg_id != id)
+ return -EINVAL;
+
+ if (txn)
+ *txn = hdr->txn_id;
+
+ return qmi_decode(ei, c_struct, pkt->data + sizeof(*hdr), pkt->data_len - sizeof(*hdr), 1);
+}
+
+/* Common header in all QMI responses */
+struct qmi_elem_info qmi_response_type_v01_ei[] = {
+ {
+ .data_type = QMI_SIGNED_2_BYTE_ENUM,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .array_type = NO_ARRAY,
+ .tlv_type = QMI_COMMON_TLV_TYPE,
+ .offset = offsetof(struct qmi_response_type_v01, result),
+ .ei_array = NULL,
+ },
+ {
+ .data_type = QMI_SIGNED_2_BYTE_ENUM,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .array_type = NO_ARRAY,
+ .tlv_type = QMI_COMMON_TLV_TYPE,
+ .offset = offsetof(struct qmi_response_type_v01, error),
+ .ei_array = NULL,
+ },
+ {
+ .data_type = QMI_EOTI,
+ .elem_len = 0,
+ .elem_size = 0,
+ .array_type = NO_ARRAY,
+ .tlv_type = QMI_COMMON_TLV_TYPE,
+ .offset = 0,
+ .ei_array = NULL,
+ },
+};
diff --git a/qcom/qrtr/lib/qrtr.c b/qcom/qrtr/lib/qrtr.c
new file mode 100644
index 0000000..7c1c389
--- /dev/null
+++ b/qcom/qrtr/lib/qrtr.c
@@ -0,0 +1,258 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <linux/qrtr.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include <poll.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+#include "ns.h"
+
+static int qrtr_getname(int sock, struct sockaddr_qrtr *sq)
+{
+ socklen_t sl = sizeof(*sq);
+ int rc;
+
+ rc = getsockname(sock, (void *)sq, &sl);
+ if (rc) {
+ PLOGE("getsockname()");
+ return -1;
+ }
+
+ if (sq->sq_family != AF_QIPCRTR || sl != sizeof(*sq))
+ return -1;
+
+ return 0;
+}
+
+int qrtr_open(int rport)
+{
+ struct timeval tv;
+ int sock;
+ int rc;
+
+ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (sock < 0) {
+ PLOGE("socket(AF_QIPCRTR)");
+ return -1;
+ }
+
+ tv.tv_sec = 1;
+ tv.tv_usec = 0;
+
+ rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+ if (rc) {
+ PLOGE("setsockopt(SO_RCVTIMEO)");
+ goto err;
+ }
+
+ if (rport != 0) {
+ struct sockaddr_qrtr sq;
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = 1;
+ sq.sq_port = rport;
+
+ rc = bind(sock, (void *)&sq, sizeof(sq));
+ if (rc < 0) {
+ PLOGE("bind(%d)", rport);
+ goto err;
+ }
+ }
+
+ return sock;
+err:
+ close(sock);
+ return -1;
+}
+
+void qrtr_close(int sock)
+{
+ close(sock);
+}
+
+int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz)
+{
+ struct sockaddr_qrtr sq;
+ int rc;
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = node;
+ sq.sq_port = port;
+
+ rc = sendto(sock, data, sz, 0, (void *)&sq, sizeof(sq));
+ if (rc < 0) {
+ PLOGE("sendto()");
+ return -1;
+ }
+
+ return 0;
+}
+
+int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+
+ if (qrtr_getname(sock, &sq))
+ return -1;
+
+ memset(&pkt, 0, sizeof(pkt));
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
+ pkt.server.service = cpu_to_le32(service);
+ pkt.server.instance = cpu_to_le32(instance << 8 | version);
+
+ return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
+}
+
+int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+
+ if (qrtr_getname(sock, &sq))
+ return -1;
+
+ memset(&pkt, 0, sizeof(pkt));
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
+ pkt.server.service = cpu_to_le32(service);
+ pkt.server.instance = cpu_to_le32(instance << 8 | version);
+ pkt.server.node = cpu_to_le32(sq.sq_node);
+ pkt.server.port = cpu_to_le32(sq.sq_port);
+
+ return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
+}
+
+int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ return qrtr_new_server(sock, service, version, instance);
+}
+
+int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ return qrtr_remove_server(sock, service, version, instance);
+}
+
+int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+
+ if (qrtr_getname(sock, &sq))
+ return -1;
+
+ memset(&pkt, 0, sizeof(pkt));
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP);
+ pkt.server.service = cpu_to_le32(service);
+ pkt.server.instance = cpu_to_le32(instance << 8 | version);
+
+ return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
+}
+
+int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+
+ if (qrtr_getname(sock, &sq))
+ return -1;
+
+ memset(&pkt, 0, sizeof(pkt));
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_LOOKUP);
+ pkt.server.service = cpu_to_le32(service);
+ pkt.server.instance = cpu_to_le32(instance << 8 | version);
+ pkt.server.node = cpu_to_le32(sq.sq_node);
+ pkt.server.port = cpu_to_le32(sq.sq_port);
+
+ return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
+}
+
+int qrtr_poll(int sock, unsigned int ms)
+{
+ struct pollfd fds;
+
+ fds.fd = sock;
+ fds.revents = 0;
+ fds.events = POLLIN | POLLERR;
+
+ return poll(&fds, 1, ms);
+}
+
+int qrtr_recv(int sock, void *buf, unsigned int bsz)
+{
+ int rc;
+
+ rc = recv(sock, buf, bsz, 0);
+ if (rc < 0)
+ PLOGE("recv()");
+ return rc;
+}
+
+int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port)
+{
+ struct sockaddr_qrtr sq;
+ socklen_t sl;
+ int rc;
+
+ sl = sizeof(sq);
+ rc = recvfrom(sock, buf, bsz, 0, (void *)&sq, &sl);
+ if (rc < 0) {
+ PLOGE("recvfrom()");
+ return rc;
+ }
+ if (node)
+ *node = sq.sq_node;
+ if (port)
+ *port = sq.sq_port;
+ return rc;
+}
+
+int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len,
+ const struct sockaddr_qrtr *sq)
+{
+ const struct qrtr_ctrl_pkt *ctrl = buf;
+
+ if (sq->sq_port == QRTR_PORT_CTRL){
+ if (len < sizeof(*ctrl))
+ return -EMSGSIZE;
+
+ dest->type = le32_to_cpu(ctrl->cmd);
+ switch (dest->type) {
+ case QRTR_TYPE_BYE:
+ dest->node = le32_to_cpu(ctrl->client.node);
+ break;
+ case QRTR_TYPE_DEL_CLIENT:
+ dest->node = le32_to_cpu(ctrl->client.node);
+ dest->port = le32_to_cpu(ctrl->client.port);
+ break;
+ case QRTR_TYPE_NEW_SERVER:
+ case QRTR_TYPE_DEL_SERVER:
+ dest->node = le32_to_cpu(ctrl->server.node);
+ dest->port = le32_to_cpu(ctrl->server.port);
+ dest->service = le32_to_cpu(ctrl->server.service);
+ dest->version = le32_to_cpu(ctrl->server.instance) & 0xff;
+ dest->instance = le32_to_cpu(ctrl->server.instance) >> 8;
+ break;
+ default:
+ dest->type = 0;
+ }
+ } else {
+ dest->type = QRTR_TYPE_DATA;
+ dest->node = sq->sq_node;
+ dest->port = sq->sq_port;
+
+ dest->data = buf;
+ dest->data_len = len;
+ }
+
+ return 0;
+}
diff --git a/qcom/qrtr/qrtr-ns.service.in b/qcom/qrtr/qrtr-ns.service.in
new file mode 100644
index 0000000..eaa791a
--- /dev/null
+++ b/qcom/qrtr/qrtr-ns.service.in
@@ -0,0 +1,9 @@
+[Unit]
+Description=QIPCRTR Name Service
+
+[Service]
+ExecStart=QRTR_NS_PATH/qrtr-ns -f 1
+Restart=always
+
+[Install]
+WantedBy=multi-user.target
diff --git a/qcom/qrtr/qrtr.py b/qcom/qrtr/qrtr.py
new file mode 100755
index 0000000..ab6189a
--- /dev/null
+++ b/qcom/qrtr/qrtr.py
@@ -0,0 +1,97 @@
+#!/usr/bin/env python2.7
+
+import ctypes
+import collections
+
+from ctypes import CDLL, CFUNCTYPE, POINTER, cast, py_object
+from ctypes import c_char_p, c_void_p, c_int, pointer
+
+_qrtr = CDLL("./libqrtr.so")
+
+class qrtr:
+ Result = collections.namedtuple('Result', ['service', 'instance', 'addr'])
+ _cbtype = CFUNCTYPE(None, c_void_p, c_int, c_int, c_int, c_int)
+ def __init__(self, port=0):
+ self.sock = _qrtr.qrtr_open(port)
+ if self.sock < 0:
+ raise RuntimeError("unable to open qrtr socket")
+ self.service = None
+ self._qrtr = _qrtr
+
+ def __del__(self):
+ self._qrtr.qrtr_close(self.sock)
+
+ def _lookup_list_add(self, ptr, srv, instance, node, port):
+ res = qrtr.Result(srv, instance, (node, port))
+ cast(ptr, POINTER(py_object)).contents.value.append(res)
+
+ def lookup(self, srv, instance=0, ifilter=0):
+ results = []
+ err = _qrtr.qrtr_lookup(self.sock, srv, instance, ifilter,
+ qrtr._cbtype(self._lookup_list_add), cast(pointer(py_object(results)), c_void_p))
+ if err:
+ raise RuntimeError("query failed")
+ return results
+
+ def publish(self, service, version, instance):
+ err = _qrtr.qrtr_publish(self.sock, service, version, instance)
+ if err:
+ raise RuntimeError("publish failed")
+ self.service = (service, version, instance)
+
+ def new_server(self, service, version, instance):
+ err = _qrtr.qrtr_new_server(self.sock, service, version, instance)
+ if err:
+ raise RuntimeError("new_server failed")
+ self.service = (service, version, instance)
+
+ return self.service
+
+ def remove_server(self, service):
+ err = _qrtr.qrtr_remove_server(self.sock, *service)
+ if err:
+ raise RuntimeError("remove_server failed")
+ self.service = None
+
+ def new_lookup(self, service, version, instance):
+ err = _qrtr.qrtr_new_lookup(self.sock, service, version, instance)
+ if err:
+ raise RuntimeError("new_lookup failed")
+ return (service, version, instance)
+
+ def remove_lookup(self, lookup):
+ err = _qrtr.qrtr_remove_lookup(self.sock, *lookup)
+ if err:
+ raise RuntimeError("remove_lookup failed")
+
+ def send(self, addr, data):
+ node, port = addr
+ n = _qrtr.qrtr_sendto(self.sock, node, port, c_char_p(data), len(data))
+ if n:
+ raise RuntimeError("sendto failed")
+
+ def recv(self, sz=65536):
+ buf = ctypes.create_string_buffer(sz)
+ n = _qrtr.qrtr_recv(self.sock, c_char_p(ctypes.addressof(buf)), sz)
+ if n <= 0:
+ raise RuntimeError("recv failed")
+ return buf[0:n]
+
+ def recvfrom(self, sz=65536):
+ node = ctypes.c_int()
+ port = ctypes.c_int()
+ buf = ctypes.create_string_buffer(sz)
+ n = _qrtr.qrtr_recvfrom(self.sock, c_char_p(ctypes.addressof(buf)),
+ ctypes.byref(node), ctypes.byref(port))
+ if n <= 0:
+ raise RuntimeError("recvfrom failed")
+ return (buf[0:n], (node.value, port.value))
+
+ def poll(self, tout=0):
+ return _qrtr.qrtr_poll(self.sock, tout)
+
+if __name__ == "__main__":
+ svcs = qrtr().lookup(15) # 15 is the test service
+ print " service instance addr"
+ for svc in svcs:
+ print "% 8d % 8d %s" % (svc.service, svc.instance, svc.addr)
diff --git a/qcom/qrtr/src/addr.c b/qcom/qrtr/src/addr.c
new file mode 100644
index 0000000..61417dd
--- /dev/null
+++ b/qcom/qrtr/src/addr.c
@@ -0,0 +1,77 @@
+#include <err.h>
+#include <errno.h>
+#include <linux/qrtr.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+
+void qrtr_set_address(uint32_t addr)
+{
+ struct {
+ struct nlmsghdr nh;
+ struct ifaddrmsg ifa;
+ char attrbuf[32];
+ } req;
+ struct {
+ struct nlmsghdr nh;
+ struct nlmsgerr err;
+ } resp;
+ struct sockaddr_qrtr sq;
+ struct rtattr *rta;
+ socklen_t sl = sizeof(sq);
+ int sock;
+ int ret;
+
+ /* Trigger loading of the qrtr kernel module */
+ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (sock < 0)
+ PLOGE_AND_EXIT("failed to create AF_QIPCRTR socket");
+
+ ret = getsockname(sock, (void*)&sq, &sl);
+ if (ret < 0)
+ PLOGE_AND_EXIT("getsockname()");
+ close(sock);
+
+ /* Skip configuring the address, if it's same as current */
+ if (sl == sizeof(sq) && sq.sq_node == addr)
+ return;
+
+ sock = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
+ if (sock < 0)
+ PLOGE_AND_EXIT("failed to create netlink socket");
+
+ memset(&req, 0, sizeof(req));
+ req.nh.nlmsg_len = NLMSG_SPACE(sizeof(struct ifaddrmsg));
+ req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+ req.nh.nlmsg_type = RTM_NEWADDR;
+ req.ifa.ifa_family = AF_QIPCRTR;
+
+ rta = (struct rtattr *)(((char *) &req) + req.nh.nlmsg_len);
+ rta->rta_type = IFA_LOCAL;
+ rta->rta_len = RTA_LENGTH(sizeof(addr));
+ memcpy(RTA_DATA(rta), &addr, sizeof(addr));
+
+ req.nh.nlmsg_len += rta->rta_len;
+
+ ret = send(sock, &req, req.nh.nlmsg_len, 0);
+ if (ret < 0)
+ PLOGE_AND_EXIT("failed to send netlink request");
+
+ ret = recv(sock, &resp, sizeof(resp), 0);
+ if (ret < 0)
+ PLOGE_AND_EXIT("failed to receive netlink response");
+
+ if (resp.nh.nlmsg_type == NLMSG_ERROR && resp.err.error != 0) {
+ errno = -resp.err.error;
+ PLOGE_AND_EXIT("failed to configure node id");
+ }
+
+ close(sock);
+}
diff --git a/qcom/qrtr/src/addr.h b/qcom/qrtr/src/addr.h
new file mode 100644
index 0000000..524e335
--- /dev/null
+++ b/qcom/qrtr/src/addr.h
@@ -0,0 +1,8 @@
+#ifndef __ADDR_H_
+#define __ADDR_H_
+
+#include <stdint.h>
+
+void qrtr_set_address(uint32_t addr);
+
+#endif
diff --git a/qcom/qrtr/src/cfg.c b/qcom/qrtr/src/cfg.c
new file mode 100644
index 0000000..97a8352
--- /dev/null
+++ b/qcom/qrtr/src/cfg.c
@@ -0,0 +1,44 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <limits.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <linux/qrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "addr.h"
+#include "libqrtr.h"
+#include "logging.h"
+
+static void usage(const char *progname)
+{
+ fprintf(stderr, "%s <node-id>\n", progname);
+ exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ unsigned long addrul;
+ uint32_t addr;
+ char *ep;
+ const char *progname = basename(argv[0]);
+
+ qlog_setup(progname, false);
+
+ if (argc != 2)
+ usage(progname);
+
+ addrul = strtoul(argv[1], &ep, 10);
+ if (argv[1][0] == '\0' || *ep != '\0' || addrul >= UINT_MAX)
+ usage(progname);
+ addr = addrul;
+ qrtr_set_address(addr);
+
+ return 0;
+}
diff --git a/qcom/qrtr/src/hash.c b/qcom/qrtr/src/hash.c
new file mode 100644
index 0000000..ecfbc0a
--- /dev/null
+++ b/qcom/qrtr/src/hash.c
@@ -0,0 +1,37 @@
+#include <string.h>
+#include "hash.h"
+
+unsigned int hash_mem(const void *data, unsigned int len)
+{
+ unsigned int h;
+ unsigned int i;
+
+ h = len;
+
+ for (i = 0; i < len; ++i)
+ h = ((h >> 27) ^ (h << 5)) ^ ((const unsigned char *)data)[i];
+
+ return h;
+}
+
+unsigned int hash_string(const char *value)
+{
+ return hash_mem(value, strlen(value));
+}
+
+unsigned int hash_u32(uint32_t value)
+{
+ return value * 2654435761UL;
+}
+
+unsigned int hash_u64(uint64_t value)
+{
+ return hash_u32(value & 0xffffffff) ^ hash_u32(value >> 32);
+}
+
+unsigned int hash_pointer(void *value)
+{
+ if (sizeof(value) == sizeof(uint64_t))
+ return hash_u64((long)value);
+ return hash_u32((long)value);
+}
diff --git a/qcom/qrtr/src/hash.h b/qcom/qrtr/src/hash.h
new file mode 100644
index 0000000..5b004b5
--- /dev/null
+++ b/qcom/qrtr/src/hash.h
@@ -0,0 +1,12 @@
+#ifndef _HASH_H_
+#define _HASH_H_
+
+#include <stdint.h>
+
+unsigned int hash_mem(const void *data, unsigned int len);
+unsigned int hash_string(const char *value);
+unsigned int hash_u32(uint32_t value);
+unsigned int hash_u64(uint64_t value);
+unsigned int hash_pointer(void *value);
+
+#endif
diff --git a/qcom/qrtr/src/list.h b/qcom/qrtr/src/list.h
new file mode 100644
index 0000000..d740743
--- /dev/null
+++ b/qcom/qrtr/src/list.h
@@ -0,0 +1,128 @@
+#ifndef _LIST_H_
+#define _LIST_H_
+
+#ifndef offsetof
+#define offsetof(type, md) ((unsigned long)&((type *)0)->md)
+#endif
+
+#ifndef container_of
+#define container_of(ptr, type, member) \
+ ((type *)((char *)(ptr) - offsetof(type, member)))
+#endif
+
+struct list_item {
+ struct list_item *next;
+ struct list_item *prev;
+};
+
+struct list {
+ struct list_item *head;
+ struct list_item *tail;
+};
+
+#define LIST_INIT(name) { 0, 0 }
+
+#define LIST(name) \
+ struct list name = LIST_INIT(name)
+
+#define list_entry(ptr, type, member) \
+ container_of(ptr, type, member)
+
+static inline void list_init(struct list *list)
+{
+ list->head = 0;
+ list->tail = 0;
+}
+
+static inline void list_append(struct list *list, struct list_item *item)
+{
+ item->next = 0;
+ item->prev = list->tail;
+ if (list->tail != 0)
+ list->tail->next = item;
+ else
+ list->head = item;
+ list->tail = item;
+}
+
+static inline void list_prepend(struct list *list, struct list_item *item)
+{
+ item->prev = 0;
+ item->next = list->head;
+ if (list->head == 0)
+ list->tail = item;
+ list->head = item;
+}
+
+static inline void list_insert(struct list *list, struct list_item *after, struct list_item *item)
+{
+ if (after == 0) {
+ list_prepend(list, item);
+ return;
+ }
+ item->prev = after;
+ item->next = after->next;
+ after->next = item;
+ if (item->next)
+ item->next->prev = item;
+ if (list->tail == after)
+ list->tail = item;
+}
+
+static inline void list_remove(struct list *list, struct list_item *item)
+{
+ if (item->next)
+ item->next->prev = item->prev;
+ if (list->head == item) {
+ list->head = item->next;
+ if (list->head == 0)
+ list->tail = 0;
+ } else {
+ item->prev->next = item->next;
+ if (list->tail == item)
+ list->tail = item->prev;
+ }
+ item->prev = item->next = 0;
+}
+
+static inline struct list_item *list_pop(struct list *list)
+{
+ struct list_item *item;
+ item = list->head;
+ if (item == 0)
+ return 0;
+ list_remove(list, item);
+ return item;
+}
+
+static inline struct list_item *list_last(struct list *list)
+{
+ return list->tail;
+}
+
+static inline struct list_item *list_first(struct list *list)
+{
+ return list->head;
+}
+
+
+static inline struct list_item *list_next(struct list_item *item)
+{
+ return item->next;
+}
+
+#define list_push list_append
+
+#define list_for_each(_list, _iter) \
+ for (_iter = (_list)->head; (_iter) != 0; _iter = (_iter)->next)
+
+#define list_for_each_after(_node, _iter) \
+ for (_iter = (_node)->next; (_iter) != 0; _iter = (_iter)->next)
+
+#define list_for_each_safe(_list, _iter, _bkup) \
+ for (_iter = (_list)->head; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup))
+
+#define list_for_each_safe_after(_node, _iter, _bkup) \
+ for (_iter = (_node)->next; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup))
+
+#endif
diff --git a/qcom/qrtr/src/lookup.c b/qcom/qrtr/src/lookup.c
new file mode 100644
index 0000000..3312e40
--- /dev/null
+++ b/qcom/qrtr/src/lookup.c
@@ -0,0 +1,237 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <linux/qrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+#include "ns.h"
+#include "util.h"
+
+#define DIAG_SERVICE 4097
+
+static const struct {
+ unsigned int service;
+ unsigned int ifilter;
+ const char *name;
+} common_names[] = {
+ { 0, 0, "Control service" },
+ { 1, 0, "Wireless Data Service" },
+ { 2, 0, "Device Management Service" },
+ { 3, 0, "Network Access Service" },
+ { 4, 0, "Quality Of Service service" },
+ { 5, 0, "Wireless Messaging Service" },
+ { 6, 0, "Position Determination Service" },
+ { 7, 0, "Authentication service" },
+ { 8, 0, "AT service" },
+ { 9, 0, "Voice service" },
+ { 10, 0, "Card Application Toolkit service (v2)" },
+ { 11, 0, "User Identity Module service" },
+ { 12, 0, "Phonebook Management service" },
+ { 13, 0, "QCHAT service" },
+ { 14, 0, "Remote file system service" },
+ { 15, 0, "Test service" },
+ { 16, 0, "Location service (~ PDS v2)" },
+ { 17, 0, "Specific absorption rate service" },
+ { 18, 0, "IMS settings service" },
+ { 19, 0, "Analog to digital converter driver service" },
+ { 20, 0, "Core sound driver service" },
+ { 21, 0, "Modem embedded file system service" },
+ { 22, 0, "Time service" },
+ { 23, 0, "Thermal sensors service" },
+ { 24, 0, "Thermal mitigation device service" },
+ { 25, 0, "Service access proxy service" },
+ { 26, 0, "Wireless data administrative service" },
+ { 27, 0, "TSYNC control service" },
+ { 28, 0, "Remote file system access service" },
+ { 29, 0, "Circuit switched videotelephony service" },
+ { 30, 0, "Qualcomm mobile access point service" },
+ { 31, 0, "IMS presence service" },
+ { 32, 0, "IMS videotelephony service" },
+ { 33, 0, "IMS application service" },
+ { 34, 0, "Coexistence service" },
+ { 36, 0, "Persistent device configuration service" },
+ { 38, 0, "Simultaneous transmit service" },
+ { 39, 0, "Bearer independent transport service" },
+ { 40, 0, "IMS RTP service" },
+ { 41, 0, "RF radiated performance enhancement service" },
+ { 42, 0, "Data system determination service" },
+ { 43, 0, "Subsystem control service" },
+ { 49, 0, "IPA control service" },
+ { 51, 0, "CoreSight remote tracing service" },
+ { 52, 0, "Dynamic Heap Memory Sharing" },
+ { 64, 0, "Service registry locator service" },
+ { 66, 0, "Service registry notification service" },
+ { 69, 0, "ATH10k WLAN firmware service" },
+ { 224, 0, "Card Application Toolkit service (v1)" },
+ { 225, 0, "Remote Management Service" },
+ { 226, 0, "Open Mobile Alliance device management service" },
+ { 312, 0, "QBT1000 Ultrasonic Fingerprint Sensor service" },
+ { 769, 0, "SLIMbus control service" },
+ { 771, 0, "Peripheral Access Control Manager service" },
+ { DIAG_SERVICE, 0, "DIAG service" },
+};
+
+static const char *diag_instance_base_str(unsigned int instance_base)
+{
+ switch (instance_base) {
+ case 0: return "MODEM";
+ case 1: return "LPASS";
+ case 2: return "WCNSS";
+ case 3: return "SENSORS";
+ case 4: return "CDSP";
+ case 5: return "WDSP";
+ default: return "<unk>";
+ }
+}
+
+static const char *diag_instance_str(unsigned int instance)
+{
+ switch (instance) {
+ case 0: return "CNTL";
+ case 1: return "CMD";
+ case 2: return "DATA";
+ case 3: return "DCI_CMD";
+ case 4: return "DCI";
+ default: return "<unk>";
+ }
+}
+
+static int get_diag_instance_info(char *str, size_t size, unsigned int instance)
+{
+ return snprintf(str, size, "%s:%s",
+ diag_instance_base_str(instance >> 6),
+ diag_instance_str(instance & 0x3f));
+}
+
+static unsigned int read_num_le(const char *str, int *rcp)
+{
+ unsigned int ret;
+ char *e;
+
+ if (*rcp)
+ return 0;
+
+ errno = 0;
+ ret = strtoul(str, &e, 0);
+ *rcp = -(errno || *e);
+
+ return cpu_to_le32(ret);
+}
+
+int main(int argc, char **argv)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ unsigned int instance;
+ unsigned int service;
+ unsigned int version;
+ unsigned int node;
+ unsigned int port;
+ socklen_t sl = sizeof(sq);
+ struct timeval tv;
+ int sock;
+ int len;
+ int rc;
+ const char *progname = basename(argv[0]);
+
+ qlog_setup(progname, false);
+
+ rc = 0;
+ memset(&pkt, 0, sizeof(pkt));
+
+ switch (argc) {
+ default:
+ rc = -1;
+ break;
+ case 3: pkt.server.instance = read_num_le(argv[2], &rc);
+ case 2: pkt.server.service = read_num_le(argv[1], &rc);
+ case 1: break;
+ }
+ if (rc) {
+ fprintf(stderr, "Usage: %s [<service> [<instance> [<filter>]]]\n", progname);
+ exit(1);
+ }
+
+ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (sock < 0)
+ PLOGE_AND_EXIT("sock(AF_QIPCRTR)");
+
+ rc = getsockname(sock, (void *)&sq, &sl);
+ if (rc || sq.sq_family != AF_QIPCRTR || sl != sizeof(sq))
+ PLOGE_AND_EXIT("getsockname()");
+
+ sq.sq_port = QRTR_PORT_CTRL;
+
+ tv.tv_sec = 1;
+ tv.tv_usec = 0;
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP);
+
+ rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+ if (rc)
+ PLOGE_AND_EXIT("setsockopt(SO_RCVTIMEO)");
+
+ rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGE_AND_EXIT("sendto()");
+
+ printf(" Service Version Instance Node Port\n");
+
+ while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) {
+ unsigned int type = le32_to_cpu(pkt.cmd);
+ const char *name = NULL;
+ unsigned int i;
+
+ if (len < sizeof(pkt) || type != QRTR_TYPE_NEW_SERVER) {
+ PLOGW("invalid/short packet");
+ continue;
+ }
+
+ if (!pkt.server.service && !pkt.server.instance &&
+ !pkt.server.node && !pkt.server.port)
+ break;
+
+ service = le32_to_cpu(pkt.server.service);
+ version = le32_to_cpu(pkt.server.instance) & 0xff;
+ instance = le32_to_cpu(pkt.server.instance) >> 8;
+ node = le32_to_cpu(pkt.server.node);
+ port = le32_to_cpu(pkt.server.port);
+
+ for (i = 0; i < sizeof(common_names)/sizeof(common_names[0]); ++i) {
+ if (service != common_names[i].service)
+ continue;
+ if (instance &&
+ (instance & common_names[i].ifilter) != common_names[i].ifilter)
+ continue;
+ name = common_names[i].name;
+ }
+ if (!name)
+ name = "<unknown>";
+
+ if (service == DIAG_SERVICE) {
+ char buf[24];
+ instance = le32_to_cpu(pkt.server.instance);
+ get_diag_instance_info(buf, sizeof(buf), instance);
+ printf("%9d %7s %8d %4d %5d %s (%s)\n",
+ service, "N/A", instance, node, port, name, buf);
+ } else {
+ printf("%9d %7d %8d %4d %5d %s\n",
+ service, version, instance, node, port, name);
+ }
+ }
+
+ if (len < 0)
+ PLOGE_AND_EXIT("recv()");
+
+ close(sock);
+
+ return 0;
+}
diff --git a/qcom/qrtr/src/map.c b/qcom/qrtr/src/map.c
new file mode 100644
index 0000000..eed3488
--- /dev/null
+++ b/qcom/qrtr/src/map.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2008-2009, Courtney Cavin
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdlib.h>
+#include "map.h"
+
+struct map_entry {
+ struct map_item *item;
+};
+
+/* Marker for deleted items */
+static struct map_item deleted;
+
+void map_destroy(struct map *map)
+{
+ free(map->data);
+}
+
+void map_clear(struct map *map, void (*release)(struct map_item *))
+{
+ int i;
+
+ for (i = 0; i < map->size; ++i){
+ if (!map->data[i].item)
+ continue;
+ if (map->data[i].item != &deleted)
+ (* release)(map->data[i].item);
+ map->data[i].item = NULL;
+ }
+ map->count = 0;
+}
+
+int map_create(struct map *map)
+{
+ map->size = 0;
+ map->data = 0;
+ map->count = 0;
+ return 0;
+}
+
+static int map_hash(struct map *map, unsigned int key)
+{
+ struct map_entry *e;
+ int idx, i;
+
+ if (map->count == map->size)
+ return -1;
+
+ idx = key % map->size;
+
+ for (i = 0; i < map->size; ++i) {
+ e = &map->data[idx];
+ if (!e->item || e->item == &deleted) {
+ ++map->count;
+ return idx;
+ }
+ if (e->item->key == key)
+ return idx;
+
+ idx = (idx + 1) % map->size;
+ }
+
+ return -2;
+}
+
+static int map_rehash(struct map *map);
+
+int map_reput(struct map *map, unsigned int key, struct map_item *value,
+ struct map_item **old)
+{
+ int rc;
+
+ while ((rc = map_hash(map, key)) < 0) {
+ if ((rc = map_rehash(map)) < 0)
+ return rc;
+ }
+
+ if (old) {
+ if (map->data[rc].item == &deleted)
+ *old = NULL;
+ else
+ *old = map->data[rc].item;
+ }
+ map->data[rc].item = value;
+ if (value)
+ map->data[rc].item->key = key;
+
+ return 0;
+}
+
+int map_put(struct map *map, unsigned int key, struct map_item *value)
+{
+ return map_reput(map, key, value, NULL);
+}
+
+static int map_rehash(struct map *map)
+{
+ struct map_entry *oldt, *newt;
+ int o_size, i;
+ int rc;
+
+ newt = calloc(sizeof(struct map_entry), map->size + 256);
+ if (!newt)
+ return -1;
+
+ oldt = map->data;
+ map->data = newt;
+
+ o_size = map->size;
+ map->size += 256;
+ map->count = 0;
+
+ for (i = 0; i < o_size; ++i){
+ if (!oldt[i].item || oldt[i].item == &deleted)
+ continue;
+ rc = map_put(map, oldt[i].item->key, oldt[i].item);
+ if (rc < 0)
+ return rc;
+ }
+
+ free(oldt);
+
+ return 0;
+}
+
+static struct map_entry *map_find(const struct map *map, unsigned int key)
+{
+ struct map_entry *e;
+ int idx, i;
+
+ if (map->size == 0)
+ return NULL;
+
+ idx = key % map->size;
+
+ for (i = 0; i < map->size; ++i) {
+ e = &map->data[idx];
+ idx = (idx + 1) % map->size;
+
+ if (!e->item)
+ break;
+ if (e->item == &deleted)
+ continue;
+ if (e->item->key == key)
+ return e;
+ }
+ return NULL;
+}
+
+int map_contains(const struct map *map, unsigned int key)
+{
+ return (map_find(map, key) == NULL) ? 0 : 1;
+}
+
+struct map_item *map_get(const struct map *map, unsigned int key)
+{
+ struct map_entry *e;
+
+ e = map_find(map, key);
+ if (e == NULL)
+ return NULL;
+ return e->item;
+}
+
+int map_remove(struct map *map, unsigned int key)
+{
+ struct map_entry *e;
+
+ e = map_find(map, key);
+ if (e) {
+ e->item = &deleted;
+ --map->count;
+ }
+ return !e;
+}
+
+unsigned int map_length(struct map *map)
+{
+ return map ? map->count : 0;
+}
+
+static struct map_entry *map_iter_from(const struct map *map, unsigned int start)
+{
+ unsigned int i = start;
+
+ for (; i < map->size; ++i) {
+ if (map->data[i].item && map->data[i].item != &deleted)
+ return &map->data[i];
+ }
+ return NULL;
+}
+
+struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter)
+{
+ if (iter == NULL)
+ return NULL;
+
+ return map_iter_from(map, (iter - map->data) + 1);
+}
+
+struct map_entry *map_iter_first(const struct map *map)
+{
+ return map_iter_from(map, 0);
+}
+
+
+struct map_item *map_iter_item(struct map_entry *iter)
+{
+ return iter->item;
+}
diff --git a/qcom/qrtr/src/map.h b/qcom/qrtr/src/map.h
new file mode 100644
index 0000000..de68e19
--- /dev/null
+++ b/qcom/qrtr/src/map.h
@@ -0,0 +1,38 @@
+#ifndef _MAP_H_
+#define _MAP_H_
+
+struct map_item {
+ unsigned int key;
+};
+
+struct map_entry;
+
+struct map {
+ unsigned int size;
+ unsigned int count;
+ struct map_entry *data;
+};
+
+int map_create(struct map *map);
+void map_destroy(struct map *map);
+void map_clear(struct map *map, void (*release)(struct map_item *));
+
+int map_put(struct map *map, unsigned int key, struct map_item *v);
+int map_reput(struct map *map, unsigned int key, struct map_item *v,
+ struct map_item **old);
+int map_contains(const struct map *map, unsigned int key);
+struct map_item *map_get(const struct map *map, unsigned int key);
+int map_remove(struct map *map, unsigned int key);
+unsigned int map_length(struct map *map);
+
+struct map_entry *map_iter_first(const struct map *map);
+struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter);
+struct map_item *map_iter_item(struct map_entry *iter);
+
+#define map_for_each(map, iter) \
+ for (iter = map_iter_first(map); iter; iter = map_iter_next(map, iter))
+
+#define map_iter_data(iter, type, member) \
+ container_of(map_iter_item(iter), type, member)
+
+#endif
diff --git a/qcom/qrtr/src/ns.c b/qcom/qrtr/src/ns.c
new file mode 100644
index 0000000..da1557a
--- /dev/null
+++ b/qcom/qrtr/src/ns.c
@@ -0,0 +1,793 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <limits.h>
+#include <linux/qrtr.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "addr.h"
+#include "hash.h"
+#include "list.h"
+#include "map.h"
+#include "ns.h"
+#include "util.h"
+#include "waiter.h"
+
+#include "libqrtr.h"
+#include "logging.h"
+
+static const char *ctrl_pkt_strings[] = {
+ [QRTR_TYPE_HELLO] = "hello",
+ [QRTR_TYPE_BYE] = "bye",
+ [QRTR_TYPE_NEW_SERVER] = "new-server",
+ [QRTR_TYPE_DEL_SERVER] = "del-server",
+ [QRTR_TYPE_DEL_CLIENT] = "del-client",
+ [QRTR_TYPE_RESUME_TX] = "resume-tx",
+ [QRTR_TYPE_EXIT] = "exit",
+ [QRTR_TYPE_PING] = "ping",
+ [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
+ [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
+};
+
+#define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0]))
+
+struct context {
+ int sock;
+
+ int local_node;
+
+ struct sockaddr_qrtr bcast_sq;
+
+ struct list lookups;
+};
+
+struct server_filter {
+ unsigned int service;
+ unsigned int instance;
+ unsigned int ifilter;
+};
+
+struct lookup {
+ unsigned int service;
+ unsigned int instance;
+
+ struct sockaddr_qrtr sq;
+ struct list_item li;
+};
+
+struct server {
+ unsigned int service;
+ unsigned int instance;
+
+ unsigned int node;
+ unsigned int port;
+ struct map_item mi;
+ struct list_item qli;
+};
+
+struct node {
+ unsigned int id;
+
+ struct map_item mi;
+ struct map services;
+};
+
+static struct map nodes;
+
+static void server_mi_free(struct map_item *mi);
+
+static struct node *node_get(unsigned int node_id)
+{
+ struct map_item *mi;
+ struct node *node;
+ int rc;
+
+ mi = map_get(&nodes, hash_u32(node_id));
+ if (mi)
+ return container_of(mi, struct node, mi);
+
+ node = calloc(1, sizeof(*node));
+ if (!node)
+ return NULL;
+
+ node->id = node_id;
+
+ rc = map_create(&node->services);
+ if (rc)
+ LOGE_AND_EXIT("unable to create map");
+
+ rc = map_put(&nodes, hash_u32(node_id), &node->mi);
+ if (rc) {
+ map_destroy(&node->services);
+ free(node);
+ return NULL;
+ }
+
+ return node;
+}
+
+static int server_match(const struct server *srv, const struct server_filter *f)
+{
+ unsigned int ifilter = f->ifilter;
+
+ if (f->service != 0 && srv->service != f->service)
+ return 0;
+ if (!ifilter && f->instance)
+ ifilter = ~0;
+ return (srv->instance & ifilter) == f->instance;
+}
+
+static int server_query(const struct server_filter *f, struct list *list)
+{
+ struct map_entry *node_me;
+ struct map_entry *me;
+ struct node *node;
+ int count = 0;
+
+ list_init(list);
+ map_for_each(&nodes, node_me) {
+ node = map_iter_data(node_me, struct node, mi);
+
+ map_for_each(&node->services, me) {
+ struct server *srv;
+
+ srv = map_iter_data(me, struct server, mi);
+ if (!server_match(srv, f))
+ continue;
+
+ list_append(list, &srv->qli);
+ ++count;
+ }
+ }
+
+ return count;
+}
+
+static int service_announce_new(struct context *ctx,
+ struct sockaddr_qrtr *dest,
+ struct server *srv)
+{
+ struct qrtr_ctrl_pkt cmsg;
+ int rc;
+
+ LOGD("advertising new server [%d:%x]@[%d:%d]\n",
+ srv->service, srv->instance, srv->node, srv->port);
+
+ cmsg.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
+ cmsg.server.service = cpu_to_le32(srv->service);
+ cmsg.server.instance = cpu_to_le32(srv->instance);
+ cmsg.server.node = cpu_to_le32(srv->node);
+ cmsg.server.port = cpu_to_le32(srv->port);
+
+ rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0,
+ (struct sockaddr *)dest, sizeof(*dest));
+ if (rc < 0)
+ PLOGW("sendto()");
+
+ return rc;
+}
+
+static int service_announce_del(struct context *ctx,
+ struct sockaddr_qrtr *dest,
+ struct server *srv)
+{
+ struct qrtr_ctrl_pkt cmsg;
+ int rc;
+
+ LOGD("advertising removal of server [%d:%x]@[%d:%d]\n",
+ srv->service, srv->instance, srv->node, srv->port);
+
+ cmsg.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
+ cmsg.server.service = cpu_to_le32(srv->service);
+ cmsg.server.instance = cpu_to_le32(srv->instance);
+ cmsg.server.node = cpu_to_le32(srv->node);
+ cmsg.server.port = cpu_to_le32(srv->port);
+
+ rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0,
+ (struct sockaddr *)dest, sizeof(*dest));
+ if (rc < 0)
+ PLOGW("sendto()");
+
+ return rc;
+}
+
+static int lookup_notify(struct context *ctx, struct sockaddr_qrtr *to,
+ struct server *srv, bool new)
+{
+ struct qrtr_ctrl_pkt pkt = {};
+ int rc;
+
+ pkt.cmd = new ? QRTR_TYPE_NEW_SERVER : QRTR_TYPE_DEL_SERVER;
+ if (srv) {
+ pkt.server.service = cpu_to_le32(srv->service);
+ pkt.server.instance = cpu_to_le32(srv->instance);
+ pkt.server.node = cpu_to_le32(srv->node);
+ pkt.server.port = cpu_to_le32(srv->port);
+ }
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)to, sizeof(*to));
+ if (rc < 0)
+ PLOGW("send lookup result failed");
+ return rc;
+}
+
+static int annouce_servers(struct context *ctx, struct sockaddr_qrtr *sq)
+{
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ node = node_get(ctx->local_node);
+ if (!node)
+ return 0;
+
+ map_for_each(&node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ rc = service_announce_new(ctx, sq, srv);
+ if (rc < 0)
+ return rc;
+ }
+
+ return 0;
+}
+
+static struct server *server_add(unsigned int service, unsigned int instance,
+ unsigned int node_id, unsigned int port)
+{
+ struct map_item *mi;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ if (!service || !port)
+ return NULL;
+
+ srv = calloc(1, sizeof(*srv));
+ if (srv == NULL)
+ return NULL;
+
+ srv->service = service;
+ srv->instance = instance;
+ srv->node = node_id;
+ srv->port = port;
+
+ node = node_get(node_id);
+ if (!node)
+ goto err;
+
+ rc = map_reput(&node->services, hash_u32(port), &srv->mi, &mi);
+ if (rc)
+ goto err;
+
+ LOGD("add server [%d:%x]@[%d:%d]\n", srv->service, srv->instance,
+ srv->node, srv->port);
+
+ if (mi) { /* we replaced someone */
+ struct server *old = container_of(mi, struct server, mi);
+ free(old);
+ }
+
+ return srv;
+
+err:
+ free(srv);
+ return NULL;
+}
+
+static int server_del(struct context *ctx, struct node *node, unsigned int port)
+{
+ struct lookup *lookup;
+ struct list_item *li;
+ struct map_item *mi;
+ struct server *srv;
+
+ mi = map_get(&node->services, hash_u32(port));
+ if (!mi)
+ return -ENOENT;
+
+ srv = container_of(mi, struct server, mi);
+ map_remove(&node->services, srv->mi.key);
+
+ /* Broadcast the removal of local services */
+ if (srv->node == ctx->local_node)
+ service_announce_del(ctx, &ctx->bcast_sq, srv);
+
+ /* Announce the service's disappearance to observers */
+ list_for_each(&ctx->lookups, li) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->service && lookup->service != srv->service)
+ continue;
+ if (lookup->instance && lookup->instance != srv->instance)
+ continue;
+
+ lookup_notify(ctx, &lookup->sq, srv, false);
+ }
+
+ free(srv);
+
+ return 0;
+}
+
+static int ctrl_cmd_hello(struct context *ctx, struct sockaddr_qrtr *sq,
+ const void *buf, size_t len)
+{
+ int rc;
+
+ rc = sendto(ctx->sock, buf, len, 0, (void *)sq, sizeof(*sq));
+ if (rc > 0)
+ rc = annouce_servers(ctx, sq);
+
+ return rc;
+}
+
+static int ctrl_cmd_bye(struct context *ctx, struct sockaddr_qrtr *from)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ struct node *local_node;
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ node = node_get(from->sq_node);
+ if (!node)
+ return 0;
+
+ map_for_each(&node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ server_del(ctx, node, srv->port);
+ }
+
+ /* Advertise the removal of this client to all local services */
+ local_node = node_get(ctx->local_node);
+ if (!local_node)
+ return 0;
+
+ memset(&pkt, 0, sizeof(pkt));
+ pkt.cmd = QRTR_TYPE_BYE;
+ pkt.client.node = from->sq_node;
+
+ map_for_each(&local_node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = srv->node;
+ sq.sq_port = srv->port;
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGW("bye propagation failed");
+ }
+
+ return 0;
+}
+
+static int ctrl_cmd_del_client(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned node_id, unsigned port)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ struct node *local_node;
+ struct list_item *tmp;
+ struct lookup *lookup;
+ struct list_item *li;
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ /* Local DEL_CLIENT messages comes from the port being closed */
+ if (from->sq_node == ctx->local_node && from->sq_port != port)
+ return -EINVAL;
+
+ /* Remove any lookups by this client */
+ list_for_each_safe(&ctx->lookups, li, tmp) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->sq.sq_node != node_id)
+ continue;
+ if (lookup->sq.sq_port != port)
+ continue;
+
+ list_remove(&ctx->lookups, &lookup->li);
+ free(lookup);
+ }
+
+ /* Remove the server belonging to this port*/
+ node = node_get(node_id);
+ if (node)
+ server_del(ctx, node, port);
+
+ /* Advertise the removal of this client to all local services */
+ local_node = node_get(ctx->local_node);
+ if (!local_node)
+ return 0;
+
+ pkt.cmd = QRTR_TYPE_DEL_CLIENT;
+ pkt.client.node = node_id;
+ pkt.client.port = port;
+
+ map_for_each(&local_node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = srv->node;
+ sq.sq_port = srv->port;
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGW("del_client propagation failed");
+ }
+
+ return 0;
+}
+
+static int ctrl_cmd_new_server(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance,
+ unsigned int node_id, unsigned int port)
+{
+ struct lookup *lookup;
+ struct list_item *li;
+ struct server *srv;
+ int rc = 0;
+
+ /* Ignore specified node and port for local servers*/
+ if (from->sq_node == ctx->local_node) {
+ node_id = from->sq_node;
+ port = from->sq_port;
+ }
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ srv = server_add(service, instance, node_id, port);
+ if (!srv)
+ return -EINVAL;
+
+ if (srv->node == ctx->local_node)
+ rc = service_announce_new(ctx, &ctx->bcast_sq, srv);
+
+ list_for_each(&ctx->lookups, li) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->service && lookup->service != service)
+ continue;
+ if (lookup->instance && lookup->instance != instance)
+ continue;
+
+ lookup_notify(ctx, &lookup->sq, srv, true);
+ }
+
+ return rc;
+}
+
+static int ctrl_cmd_del_server(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance,
+ unsigned int node_id, unsigned int port)
+{
+ struct node *node;
+
+ /* Ignore specified node and port for local servers*/
+ if (from->sq_node == ctx->local_node) {
+ node_id = from->sq_node;
+ port = from->sq_port;
+ }
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ /* Local servers may only unregister themselves */
+ if (from->sq_node == ctx->local_node && from->sq_port != port)
+ return -EINVAL;
+
+ node = node_get(node_id);
+ if (!node)
+ return -ENOENT;
+
+ return server_del(ctx, node, port);
+}
+
+static int ctrl_cmd_new_lookup(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance)
+{
+ struct server_filter filter;
+ struct list reply_list;
+ struct lookup *lookup;
+ struct list_item *li;
+ struct server *srv;
+
+ /* Accept only local observers */
+ if (from->sq_node != ctx->local_node)
+ return -EINVAL;
+
+ lookup = calloc(1, sizeof(*lookup));
+ if (!lookup)
+ return -EINVAL;
+
+ lookup->sq = *from;
+ lookup->service = service;
+ lookup->instance = instance;
+ list_append(&ctx->lookups, &lookup->li);
+
+ memset(&filter, 0, sizeof(filter));
+ filter.service = service;
+ filter.instance = instance;
+
+ server_query(&filter, &reply_list);
+ list_for_each(&reply_list, li) {
+ srv = container_of(li, struct server, qli);
+
+ lookup_notify(ctx, from, srv, true);
+ }
+
+ lookup_notify(ctx, from, NULL, true);
+
+ return 0;
+}
+
+static int ctrl_cmd_del_lookup(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance)
+{
+ struct lookup *lookup;
+ struct list_item *tmp;
+ struct list_item *li;
+
+ list_for_each_safe(&ctx->lookups, li, tmp) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->sq.sq_node != from->sq_node)
+ continue;
+ if (lookup->sq.sq_port != from->sq_port)
+ continue;
+ if (lookup->service != service)
+ continue;
+ if (lookup->instance && lookup->instance != instance)
+ continue;
+
+ list_remove(&ctx->lookups, &lookup->li);
+ free(lookup);
+ }
+
+ return 0;
+}
+
+static void ctrl_port_fn(void *vcontext, struct waiter_ticket *tkt)
+{
+ struct context *ctx = vcontext;
+ struct sockaddr_qrtr sq;
+ int sock = ctx->sock;
+ struct qrtr_ctrl_pkt *msg;
+ unsigned int cmd;
+ char buf[4096];
+ socklen_t sl;
+ ssize_t len;
+ int rc;
+
+ sl = sizeof(sq);
+ len = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (len <= 0) {
+ PLOGW("recvfrom()");
+ close(sock);
+ ctx->sock = -1;
+ goto out;
+ }
+ msg = (void *)buf;
+
+ if (len < 4) {
+ LOGW("short packet from %d:%d", sq.sq_node, sq.sq_port);
+ goto out;
+ }
+
+ cmd = le32_to_cpu(msg->cmd);
+ if (cmd < ARRAY_SIZE(ctrl_pkt_strings) && ctrl_pkt_strings[cmd])
+ LOGD("%s from %d:%d\n", ctrl_pkt_strings[cmd], sq.sq_node, sq.sq_port);
+ else
+ LOGD("UNK (%08x) from %d:%d\n", cmd, sq.sq_node, sq.sq_port);
+
+ rc = 0;
+ switch (cmd) {
+ case QRTR_TYPE_HELLO:
+ rc = ctrl_cmd_hello(ctx, &sq, buf, len);
+ break;
+ case QRTR_TYPE_BYE:
+ rc = ctrl_cmd_bye(ctx, &sq);
+ break;
+ case QRTR_TYPE_DEL_CLIENT:
+ rc = ctrl_cmd_del_client(ctx, &sq,
+ le32_to_cpu(msg->client.node),
+ le32_to_cpu(msg->client.port));
+ break;
+ case QRTR_TYPE_NEW_SERVER:
+ rc = ctrl_cmd_new_server(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance),
+ le32_to_cpu(msg->server.node),
+ le32_to_cpu(msg->server.port));
+ break;
+ case QRTR_TYPE_DEL_SERVER:
+ rc = ctrl_cmd_del_server(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance),
+ le32_to_cpu(msg->server.node),
+ le32_to_cpu(msg->server.port));
+ break;
+ case QRTR_TYPE_EXIT:
+ case QRTR_TYPE_PING:
+ case QRTR_TYPE_RESUME_TX:
+ break;
+ case QRTR_TYPE_NEW_LOOKUP:
+ rc = ctrl_cmd_new_lookup(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance));
+ break;
+ case QRTR_TYPE_DEL_LOOKUP:
+ rc = ctrl_cmd_del_lookup(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance));
+ break;
+ }
+
+ if (rc < 0)
+ LOGW("failed while handling packet from %d:%d",
+ sq.sq_node, sq.sq_port);
+out:
+ waiter_ticket_clear(tkt);
+}
+
+static int say_hello(struct context *ctx)
+{
+ struct qrtr_ctrl_pkt pkt;
+ int rc;
+
+ memset(&pkt, 0, sizeof(pkt));
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&ctx->bcast_sq, sizeof(ctx->bcast_sq));
+ if (rc < 0)
+ return rc;
+
+ return 0;
+}
+
+static void server_mi_free(struct map_item *mi)
+{
+ free(container_of(mi, struct server, mi));
+}
+
+static void node_mi_free(struct map_item *mi)
+{
+ struct node *node = container_of(mi, struct node, mi);
+
+ map_clear(&node->services, server_mi_free);
+ map_destroy(&node->services);
+
+ free(node);
+}
+
+static void usage(const char *progname)
+{
+ fprintf(stderr, "%s [-f] [-s] [<node-id>]\n", progname);
+ exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ struct waiter_ticket *tkt;
+ struct sockaddr_qrtr sq;
+ struct context ctx;
+ unsigned long addr = (unsigned long)-1;
+ struct waiter *w;
+ socklen_t sl = sizeof(sq);
+ bool foreground = false;
+ bool use_syslog = false;
+ bool verbose_log = false;
+ char *ep;
+ int opt;
+ int rc;
+ const char *progname = basename(argv[0]);
+
+ while ((opt = getopt(argc, argv, "fsv")) != -1) {
+ switch (opt) {
+ case 'f':
+ foreground = true;
+ break;
+ case 's':
+ use_syslog = true;
+ break;
+ case 'v':
+ verbose_log = true;
+ break;
+ default:
+ usage(progname);
+ }
+ }
+
+ qlog_setup(progname, use_syslog);
+ if (verbose_log)
+ qlog_set_min_priority(LOG_DEBUG);
+
+ if (optind < argc) {
+ addr = strtoul(argv[optind], &ep, 10);
+ if (argv[1][0] == '\0' || *ep != '\0' || addr >= UINT_MAX)
+ usage(progname);
+
+ qrtr_set_address(addr);
+ optind++;
+ }
+
+ if (optind != argc)
+ usage(progname);
+
+ w = waiter_create();
+ if (w == NULL)
+ LOGE_AND_EXIT("unable to create waiter");
+
+ list_init(&ctx.lookups);
+
+ rc = map_create(&nodes);
+ if (rc)
+ LOGE_AND_EXIT("unable to create node map");
+
+ ctx.sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (ctx.sock < 0)
+ PLOGE_AND_EXIT("unable to create control socket");
+
+ rc = getsockname(ctx.sock, (void*)&sq, &sl);
+ if (rc < 0)
+ PLOGE_AND_EXIT("getsockname()");
+ sq.sq_port = QRTR_PORT_CTRL;
+ ctx.local_node = sq.sq_node;
+
+ rc = bind(ctx.sock, (void *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGE_AND_EXIT("bind control socket");
+
+ ctx.bcast_sq.sq_family = AF_QIPCRTR;
+ ctx.bcast_sq.sq_node = QRTR_NODE_BCAST;
+ ctx.bcast_sq.sq_port = QRTR_PORT_CTRL;
+
+ rc = say_hello(&ctx);
+ if (rc)
+ PLOGE_AND_EXIT("unable to say hello");
+
+ /* If we're going to background, fork and exit parent */
+ if (!foreground && fork() != 0) {
+ close(ctx.sock);
+ exit(0);
+ }
+
+ tkt = waiter_add_fd(w, ctx.sock);
+ waiter_ticket_callback(tkt, ctrl_port_fn, &ctx);
+
+ while (ctx.sock >= 0)
+ waiter_wait(w);
+
+ puts("exiting cleanly");
+
+ waiter_destroy(w);
+
+ map_clear(&nodes, node_mi_free);
+ map_destroy(&nodes);
+
+ return 0;
+}
diff --git a/qcom/qrtr/src/ns.h b/qcom/qrtr/src/ns.h
new file mode 100644
index 0000000..d661ac8
--- /dev/null
+++ b/qcom/qrtr/src/ns.h
@@ -0,0 +1,10 @@
+#ifndef __NS_H_
+#define __NS_H_
+
+#include <endian.h>
+#include <stdint.h>
+
+static inline __le32 cpu_to_le32(uint32_t x) { return htole32(x); }
+static inline uint32_t le32_to_cpu(__le32 x) { return le32toh(x); }
+
+#endif
diff --git a/qcom/qrtr/src/util.c b/qcom/qrtr/src/util.c
new file mode 100644
index 0000000..48d9d27
--- /dev/null
+++ b/qcom/qrtr/src/util.c
@@ -0,0 +1,18 @@
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+
+#include "util.h"
+
+uint64_t time_ms(void)
+{
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+ return (uint64_t)tv.tv_sec*1000 + tv.tv_usec/1000;
+}
+
+void util_sleep(int ms)
+{
+ usleep(ms * 1000);
+}
diff --git a/qcom/qrtr/src/util.h b/qcom/qrtr/src/util.h
new file mode 100644
index 0000000..3f371cd
--- /dev/null
+++ b/qcom/qrtr/src/util.h
@@ -0,0 +1,9 @@
+#ifndef __UTIL_H_
+#define __UTIL_H_
+
+#include <stdint.h>
+
+uint64_t time_ms(void);
+void util_sleep(int ms);
+
+#endif
diff --git a/qcom/qrtr/src/waiter.c b/qcom/qrtr/src/waiter.c
new file mode 100644
index 0000000..f21896f
--- /dev/null
+++ b/qcom/qrtr/src/waiter.c
@@ -0,0 +1,378 @@
+/*
+ * Copyright (c) 2013-2014, Sony Mobile Communications Inc.
+ * Copyright (c) 2014, Courtney Cavin
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * - Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * - Neither the name of the organization nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+
+#include "list.h"
+#include "waiter.h"
+#include "util.h"
+
+struct pollset {
+ int nfds;
+ int cause;
+};
+
+static struct pollset *pollset_create(int count)
+{
+ struct pollset *ps;
+
+ ps = calloc(1, sizeof(*ps) + sizeof(struct pollfd) * count);
+ if (ps == NULL)
+ return NULL;
+
+ return ps;
+}
+
+static void pollset_destroy(struct pollset *ps)
+{
+ free(ps);
+}
+
+static void pollset_reset(struct pollset *ps)
+{
+ ps->nfds = 0;
+}
+
+static void pollset_add_fd(struct pollset *ps, int fd)
+{
+ struct pollfd *pfd = (struct pollfd *)(ps + 1);
+ pfd[ps->nfds].fd = fd;
+ pfd[ps->nfds].events = POLLERR | POLLIN;
+ ps->nfds++;
+}
+
+static int pollset_wait(struct pollset *ps, int ms)
+{
+ struct pollfd *pfd = (struct pollfd *)(ps + 1);
+ int rc;
+ int i;
+
+ rc = poll(pfd, ps->nfds, ms);
+ if (rc <= 0)
+ return rc;
+
+ ps->cause = -1;
+ for (i = 0; i < ps->nfds; ++i) {
+ if (pfd[i].revents & (POLLERR | POLLIN)) {
+ ps->cause = i;
+ break;
+ }
+ }
+ return rc;
+
+}
+
+static int pollset_cause_fd(struct pollset *ps, int fd)
+{
+ struct pollfd *pfd = (struct pollfd *)(ps + 1);
+ return (ps->cause >= 0 && pfd[ps->cause].fd == fd);
+}
+
+enum waiter_type {
+ WATCH_TYPE_NULL,
+ WATCH_TYPE_FD,
+ WATCH_TYPE_TIMEOUT,
+};
+
+struct waiter_ticket {
+ enum waiter_type type;
+ union {
+ int filedes;
+ unsigned int event;
+ unsigned int interval;
+ };
+ struct {
+ void (* fn)(void *data, struct waiter_ticket *);
+ void *data;
+ } callback;
+
+ uint64_t start;
+ int updated;
+ struct waiter *waiter;
+ struct list_item list_item;
+};
+
+struct waiter {
+ struct list tickets;
+ struct pollset *pollset;
+ int count;
+};
+
+struct waiter *waiter_create(void)
+{
+ struct waiter *w;
+
+ w = calloc(1, sizeof(*w));
+ if (w == NULL)
+ return NULL;
+
+ list_init(&w->tickets);
+ return w;
+}
+
+void waiter_destroy(struct waiter *w)
+{
+ struct waiter_ticket *ticket;
+ struct list_item *safe;
+ struct list_item *node;
+
+ list_for_each_safe(&w->tickets, node, safe) {
+ ticket = list_entry(node, struct waiter_ticket, list_item);
+ free(ticket);
+ }
+
+ if (w->pollset)
+ pollset_destroy(w->pollset);
+ free(w);
+}
+
+void waiter_synchronize(struct waiter *w)
+{
+ struct waiter_ticket *oticket;
+ struct waiter_ticket *ticket;
+ struct list_item *node;
+
+ list_for_each(&w->tickets, node) {
+ struct list_item *onode;
+ ticket = list_entry(node, struct waiter_ticket, list_item);
+
+ if (ticket->type != WATCH_TYPE_TIMEOUT)
+ continue;
+
+ list_for_each_after(node, onode) {
+ oticket = list_entry(onode, struct waiter_ticket, list_item);
+ if (oticket->type != WATCH_TYPE_TIMEOUT)
+ continue;
+
+ if (oticket->interval == ticket->interval) {
+ oticket->start = ticket->start;
+ break;
+ }
+ }
+ }
+}
+
+void waiter_wait(struct waiter *w)
+{
+ struct pollset *ps = w->pollset;
+ struct waiter_ticket *ticket;
+ struct list_item *node;
+ uint64_t term_time;
+ uint64_t now;
+ int rc;
+
+ pollset_reset(ps);
+
+ term_time = (uint64_t)-1;
+ list_for_each(&w->tickets, node) {
+ ticket = list_entry(node, struct waiter_ticket, list_item);
+ switch (ticket->type) {
+ case WATCH_TYPE_TIMEOUT:
+ if (ticket->start + ticket->interval < term_time)
+ term_time = ticket->start + ticket->interval;
+ break;
+ case WATCH_TYPE_FD:
+ pollset_add_fd(ps, ticket->filedes);
+ break;
+ case WATCH_TYPE_NULL:
+ break;
+ }
+ }
+
+ if (term_time == (uint64_t)-1) { /* wait forever */
+ rc = pollset_wait(ps, -1);
+ } else {
+ now = time_ms();
+ if (now >= term_time) { /* already past timeout, skip poll */
+ rc = 0;
+ } else {
+ uint64_t delta;
+
+ delta = term_time - now;
+ if (delta > ((1u << 31) - 1))
+ delta = ((1u << 31) - 1);
+ rc = pollset_wait(ps, (int)delta);
+ }
+ }
+
+ if (rc < 0)
+ return;
+
+ now = time_ms();
+ list_for_each(&w->tickets, node) {
+ int fresh = 0;
+
+ ticket = list_entry(node, struct waiter_ticket, list_item);
+ switch (ticket->type) {
+ case WATCH_TYPE_TIMEOUT:
+ if (now >= ticket->start + ticket->interval) {
+ ticket->start = now;
+ fresh = !ticket->updated;
+ }
+ break;
+ case WATCH_TYPE_FD:
+ if (rc == 0) /* timed-out */
+ break;
+ if (pollset_cause_fd(ps, ticket->filedes))
+ fresh = !ticket->updated;
+ break;
+ case WATCH_TYPE_NULL:
+ break;
+ }
+ if (fresh) {
+ ticket->updated = 1;
+ if (ticket->callback.fn)
+ (* ticket->callback.fn)(
+ ticket->callback.data,
+ ticket
+ );
+ }
+ }
+}
+
+int waiter_wait_timeout(struct waiter *w, unsigned int ms)
+{
+ struct waiter_ticket ticket;
+ int rc;
+
+ memset(&ticket, 0, sizeof(ticket));
+ waiter_ticket_set_timeout(&ticket, ms);
+ list_append(&w->tickets, &ticket.list_item);
+ w->count++;
+
+ waiter_wait(w);
+ rc = waiter_ticket_check(&ticket);
+
+ list_remove(&w->tickets, &ticket.list_item);
+ w->count--;
+
+ return -!rc;
+}
+
+void waiter_ticket_set_null(struct waiter_ticket *ticket)
+{
+ ticket->type = WATCH_TYPE_NULL;
+}
+
+void waiter_ticket_set_fd(struct waiter_ticket *ticket, int fd)
+{
+ ticket->type = WATCH_TYPE_FD;
+ ticket->filedes = fd;
+}
+
+void waiter_ticket_set_timeout(struct waiter_ticket *ticket, unsigned int ms)
+{
+ ticket->type = WATCH_TYPE_TIMEOUT;
+ ticket->interval = ms;
+ ticket->start = time_ms();
+}
+
+struct waiter_ticket *waiter_add_null(struct waiter *w)
+{
+ struct waiter_ticket *ticket;
+
+ ticket = calloc(1, sizeof(*ticket));
+ if (ticket == NULL)
+ return NULL;
+ ticket->waiter = w;
+
+ list_append(&w->tickets, &ticket->list_item);
+ if ((w->count % 32) == 0) {
+ if (w->pollset)
+ pollset_destroy(w->pollset);
+ w->pollset = pollset_create(w->count + 33);
+ if (w->pollset == NULL)
+ return NULL;
+ }
+ w->count++;
+
+ waiter_ticket_set_null(ticket);
+
+ return ticket;
+}
+
+struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd)
+{
+ struct waiter_ticket *ticket;
+
+ ticket = waiter_add_null(w);
+ if (ticket == NULL)
+ return NULL;
+
+ waiter_ticket_set_fd(ticket, fd);
+
+ return ticket;
+}
+
+struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms)
+{
+ struct waiter_ticket *ticket;
+
+ ticket = waiter_add_null(w);
+ if (ticket == NULL)
+ return NULL;
+
+ waiter_ticket_set_timeout(ticket, ms);
+
+ return ticket;
+}
+
+void waiter_ticket_delete(struct waiter_ticket *ticket)
+{
+ struct waiter *w = ticket->waiter;
+ list_remove(&w->tickets, &ticket->list_item);
+ w->count--;
+ free(ticket);
+}
+
+void waiter_ticket_callback(struct waiter_ticket *ticket, waiter_ticket_cb_t cb_fn, void *data)
+{
+ ticket->callback.fn = cb_fn;
+ ticket->callback.data = data;
+}
+
+int waiter_ticket_check(const struct waiter_ticket *ticket)
+{
+ return -(ticket->updated == 0);
+}
+
+int waiter_ticket_clear(struct waiter_ticket *ticket)
+{
+ int ret;
+
+ ret = waiter_ticket_check(ticket);
+ ticket->updated = 0;
+
+ return ret;
+}
diff --git a/qcom/qrtr/src/waiter.h b/qcom/qrtr/src/waiter.h
new file mode 100644
index 0000000..e311453
--- /dev/null
+++ b/qcom/qrtr/src/waiter.h
@@ -0,0 +1,103 @@
+#ifndef _WAITER_H_
+#define _WAITER_H_
+
+/** Waiter type. */
+struct waiter;
+
+/** Create a new waiter.
+ * @return Newly created waiter on success, NULL on failure.
+ */
+struct waiter *waiter_create(void);
+
+/** Destroy existing waiter.
+ * @param w waiter to destroy.
+ */
+void waiter_destroy(struct waiter *w);
+
+/** Wait for next ticket.
+ * @param w waiter.
+ */
+void waiter_wait(struct waiter *w);
+
+/** Wait for next ticket or timeout.
+ * @param w waiter.
+ * @param ms timeout in milliseconds.
+ * @return 0 on ticket; !0 on timeout.
+ */
+int waiter_wait_timeout(struct waiter *w, unsigned int ms);
+
+/** Synchronize timer-based tickets.
+ * @param w waiter.
+ */
+void waiter_synchronize(struct waiter *w);
+
+/** Waiter ticket type. */
+struct waiter_ticket;
+
+/** Add a null wait ticket to pool.
+ * @param w waiter
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_null(struct waiter *w);
+
+/** Add a file descriptor to the pool.
+ * @param w waiter.
+ * @param fd file descriptor.
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd);
+
+/** Add a timeout to the pool.
+ * @param w waiter.
+ * @param ms duration of timeout in milliseconds.
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms);
+
+/** Set ticket type to null.
+ * @param tkt wait ticket.
+ */
+void waiter_ticket_set_null(struct waiter_ticket *tkt);
+
+/** Set ticket type to file descriptor.
+ * @param tkt wait ticket.
+ * @param fd file descriptor.
+ */
+void waiter_ticket_set_fd(struct waiter_ticket *tkt, int fd);
+
+/** Set ticket type to timeout.
+ * @param tkt wait ticket.
+ * @param ms timeout in milliseconds.
+ */
+void waiter_ticket_set_timeout(struct waiter_ticket *tkt, unsigned int ms);
+
+/** Destroy ticket.
+ * @param tkt wait ticket.
+ */
+void waiter_ticket_delete(struct waiter_ticket *tkt);
+
+/** Check to see if ticket has triggered.
+ * @param tkt wait ticket.
+ * @return 0 if triggered, !0 otherwise.
+ */
+int waiter_ticket_check(const struct waiter_ticket *tkt);
+
+
+/** Clear ticket trigger status.
+ * @param tkt wait ticket.
+ * @return 0 if triggered, !0 otherwise.
+ */
+int waiter_ticket_clear(struct waiter_ticket *tkt);
+
+/** Wait ticket callback function type. */
+typedef void (* waiter_ticket_cb_t)(void *, struct waiter_ticket *);
+
+/** Register callback function for ticket trigger.
+ * @param tkt wait ticket.
+ * @param cb_fn callback function.
+ * @param data private data to pass to callback function.
+ */
+void waiter_ticket_callback(struct waiter_ticket *tkt,
+ waiter_ticket_cb_t cb_fn, void *data);
+
+#endif