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/Android.bp b/Android.bp
index 64cdc29..03c85c1 100644
--- a/Android.bp
+++ b/Android.bp
@@ -13,3 +13,10 @@
// limitations under the License.
soong_namespace {}
+
+subdirs = [
+ "qcom/pd-mapper",
+ "qcom/qrtr",
+ "qcom/rmtfs",
+ "qcom/tqftpserv",
+]
diff --git a/db845c/device.mk b/db845c/device.mk
index be50f51..f141081 100644
--- a/db845c/device.mk
+++ b/db845c/device.mk
@@ -14,6 +14,9 @@
# limitations under the License.
#
+PRODUCT_SOONG_NAMESPACES += \
+ device/linaro/dragonboard
+
# setup dalvik vm configs
$(call inherit-product, frameworks/native/build/tablet-10in-xhdpi-2048-dalvik-heap.mk)
@@ -30,5 +33,13 @@
# Build generic Audio HAL
PRODUCT_PACKAGES := audio.primary.db845c
+PRODUCT_PACKAGES += \
+ pd-mapper \
+ qrtr-ns \
+ qrtr-cfg \
+ qrtr-lookup \
+ rmtfs \
+ tqftpserv
+
# Copy firmware files
$(call inherit-product-if-exists, $(LOCAL_PATH)/firmware/device.mk)
diff --git a/qcom/pd-mapper/Android.bp b/qcom/pd-mapper/Android.bp
new file mode 100644
index 0000000..84c065f
--- /dev/null
+++ b/qcom/pd-mapper/Android.bp
@@ -0,0 +1,9 @@
+cc_binary {
+ name: "pd-mapper",
+ vendor: true,
+ srcs: [
+ "pd-mapper.c",
+ "servreg_loc.c",
+ ],
+ shared_libs: ["libqrtr"],
+}
diff --git a/qcom/pd-mapper/LICENSE b/qcom/pd-mapper/LICENSE
new file mode 100644
index 0000000..aa8cb35
--- /dev/null
+++ b/qcom/pd-mapper/LICENSE
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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.
+ */
+/*
+ * Copyright (c) 2016, Bjorn Andersson <bjorn@kryo.se>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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/pd-mapper/Makefile b/qcom/pd-mapper/Makefile
new file mode 100644
index 0000000..ca58003
--- /dev/null
+++ b/qcom/pd-mapper/Makefile
@@ -0,0 +1,26 @@
+PD_MAPPER := pd-mapper
+
+CFLAGS := -Wall -g -O2
+LDFLAGS := -lqrtr
+
+prefix ?= /usr/local
+bindir := $(prefix)/bin
+servicedir := $(prefix)/lib/systemd/system
+
+SRCS := pd-mapper.c \
+ servreg_loc.c
+
+OBJS := $(SRCS:.c=.o)
+
+$(PD_MAPPER): $(OBJS)
+ $(CC) -o $@ $^ $(LDFLAGS)
+
+pd-mapper.service: pd-mapper.service.in
+ @sed 's+PD_MAPPER_PATH+$(bindir)+g' $< > $@
+
+install: $(PD_MAPPER) pd-mapper.service
+ @install -D -m 755 $(PD_MAPPER) $(DESTDIR)$(bindir)/$(PD_MAPPER)
+ @install -D -m 644 pd-mapper.service $(DESTDIR)$(servicedir)/pd-mapper.service
+
+clean:
+ rm -f $(PD_MAPPER) $(OBJS) pd-mapper.service
diff --git a/qcom/pd-mapper/pd-mapper.c b/qcom/pd-mapper/pd-mapper.c
new file mode 100644
index 0000000..b64181e
--- /dev/null
+++ b/qcom/pd-mapper/pd-mapper.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 <errno.h>
+#include <libqrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "servreg_loc.h"
+
+struct pd_map {
+ const char *service;
+ const char *domain;
+ int instance;
+};
+
+static const struct pd_map pd_maps[] = {
+ { "kernel/elf_loader", "msm/modem/wlan_pd", 1 },
+ {}
+};
+
+static void handle_get_domain_list(int sock, const struct qrtr_packet *pkt)
+{
+ struct servreg_loc_get_domain_list_resp resp = {};
+ struct servreg_loc_get_domain_list_req req = {};
+ struct servreg_loc_domain_list_entry *entry;
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ const struct pd_map *pd_map = pd_maps;
+ unsigned int txn;
+ ssize_t len;
+ int ret;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ SERVREG_LOC_GET_DOMAIN_LIST,
+ servreg_loc_get_domain_list_req_ei);
+ if (ret < 0) {
+ resp.result.result = QMI_RESULT_FAILURE;
+ resp.result.error = QMI_ERR_MALFORMED_MSG;
+ goto respond;
+ }
+
+ req.name[sizeof(req.name)-1] = '\0';
+
+ resp.result.result = QMI_RESULT_SUCCESS;
+ resp.db_revision_valid = 1;
+ resp.db_revision = 1;
+
+ while (pd_map->service) {
+ if (!strcmp(pd_map->service, req.name)) {
+ entry = &resp.domain_list[resp.domain_list_len++];
+
+ strcpy(entry->name, pd_map->domain);
+ entry->name_len = strlen(pd_map->domain);
+ entry->instance_id = pd_map->instance;
+ }
+
+ pd_map++;
+ }
+
+ if (resp.domain_list_len)
+ resp.domain_list_valid = 1;
+
+ resp.total_domains_valid = 1;
+ resp.total_domains = resp.domain_list_len;
+
+respond:
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, SERVREG_LOC_GET_DOMAIN_LIST,
+ txn, &resp,
+ servreg_loc_get_domain_list_resp_ei);
+ if (len < 0) {
+ fprintf(stderr,
+ "[PD-MAPPER] failed to encode get_domain_list response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0) {
+ fprintf(stderr,
+ "[PD-MAPPER] failed to send get_domain_list response: %s\n",
+ strerror(-ret));
+ }
+}
+
+int main(int argc, char **argv)
+{
+ struct sockaddr_qrtr sq;
+ struct qrtr_packet pkt;
+ unsigned int msg_id;
+ socklen_t sl;
+ char buf[4096];
+ int ret;
+ int fd;
+
+ fd = qrtr_open(0);
+ if (fd < 0) {
+ fprintf(stderr, "failed to open qrtr socket\n");
+ exit(1);
+ }
+
+ ret = qrtr_publish(fd, SERVREG_QMI_SERVICE,
+ SERVREG_QMI_VERSION, SERVREG_QMI_INSTANCE);
+ if (ret < 0) {
+ fprintf(stderr, "failed to publish service registry service\n");
+ exit(1);
+ }
+
+ for (;;) {
+ ret = qrtr_poll(fd, -1);
+ if (ret < 0) {
+ if (errno == EINTR) {
+ continue;
+ } else {
+ fprintf(stderr, "qrtr_poll failed\n");
+ break;
+ }
+ }
+
+ sl = sizeof(sq);
+ ret = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (ret < 0) {
+ ret = -errno;
+ if (ret != -ENETRESET)
+ fprintf(stderr, "[PD-MAPPER] recvfrom failed: %d\n", ret);
+ return ret;
+ }
+
+ ret = qrtr_decode(&pkt, buf, ret, &sq);
+ if (ret < 0) {
+ fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
+ return ret;
+ }
+
+ switch (pkt.type) {
+ case QRTR_TYPE_DATA:
+ ret = qmi_decode_header(&pkt, &msg_id);
+ if (ret < 0)
+ continue;
+
+ switch (msg_id) {
+ case SERVREG_LOC_GET_DOMAIN_LIST:
+ handle_get_domain_list(fd, &pkt);
+ break;
+ case SERVREG_LOC_PFR:
+ printf("[PD-MAPPER] pfr\n");
+ break;
+ };
+ break;
+ };
+ }
+
+ close(fd);
+
+ return 0;
+}
diff --git a/qcom/pd-mapper/pd-mapper.service.in b/qcom/pd-mapper/pd-mapper.service.in
new file mode 100644
index 0000000..09b594b
--- /dev/null
+++ b/qcom/pd-mapper/pd-mapper.service.in
@@ -0,0 +1,11 @@
+[Unit]
+Description=Qualcomm PD mapper service
+Requires=qrtr-ns.service
+After=qrtr-ns.service
+
+[Service]
+ExecStart=PD_MAPPER_PATH/pd-mapper
+Restart=always
+
+[Install]
+WantedBy=multi-user.target
diff --git a/qcom/pd-mapper/servreg_loc.c b/qcom/pd-mapper/servreg_loc.c
new file mode 100644
index 0000000..6309498
--- /dev/null
+++ b/qcom/pd-mapper/servreg_loc.c
@@ -0,0 +1,169 @@
+#include <errno.h>
+#include <string.h>
+#include "servreg_loc.h"
+
+struct qmi_elem_info servreg_loc_qmi_result_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .offset = offsetof(struct servreg_loc_qmi_result, result),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .offset = offsetof(struct servreg_loc_qmi_result, error),
+ },
+ {}
+};
+
+struct qmi_elem_info servreg_loc_domain_list_entry_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = 256,
+ .elem_size = sizeof(char),
+ .offset = offsetof(struct servreg_loc_domain_list_entry, name)
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .offset = offsetof(struct servreg_loc_domain_list_entry, instance_id),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .offset = offsetof(struct servreg_loc_domain_list_entry, service_data_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .offset = offsetof(struct servreg_loc_domain_list_entry, service_data),
+ },
+ {}
+};
+
+struct qmi_elem_info servreg_loc_get_domain_list_req_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = 256,
+ .elem_size = sizeof(char),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 1,
+ .offset = offsetof(struct servreg_loc_get_domain_list_req, name)
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 16,
+ .offset = offsetof(struct servreg_loc_get_domain_list_req, offset_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 16,
+ .offset = offsetof(struct servreg_loc_get_domain_list_req, offset),
+ },
+ {}
+};
+
+struct qmi_elem_info servreg_loc_get_domain_list_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct servreg_loc_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, result),
+ .ei_array = servreg_loc_qmi_result_ei,
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 16,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, total_domains_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .tlv_type = 16,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, total_domains),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 17,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, db_revision_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .tlv_type = 17,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, db_revision),
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 18,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, domain_list_valid),
+ },
+ {
+ .data_type = QMI_DATA_LEN,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 18,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, domain_list_len),
+ },
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 255,
+ .elem_size = sizeof(struct servreg_loc_domain_list_entry),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 18,
+ .offset = offsetof(struct servreg_loc_get_domain_list_resp, domain_list),
+ .ei_array = servreg_loc_domain_list_entry_ei,
+ },
+ {}
+};
+
+struct qmi_elem_info servreg_loc_pfr_req_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = 256,
+ .elem_size = sizeof(char),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 1,
+ .offset = offsetof(struct servreg_loc_pfr_req, service)
+ },
+ {
+ .data_type = QMI_STRING,
+ .elem_len = 256,
+ .elem_size = sizeof(char),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 2,
+ .offset = offsetof(struct servreg_loc_pfr_req, reason)
+ },
+ {}
+};
+
+struct qmi_elem_info servreg_loc_pfr_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct servreg_loc_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct servreg_loc_pfr_resp, result),
+ .ei_array = servreg_loc_qmi_result_ei,
+ },
+ {}
+};
+
diff --git a/qcom/pd-mapper/servreg_loc.h b/qcom/pd-mapper/servreg_loc.h
new file mode 100644
index 0000000..2ac5faa
--- /dev/null
+++ b/qcom/pd-mapper/servreg_loc.h
@@ -0,0 +1,67 @@
+#ifndef __QMI_SERVREG_LOC_H__
+#define __QMI_SERVREG_LOC_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "libqrtr.h"
+
+#define SERVREG_QMI_SERVICE 64
+#define SERVREG_QMI_VERSION 257
+#define SERVREG_QMI_INSTANCE 0
+#define QMI_RESULT_SUCCESS 0
+#define QMI_RESULT_FAILURE 1
+#define QMI_ERR_NONE 0
+#define QMI_ERR_INTERNAL 1
+#define QMI_ERR_MALFORMED_MSG 2
+#define SERVREG_LOC_GET_DOMAIN_LIST 33
+#define SERVREG_LOC_PFR 36
+
+struct servreg_loc_qmi_result {
+ uint16_t result;
+ uint16_t error;
+};
+
+struct servreg_loc_domain_list_entry {
+ uint32_t name_len;
+ char name[256];
+ uint32_t instance_id;
+ uint8_t service_data_valid;
+ uint32_t service_data;
+};
+
+struct servreg_loc_get_domain_list_req {
+ uint32_t name_len;
+ char name[256];
+ bool offset_valid;
+ uint32_t offset;
+};
+
+struct servreg_loc_get_domain_list_resp {
+ struct servreg_loc_qmi_result result;
+ bool total_domains_valid;
+ uint16_t total_domains;
+ bool db_revision_valid;
+ uint16_t db_revision;
+ bool domain_list_valid;
+ uint32_t domain_list_len;
+ struct servreg_loc_domain_list_entry domain_list[255];
+};
+
+struct servreg_loc_pfr_req {
+ uint32_t service_len;
+ char service[256];
+ uint32_t reason_len;
+ char reason[256];
+};
+
+struct servreg_loc_pfr_resp {
+ struct servreg_loc_qmi_result result;
+};
+
+extern struct qmi_elem_info servreg_loc_get_domain_list_req_ei[];
+extern struct qmi_elem_info servreg_loc_get_domain_list_resp_ei[];
+extern struct qmi_elem_info servreg_loc_pfr_req_ei[];
+extern struct qmi_elem_info servreg_loc_pfr_resp_ei[];
+
+#endif
diff --git a/qcom/pd-mapper/servreg_loc.qmi b/qcom/pd-mapper/servreg_loc.qmi
new file mode 100644
index 0000000..4dc04e6
--- /dev/null
+++ b/qcom/pd-mapper/servreg_loc.qmi
@@ -0,0 +1,48 @@
+package servreg_loc;
+
+const SERVREG_QMI_SERVICE = 0x40;
+const SERVREG_QMI_VERSION = 0x101;
+const SERVREG_QMI_INSTANCE = 0x0;
+
+const QMI_RESULT_SUCCESS = 0;
+const QMI_RESULT_FAILURE = 1;
+
+const QMI_ERR_NONE = 0;
+const QMI_ERR_INTERNAL = 1;
+const QMI_ERR_MALFORMED_MSG = 2;
+
+const SERVREG_LOC_GET_DOMAIN_LIST = 0x21;
+const SERVREG_LOC_PFR = 0x24;
+
+struct qmi_result {
+ u16 result;
+ u16 error;
+};
+
+struct domain_list_entry {
+ string name;
+ u32 instance_id;
+ u8 service_data_valid;
+ u32 service_data;
+};
+
+request get_domain_list_req {
+ required string name = 1;
+ optional u32 offset = 0x10;
+} = 0x20;
+
+response get_domain_list_resp {
+ required qmi_result result = 2;
+ optional u16 total_domains = 0x10;
+ optional u16 db_revision = 0x11;
+ optional domain_list_entry domain_list[255] = 0x12;
+} = 0x20;
+
+request pfr_req {
+ required string service = 1;
+ required string reason = 2;
+} = 0x24;
+
+response pfr_resp {
+ required qmi_result result = 2;
+} = 0x24;
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
diff --git a/qcom/rmtfs/Android.bp b/qcom/rmtfs/Android.bp
new file mode 100644
index 0000000..7865a99
--- /dev/null
+++ b/qcom/rmtfs/Android.bp
@@ -0,0 +1,13 @@
+cc_binary {
+ name: "rmtfs",
+ vendor: true,
+ srcs: [
+ "qmi_rmtfs.c",
+ "rmtfs.c",
+ "rproc.c",
+ "sharedmem.c",
+ "storage.c",
+ "util.c",
+ ],
+ shared_libs: ["libqrtr"],
+}
diff --git a/qcom/rmtfs/LICENSE b/qcom/rmtfs/LICENSE
new file mode 100644
index 0000000..42c13e9
--- /dev/null
+++ b/qcom/rmtfs/LICENSE
@@ -0,0 +1,27 @@
+Copyright (c) 2016, Linaro Ltd
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+
+2. 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.
+
+3. Neither the name of the copyright holder 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/rmtfs/Makefile b/qcom/rmtfs/Makefile
new file mode 100644
index 0000000..54ae35f
--- /dev/null
+++ b/qcom/rmtfs/Makefile
@@ -0,0 +1,27 @@
+OUT := rmtfs
+
+CFLAGS += -Wall -g -O2
+LDFLAGS += -lqrtr -ludev -lpthread
+prefix = /usr/local
+bindir := $(prefix)/bin
+servicedir := $(prefix)/lib/systemd/system
+
+SRCS := qmi_rmtfs.c rmtfs.c rproc.c sharedmem.c storage.c util.c
+OBJS := $(SRCS:.c=.o)
+
+$(OUT): $(OBJS)
+ $(CC) -o $@ $^ $(LDFLAGS)
+
+%.c: %.qmi
+ qmic -k < $<
+
+rmtfs.service: rmtfs.service.in
+ @sed 's+RMTFS_PATH+$(bindir)+g' $< > $@
+
+install: $(OUT) rmtfs.service
+ @install -D -m 755 $(OUT) $(DESTDIR)$(prefix)/bin/$(OUT)
+ @install -D -m 644 rmtfs.service $(DESTDIR)$(servicedir)/rmtfs.service
+
+clean:
+ rm -f $(OUT) $(OBJS) rmtfs.service
+
diff --git a/qcom/rmtfs/qmi_rmtfs.c b/qcom/rmtfs/qmi_rmtfs.c
new file mode 100644
index 0000000..bda2b78
--- /dev/null
+++ b/qcom/rmtfs/qmi_rmtfs.c
@@ -0,0 +1,256 @@
+#include <errno.h>
+#include <string.h>
+#include "qmi_rmtfs.h"
+
+struct qmi_elem_info rmtfs_qmi_result_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .offset = offsetof(struct rmtfs_qmi_result, result),
+ },
+ {
+ .data_type = QMI_UNSIGNED_2_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint16_t),
+ .offset = offsetof(struct rmtfs_qmi_result, error),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_iovec_entry_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .offset = offsetof(struct rmtfs_iovec_entry, sector_addr),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .offset = offsetof(struct rmtfs_iovec_entry, phys_offset),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .offset = offsetof(struct rmtfs_iovec_entry, num_sector),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_open_req_ei[] = {
+ {
+ .data_type = QMI_STRING,
+ .elem_len = 256,
+ .elem_size = sizeof(char),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_open_req, path)
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_open_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct rmtfs_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_open_resp, result),
+ .ei_array = rmtfs_qmi_result_ei,
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_open_resp, caller_id_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_open_resp, caller_id),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_close_req_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_close_req, caller_id),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_close_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct rmtfs_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_close_resp, result),
+ .ei_array = rmtfs_qmi_result_ei,
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_iovec_req_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_iovec_req, caller_id),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_iovec_req, direction),
+ },
+ {
+ .data_type = QMI_DATA_LEN,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 3,
+ .offset = offsetof(struct rmtfs_iovec_req, iovec_len),
+ },
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 255,
+ .elem_size = sizeof(struct rmtfs_iovec_entry),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 3,
+ .offset = offsetof(struct rmtfs_iovec_req, iovec),
+ .ei_array = rmtfs_iovec_entry_ei,
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 4,
+ .offset = offsetof(struct rmtfs_iovec_req, is_force_sync),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_iovec_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct rmtfs_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_iovec_resp, result),
+ .ei_array = rmtfs_qmi_result_ei,
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_alloc_buf_req_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_alloc_buf_req, caller_id),
+ },
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_alloc_buf_req, buff_size),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_alloc_buf_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct rmtfs_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_alloc_buf_resp, result),
+ .ei_array = rmtfs_qmi_result_ei,
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_alloc_buf_resp, buff_address_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_8_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint64_t),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_alloc_buf_resp, buff_address),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_dev_error_req_ei[] = {
+ {
+ .data_type = QMI_UNSIGNED_4_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint32_t),
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_dev_error_req, caller_id),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_dev_error_resp_ei[] = {
+ {
+ .data_type = QMI_STRUCT,
+ .elem_len = 1,
+ .elem_size = sizeof(struct rmtfs_qmi_result),
+ .tlv_type = 2,
+ .offset = offsetof(struct rmtfs_dev_error_resp, result),
+ .ei_array = rmtfs_qmi_result_ei,
+ },
+ {
+ .data_type = QMI_OPT_FLAG,
+ .elem_len = 1,
+ .elem_size = sizeof(bool),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_dev_error_resp, status_valid),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 16,
+ .offset = offsetof(struct rmtfs_dev_error_resp, status),
+ },
+ {}
+};
+
+struct qmi_elem_info rmtfs_force_sync_ei[] = {
+ {
+ .data_type = QMI_DATA_LEN,
+ .elem_len = 1,
+ .elem_size = sizeof(uint8_t),
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_force_sync, caller_id_len),
+ },
+ {
+ .data_type = QMI_UNSIGNED_1_BYTE,
+ .elem_len = 10,
+ .elem_size = sizeof(uint32_t),
+ .array_type = VAR_LEN_ARRAY,
+ .tlv_type = 1,
+ .offset = offsetof(struct rmtfs_force_sync, caller_id),
+ },
+ {}
+};
+
diff --git a/qcom/rmtfs/qmi_rmtfs.h b/qcom/rmtfs/qmi_rmtfs.h
new file mode 100644
index 0000000..03cbc8f
--- /dev/null
+++ b/qcom/rmtfs/qmi_rmtfs.h
@@ -0,0 +1,101 @@
+#ifndef __QMI_RMTFS_H__
+#define __QMI_RMTFS_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "libqrtr.h"
+
+#define QMI_RMTFS_RESULT_SUCCESS 0
+#define QMI_RMTFS_RESULT_FAILURE 1
+#define QMI_RMTFS_ERR_NONE 0
+#define QMI_RMTFS_ERR_INTERNAL 1
+#define QMI_RMTFS_ERR_MALFORMED_MSG 2
+#define QMI_RMTFS_OPEN 1
+#define QMI_RMTFS_CLOSE 2
+#define QMI_RMTFS_RW_IOVEC 3
+#define QMI_RMTFS_ALLOC_BUFF 4
+#define QMI_RMTFS_GET_DEV_ERROR 5
+#define QMI_RMTFS_FORCE_SYNC_IND 6
+
+struct rmtfs_qmi_result {
+ uint16_t result;
+ uint16_t error;
+};
+
+struct rmtfs_iovec_entry {
+ uint32_t sector_addr;
+ uint32_t phys_offset;
+ uint32_t num_sector;
+};
+
+struct rmtfs_open_req {
+ uint32_t path_len;
+ char path[256];
+};
+
+struct rmtfs_open_resp {
+ struct rmtfs_qmi_result result;
+ bool caller_id_valid;
+ uint32_t caller_id;
+};
+
+struct rmtfs_close_req {
+ uint32_t caller_id;
+};
+
+struct rmtfs_close_resp {
+ struct rmtfs_qmi_result result;
+};
+
+struct rmtfs_iovec_req {
+ uint32_t caller_id;
+ uint8_t direction;
+ size_t iovec_len;
+ struct rmtfs_iovec_entry iovec[255];
+ uint8_t is_force_sync;
+};
+
+struct rmtfs_iovec_resp {
+ struct rmtfs_qmi_result result;
+};
+
+struct rmtfs_alloc_buf_req {
+ uint32_t caller_id;
+ uint32_t buff_size;
+};
+
+struct rmtfs_alloc_buf_resp {
+ struct rmtfs_qmi_result result;
+ bool buff_address_valid;
+ uint64_t buff_address;
+};
+
+struct rmtfs_dev_error_req {
+ uint32_t caller_id;
+};
+
+struct rmtfs_dev_error_resp {
+ struct rmtfs_qmi_result result;
+ bool status_valid;
+ uint8_t status;
+};
+
+struct rmtfs_force_sync {
+ size_t caller_id_len;
+ uint32_t caller_id[10];
+};
+
+extern struct qmi_elem_info rmtfs_open_req_ei[];
+extern struct qmi_elem_info rmtfs_open_resp_ei[];
+extern struct qmi_elem_info rmtfs_close_req_ei[];
+extern struct qmi_elem_info rmtfs_close_resp_ei[];
+extern struct qmi_elem_info rmtfs_iovec_req_ei[];
+extern struct qmi_elem_info rmtfs_iovec_resp_ei[];
+extern struct qmi_elem_info rmtfs_alloc_buf_req_ei[];
+extern struct qmi_elem_info rmtfs_alloc_buf_resp_ei[];
+extern struct qmi_elem_info rmtfs_dev_error_req_ei[];
+extern struct qmi_elem_info rmtfs_dev_error_resp_ei[];
+extern struct qmi_elem_info rmtfs_force_sync_ei[];
+
+#endif
diff --git a/qcom/rmtfs/qmi_rmtfs.qmi b/qcom/rmtfs/qmi_rmtfs.qmi
new file mode 100644
index 0000000..ca350d7
--- /dev/null
+++ b/qcom/rmtfs/qmi_rmtfs.qmi
@@ -0,0 +1,77 @@
+package rmtfs;
+
+const QMI_RMTFS_RESULT_SUCCESS = 0;
+const QMI_RMTFS_RESULT_FAILURE = 1;
+
+const QMI_RMTFS_ERR_NONE = 0;
+const QMI_RMTFS_ERR_INTERNAL = 1;
+const QMI_RMTFS_ERR_MALFORMED_MSG = 2;
+
+const QMI_RMTFS_OPEN = 1;
+const QMI_RMTFS_CLOSE = 2;
+const QMI_RMTFS_RW_IOVEC = 3;
+const QMI_RMTFS_ALLOC_BUFF = 4;
+const QMI_RMTFS_GET_DEV_ERROR = 5;
+const QMI_RMTFS_FORCE_SYNC_IND = 6;
+
+struct qmi_result {
+ u16 result;
+ u16 error;
+};
+
+struct iovec_entry {
+ u32 sector_addr;
+ u32 phys_offset;
+ u32 num_sector;
+};
+
+request open_req {
+ required string path = 1;
+} = 1;
+
+response open_resp {
+ required qmi_result result = 2;
+ optional u32 caller_id = 0x10;
+} = 1;
+
+request close_req {
+ required u32 caller_id = 1;
+} = 2;
+
+response close_resp {
+ required qmi_result result = 2;
+} = 2;
+
+request iovec_req {
+ required u32 caller_id = 1;
+ required u8 direction = 2;
+ required iovec_entry iovec(255) = 3;
+ required u8 is_force_sync = 4;
+} = 3;
+
+response iovec_resp {
+ required qmi_result result = 2;
+} = 3;
+
+request alloc_buf_req {
+ required u32 caller_id = 1;
+ required u32 buff_size = 2;
+} = 4;
+
+response alloc_buf_resp {
+ required qmi_result result = 2;
+ optional u64 buff_address = 0x10;
+} = 4;
+
+request dev_error_req {
+ required u32 caller_id = 1;
+} = 5;
+
+response dev_error_resp {
+ required qmi_result result = 2;
+ optional u8 status = 0x10;
+} = 5;
+
+indication force_sync {
+ required u32 caller_id(10) = 1;
+} = 6;
diff --git a/qcom/rmtfs/qmi_tlv.c b/qcom/rmtfs/qmi_tlv.c
new file mode 100644
index 0000000..c6d8207
--- /dev/null
+++ b/qcom/rmtfs/qmi_tlv.c
@@ -0,0 +1,233 @@
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+#include "qmi_rmtfs.h"
+
+struct qmi_packet {
+ uint8_t flags;
+ uint16_t txn_id;
+ uint16_t msg_id;
+ uint16_t msg_len;
+ uint8_t data[];
+} __attribute__((__packed__));
+
+struct qmi_tlv {
+ void *allocated;
+ void *buf;
+ size_t size;
+ int error;
+};
+
+struct qmi_tlv_item {
+ uint8_t key;
+ uint16_t len;
+ uint8_t data[];
+} __attribute__((__packed__));
+
+struct qmi_tlv *qmi_tlv_init(unsigned txn, unsigned msg_id, unsigned msg_type)
+{
+ struct qmi_packet *pkt;
+ struct qmi_tlv *tlv;
+
+ tlv = malloc(sizeof(struct qmi_tlv));
+ memset(tlv, 0, sizeof(struct qmi_tlv));
+
+ tlv->size = sizeof(struct qmi_packet);
+ tlv->allocated = malloc(tlv->size);
+ tlv->buf = tlv->allocated;
+
+ pkt = tlv->buf;
+ pkt->flags = msg_type;
+ pkt->txn_id = txn;
+ pkt->msg_id = msg_id;
+ pkt->msg_len = 0;
+
+ return tlv;
+}
+
+struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, unsigned *txn, unsigned msg_type)
+{
+ struct qmi_packet *pkt = buf;
+ struct qmi_tlv *tlv;
+
+ if (pkt->flags != msg_type)
+ return NULL;
+
+ tlv = malloc(sizeof(struct qmi_tlv));
+ memset(tlv, 0, sizeof(struct qmi_tlv));
+
+ tlv->buf = buf;
+ tlv->size = len;
+
+ if (txn)
+ *txn = pkt->txn_id;
+
+ return tlv;
+}
+
+void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len)
+{
+
+ struct qmi_packet *pkt;
+
+ if (!tlv || tlv->error)
+ return NULL;
+
+ pkt = tlv->buf;
+ pkt->msg_len = tlv->size - sizeof(struct qmi_packet);
+
+ *len = tlv->size;
+ return tlv->buf;
+}
+
+void qmi_tlv_free(struct qmi_tlv *tlv)
+{
+ free(tlv->allocated);
+ free(tlv);
+}
+
+static struct qmi_tlv_item *qmi_tlv_get_item(struct qmi_tlv *tlv, unsigned id)
+{
+ struct qmi_tlv_item *item;
+ struct qmi_packet *pkt;
+ unsigned offset = 0;
+ void *pkt_data;
+
+ pkt = tlv->buf;
+ pkt_data = pkt->data;
+
+ while (offset < tlv->size) {
+ item = pkt_data + offset;
+ if (item->key == id)
+ return pkt_data + offset;
+
+ offset += sizeof(struct qmi_tlv_item) + item->len;
+ }
+ return NULL;
+}
+
+void *qmi_tlv_get(struct qmi_tlv *tlv, unsigned id, size_t *len)
+{
+ struct qmi_tlv_item *item;
+
+ item = qmi_tlv_get_item(tlv, id);
+ if (!item)
+ return NULL;
+
+ *len = item->len;
+ return item->data;
+}
+
+void *qmi_tlv_get_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, size_t *len, size_t *size)
+{
+ struct qmi_tlv_item *item;
+ unsigned count;
+ void *ptr;
+
+ item = qmi_tlv_get_item(tlv, id);
+ if (!item)
+ return NULL;
+
+ ptr = item->data;
+ switch (len_size) {
+ case 4:
+ count = *(uint32_t*)ptr++;
+ break;
+ case 2:
+ count = *(uint16_t*)ptr++;
+ break;
+ case 1:
+ count = *(uint8_t*)ptr++;
+ break;
+ }
+
+ *len = count;
+ *size = (item->len - len_size) / count;
+
+ return ptr;
+}
+
+static struct qmi_tlv_item *qmi_tlv_alloc_item(struct qmi_tlv *tlv, unsigned id, size_t len)
+{
+ struct qmi_tlv_item *item;
+ size_t new_size;
+ bool migrate;
+ void *newp;
+
+ /* If using user provided buffer, migrate data */
+ migrate = !tlv->allocated;
+
+ new_size = tlv->size + sizeof(struct qmi_tlv_item) + len;
+ newp = realloc(tlv->allocated, new_size);
+ if (!newp)
+ return NULL;
+
+ if (migrate)
+ memcpy(newp, tlv->buf, tlv->size);
+
+ item = newp + tlv->size;
+ item->key = id;
+ item->len = len;
+
+ tlv->buf = tlv->allocated = newp;
+ tlv->size = new_size;
+
+ return item;
+}
+
+int qmi_tlv_set(struct qmi_tlv *tlv, unsigned id, void *buf, size_t len)
+{
+ struct qmi_tlv_item *item;
+
+ if (!tlv)
+ return -EINVAL;
+
+ item = qmi_tlv_alloc_item(tlv, id, len);
+ if (!item) {
+ tlv->error = ENOMEM;
+ return -ENOMEM;
+ }
+
+ memcpy(item->data, buf, len);
+
+ return 0;
+}
+
+int qmi_tlv_set_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, void *buf, size_t len, size_t size)
+{
+ struct qmi_tlv_item *item;
+ size_t array_size;
+ void *ptr;
+
+ if (!tlv)
+ return -EINVAL;
+
+ array_size = len * size;
+ item = qmi_tlv_alloc_item(tlv, id, len_size + array_size);
+ if (!item) {
+ tlv->error = ENOMEM;
+ return -ENOMEM;
+ }
+
+ ptr = item->data;
+
+ switch (len_size) {
+ case 4:
+ *(uint32_t*)ptr++ = len;
+ break;
+ case 2:
+ *(uint16_t*)ptr++ = len;
+ break;
+ case 1:
+ *(uint8_t*)ptr++ = len;
+ break;
+ }
+ memcpy(ptr, buf, array_size);
+
+ return 0;
+}
+
+
diff --git a/qcom/rmtfs/rmtfs.c b/qcom/rmtfs/rmtfs.c
new file mode 100644
index 0000000..795c021
--- /dev/null
+++ b/qcom/rmtfs/rmtfs.c
@@ -0,0 +1,564 @@
+#include <sys/mman.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <ctype.h>
+#include <dirent.h>
+#include <err.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <libqrtr.h>
+#include <limits.h>
+#include <signal.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "qmi_rmtfs.h"
+#include "util.h"
+#include "rmtfs.h"
+
+#define RMTFS_QMI_SERVICE 14
+#define RMTFS_QMI_VERSION 1
+#define RMTFS_QMI_INSTANCE 0
+
+static struct rmtfs_mem *rmem;
+static sig_atomic_t sig_int_count;
+
+static bool dbgprintf_enabled;
+static void dbgprintf(const char *fmt, ...)
+{
+ va_list ap;
+
+ if (!dbgprintf_enabled)
+ return;
+
+ va_start(ap, fmt);
+ vprintf(fmt, ap);
+ va_end(ap);
+}
+
+static void qmi_result_error(struct rmtfs_qmi_result *result, unsigned error)
+{
+ /* Only propagate initial error */
+ if (result->result == QMI_RMTFS_RESULT_FAILURE)
+ return;
+
+ result->result = QMI_RMTFS_RESULT_FAILURE;
+ result->error = error;
+}
+
+static void rmtfs_open(int sock, const struct qrtr_packet *pkt)
+{
+ struct rmtfs_open_resp resp = {};
+ struct rmtfs_open_req req = {};
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ struct rmtfd *rmtfd;
+ unsigned int txn;
+ ssize_t len;
+ int caller_id = -1;
+ int ret;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ QMI_RMTFS_OPEN, rmtfs_open_req_ei);
+ if (ret < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_MALFORMED_MSG);
+ goto respond;
+ }
+
+ rmtfd = storage_open(pkt->node, req.path);
+ if (!rmtfd) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ caller_id = storage_get_caller_id(rmtfd);
+ resp.caller_id = caller_id;
+ resp.caller_id_valid = true;
+
+respond:
+ dbgprintf("[RMTFS] open %s => %d (%d:%d)\n",
+ req.path, caller_id, resp.result.result, resp.result.error);
+
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, QMI_RMTFS_OPEN, txn, &resp,
+ rmtfs_open_resp_ei);
+ if (len < 0) {
+ fprintf(stderr, "[RMTFS] failed to encode open-response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0)
+ fprintf(stderr, "[RMTFS] failed to send open-response: %s\n",
+ strerror(-ret));
+}
+
+static void rmtfs_close(int sock, const struct qrtr_packet *pkt)
+{
+ struct rmtfs_close_resp resp = {};
+ struct rmtfs_close_req req = {};
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ struct rmtfd *rmtfd;
+ unsigned int txn;
+ ssize_t len;
+ int ret;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ QMI_RMTFS_CLOSE, rmtfs_close_req_ei);
+ if (ret < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_MALFORMED_MSG);
+ goto respond;
+ }
+
+ rmtfd = storage_get(pkt->node, req.caller_id);
+ if (!rmtfd) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ storage_close(rmtfd);
+ rmtfs_mem_free(rmem);
+
+respond:
+ dbgprintf("[RMTFS] close %s => %d (%d:%d)\n",
+ req.caller_id, resp.result.result, resp.result.error);
+
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, QMI_RMTFS_CLOSE, txn, &resp,
+ rmtfs_close_resp_ei);
+ if (len < 0) {
+ fprintf(stderr, "[RMTFS] failed to encode close-response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0)
+ fprintf(stderr, "[RMTFS] failed to send close-response: %s\n",
+ strerror(-ret));
+}
+
+static void rmtfs_iovec(int sock, struct qrtr_packet *pkt)
+{
+ struct rmtfs_iovec_entry *entries;
+ struct rmtfs_iovec_resp resp = {};
+ struct rmtfs_iovec_req req = {};
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ struct rmtfd *rmtfd;
+ uint32_t caller_id = 0;
+ size_t num_entries = 0;
+ off_t sector_base;
+ uint8_t is_write;
+ off_t phys_base;
+ uint8_t force = 0;
+ unsigned txn;
+ off_t offset;
+ ssize_t len;
+ ssize_t n;
+ char buf[SECTOR_SIZE];
+ int ret;
+ int i;
+ int j;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ QMI_RMTFS_RW_IOVEC, rmtfs_iovec_req_ei);
+ if (ret < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_MALFORMED_MSG);
+ goto respond;
+ }
+
+ caller_id = req.caller_id;
+ is_write = req.direction;
+ entries = req.iovec;
+ num_entries = req.iovec_len;
+ force = req.is_force_sync;
+
+ rmtfd = storage_get(pkt->node, caller_id);
+ if (!rmtfd) {
+ fprintf(stderr, "[RMTFS] iovec request for non-existing caller\n");
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ for (i = 0; i < num_entries; i++) {
+ phys_base = entries[i].phys_offset;
+ sector_base = entries[i].sector_addr * SECTOR_SIZE;
+ offset = 0;
+
+ for (j = 0; j < entries[i].num_sector; j++) {
+ if (is_write) {
+ n = rmtfs_mem_read(rmem, phys_base + offset, buf, SECTOR_SIZE);
+ if (n == SECTOR_SIZE)
+ n = storage_pwrite(rmtfd, buf, n, sector_base + offset);
+ } else {
+ n = storage_pread(rmtfd, buf, SECTOR_SIZE, sector_base + offset);
+ if (n >= 0) {
+ if (n < SECTOR_SIZE)
+ memset(buf + n, 0, SECTOR_SIZE - n);
+ n = rmtfs_mem_write(rmem, phys_base + offset, buf, SECTOR_SIZE);
+ }
+ }
+
+ if (n != SECTOR_SIZE) {
+ fprintf(stderr, "[RMTFS] failed to %s sector %d\n",
+ is_write ? "write" : "read", entries[i].sector_addr + j);
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ offset += SECTOR_SIZE;
+ }
+ }
+
+respond:
+ dbgprintf("[RMTFS] iovec %d, %sforced => (%d:%d)\n", caller_id, force ? "" : "not ",
+ resp.result.result, resp.result.error);
+ for (i = 0; i < num_entries; i++) {
+ dbgprintf("[RMTFS] %s %d:%d 0x%x\n", is_write ? "write" : "read",
+ entries[i].sector_addr,
+ entries[i].num_sector,
+ entries[i].phys_offset);
+ }
+
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, QMI_RMTFS_RW_IOVEC, txn, &resp,
+ rmtfs_iovec_resp_ei);
+ if (len < 0) {
+ fprintf(stderr, "[RMTFS] failed to encode iovec-response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0)
+ fprintf(stderr, "[RMTFS] failed to send iovec-response: %s\n",
+ strerror(-ret));
+}
+
+static void rmtfs_alloc_buf(int sock, struct qrtr_packet *pkt)
+{
+ struct rmtfs_alloc_buf_resp resp = {};
+ struct rmtfs_alloc_buf_req req = {};
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ uint32_t alloc_size = 0;
+ uint32_t caller_id = 0;
+ int64_t address = 0;
+ unsigned txn;
+ ssize_t len;
+ int ret;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ QMI_RMTFS_ALLOC_BUFF, rmtfs_alloc_buf_req_ei);
+ if (ret < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_MALFORMED_MSG);
+ goto respond;
+ }
+
+ caller_id = req.caller_id;
+ alloc_size = req.buff_size;
+
+ address = rmtfs_mem_alloc(rmem, alloc_size);
+ if (address < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ resp.buff_address = address;
+ resp.buff_address_valid = true;
+respond:
+ dbgprintf("[RMTFS] alloc %d, %d => 0x%lx (%d:%d)\n", caller_id, alloc_size, address, resp.result.result, resp.result.error);
+
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, QMI_RMTFS_ALLOC_BUFF, txn, &resp,
+ rmtfs_alloc_buf_resp_ei);
+ if (len < 0) {
+ fprintf(stderr, "[RMTFS] failed to encode alloc-buf-response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0)
+ fprintf(stderr, "[RMTFS] failed to send alloc-buf-response: %s\n",
+ strerror(-ret));
+}
+
+static void rmtfs_get_dev_error(int sock, struct qrtr_packet *pkt)
+{
+ struct rmtfs_dev_error_resp resp = {};
+ struct rmtfs_dev_error_req req = {};
+ DEFINE_QRTR_PACKET(resp_buf, 256);
+ struct rmtfd *rmtfd;
+ unsigned txn;
+ ssize_t len;
+ int ret;
+
+ ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
+ QMI_RMTFS_GET_DEV_ERROR,
+ rmtfs_dev_error_req_ei);
+ if (ret < 0) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_MALFORMED_MSG);
+ goto respond;
+ }
+
+ rmtfd = storage_get(pkt->node, req.caller_id);
+ if (rmtfd) {
+ qmi_result_error(&resp.result, QMI_RMTFS_ERR_INTERNAL);
+ goto respond;
+ }
+
+ resp.status = storage_get_error(rmtfd);
+ resp.status_valid = true;
+
+respond:
+ dbgprintf("[RMTFS] dev_error %d => %d (%d:%d)\n", req.caller_id, resp.status, resp.result.result, resp.result.error);
+
+ len = qmi_encode_message(&resp_buf,
+ QMI_RESPONSE, QMI_RMTFS_GET_DEV_ERROR, txn,
+ &resp, rmtfs_dev_error_resp_ei);
+ if (len < 0) {
+ fprintf(stderr, "[RMTFS] failed to encode dev-error-response: %s\n",
+ strerror(-len));
+ return;
+ }
+
+ ret = qrtr_sendto(sock, pkt->node, pkt->port,
+ resp_buf.data, resp_buf.data_len);
+ if (ret < 0)
+ fprintf(stderr, "[RMTFS] failed to send dev-error-response: %s\n",
+ strerror(-ret));
+}
+
+static int rmtfs_bye(uint32_t node)
+{
+ dbgprintf("[RMTFS] bye from %d\n", node);
+
+ return 0;
+}
+
+static int rmtfs_del_client(uint32_t node, uint32_t port)
+{
+ dbgprintf("[RMTFS] del_client %d:%d\n", node, port);
+
+ return 0;
+}
+
+static int handle_rmtfs(int sock)
+{
+ struct sockaddr_qrtr sq;
+ struct qrtr_packet pkt;
+ unsigned int msg_id;
+ socklen_t sl;
+ char buf[4096];
+ int ret;
+
+ sl = sizeof(sq);
+ ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (ret < 0) {
+ ret = -errno;
+ if (ret != -ENETRESET)
+ fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret);
+ return ret;
+ }
+
+ dbgprintf("[RMTFS] packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
+
+ ret = qrtr_decode(&pkt, buf, ret, &sq);
+ if (ret < 0) {
+ fprintf(stderr, "[RMTFS] unable to decode qrtr packet\n");
+ return ret;
+ }
+
+ switch (pkt.type) {
+ case QRTR_TYPE_BYE:
+ return rmtfs_bye(pkt.node);
+ case QRTR_TYPE_DEL_CLIENT:
+ return rmtfs_del_client(pkt.node, pkt.port);
+ case QRTR_TYPE_DATA:
+ ret = qmi_decode_header(&pkt, &msg_id);
+ if (ret < 0)
+ return ret;
+
+ switch (msg_id) {
+ case QMI_RMTFS_OPEN:
+ rmtfs_open(sock, &pkt);
+ break;
+ case QMI_RMTFS_CLOSE:
+ rmtfs_close(sock, &pkt);
+ break;
+ case QMI_RMTFS_RW_IOVEC:
+ rmtfs_iovec(sock, &pkt);
+ break;
+ case QMI_RMTFS_ALLOC_BUFF:
+ rmtfs_alloc_buf(sock, &pkt);
+ break;
+ case QMI_RMTFS_GET_DEV_ERROR:
+ rmtfs_get_dev_error(sock, &pkt);
+ break;
+ default:
+ fprintf(stderr, "[RMTFS] Unknown request: %d\n", msg_id);
+ break;
+ }
+
+ return 0;
+ }
+
+ return ret;
+}
+
+static int sig_int_count;
+
+static int run_rmtfs(int rprocfd)
+{
+ bool sig_int_handled = false;
+ int rmtfs_fd;
+ fd_set rfds;
+ char done;
+ int nfds;
+ int ret;
+
+ rmtfs_fd = qrtr_open(RMTFS_QMI_SERVICE);
+ if (rmtfs_fd < 0) {
+ fprintf(stderr, "failed to create qrtr socket\n");
+ return rmtfs_fd;
+ }
+
+ dbgprintf("registering services\n");
+
+ ret = qrtr_publish(rmtfs_fd, RMTFS_QMI_SERVICE,
+ RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE);
+ if (ret < 0) {
+ fprintf(stderr, "failed to publish rmtfs service");
+ return ret;
+ }
+
+ if (rprocfd >= 0)
+ rproc_start();
+
+ for (;;) {
+ if (rprocfd >= 0 && sig_int_count == 1 && !sig_int_handled) {
+ rproc_stop();
+ sig_int_handled = true;
+ } else if (sig_int_count > 1) {
+ break;
+ }
+
+ FD_ZERO(&rfds);
+ FD_SET(rmtfs_fd, &rfds);
+ if (rprocfd >= 0)
+ FD_SET(rprocfd, &rfds);
+ nfds = MAX(rmtfs_fd, rprocfd) + 1;
+
+ ret = select(nfds, &rfds, NULL, NULL, NULL);
+ if (ret < 0 && errno != EINTR)
+ break;
+ else if (ret < 0 && errno == EINTR)
+ continue;
+
+ if (rprocfd >= 0 && FD_ISSET(rprocfd, &rfds)) {
+ ret = read(rprocfd, &done, 1);
+ if (!ret || done == 'Y')
+ break;
+ }
+
+ if (FD_ISSET(rmtfs_fd, &rfds)) {
+ ret = handle_rmtfs(rmtfs_fd);
+ if (ret == -ENETRESET)
+ break;
+ }
+ }
+
+ close(rmtfs_fd);
+
+ return ret;
+}
+
+static void sig_int_handler(int signo)
+{
+ sig_int_count++;
+}
+
+int main(int argc, char **argv)
+{
+ struct sigaction action;
+ bool use_partitions = false;
+ bool read_only = false;
+ int rprocfd = -1;
+ int ret;
+ int option;
+ const char *storage_root = NULL;
+
+ while ((option = getopt(argc, argv, "o:Prsv")) != -1) {
+ switch (option) {
+ /* -o sets the directory where EFS images are stored. */
+ case 'o':
+ storage_root = optarg;
+ break;
+
+ /* -P to find and use raw EFS partitions */
+ case 'P':
+ use_partitions = true;
+ break;
+
+ /* -r to avoid writing to storage */
+ case 'r':
+ read_only = true;
+ break;
+
+ /* enable sync for the mss rproc instance */
+ case 's':
+ rprocfd = rproc_init();
+ if (rprocfd < 0) {
+ fprintf(stderr, "Failed to get rprocfd\n");
+ return 1;
+ }
+
+ break;
+
+ /* -v is for verbose */
+ case 'v':
+ dbgprintf_enabled = 1;
+ break;
+
+ case '?':
+ fprintf(stderr, "Unknown option: -%c\n", option);
+ return 1;
+ }
+ }
+
+ sigemptyset(&action.sa_mask);
+ action.sa_handler = sig_int_handler;
+ action.sa_flags = 0;
+
+ sigaction(SIGINT, &action, NULL);
+ sigaction(SIGTERM, &action, NULL);
+
+ rmem = rmtfs_mem_open();
+ if (!rmem)
+ return 1;
+
+ ret = storage_init(storage_root, read_only, use_partitions);
+ if (ret) {
+ fprintf(stderr, "failed to initialize storage system\n");
+ goto close_rmtfs_mem;
+ }
+
+ do {
+ ret = run_rmtfs(rprocfd);
+ } while (ret == -ENETRESET);
+
+ storage_exit();
+close_rmtfs_mem:
+ rmtfs_mem_close(rmem);
+
+ return 0;
+}
diff --git a/qcom/rmtfs/rmtfs.h b/qcom/rmtfs/rmtfs.h
new file mode 100644
index 0000000..242baa5
--- /dev/null
+++ b/qcom/rmtfs/rmtfs.h
@@ -0,0 +1,42 @@
+#ifndef __RMTFS_H__
+#define __RMTFS_H__
+
+#include <stdint.h>
+#include "qmi_rmtfs.h"
+
+#define SECTOR_SIZE 512
+
+struct qmi_packet {
+ uint8_t flags;
+ uint16_t txn_id;
+ uint16_t msg_id;
+ uint16_t msg_len;
+ uint8_t data[];
+} __attribute__((__packed__));
+
+struct rmtfs_mem;
+
+struct rmtfs_mem *rmtfs_mem_open(void);
+void rmtfs_mem_close(struct rmtfs_mem *rmem);
+int64_t rmtfs_mem_alloc(struct rmtfs_mem *rmem, size_t size);
+void rmtfs_mem_free(struct rmtfs_mem *rmem);
+ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len);
+ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len);
+
+struct rmtfd;
+
+int storage_init(const char *storage_root, bool read_only, bool use_partitions);
+struct rmtfd *storage_open(unsigned node, const char *path);
+struct rmtfd *storage_get(unsigned node, int caller_id);
+void storage_close(struct rmtfd *rmtfd);
+int storage_get_caller_id(const struct rmtfd *rmtfd);
+int storage_get_error(const struct rmtfd *rmtfd);
+void storage_exit(void);
+ssize_t storage_pread(const struct rmtfd *rmtfd, void *buf, size_t nbyte, off_t offset);
+ssize_t storage_pwrite(struct rmtfd *rmtfd, const void *buf, size_t nbyte, off_t offset);
+
+int rproc_init(void);
+int rproc_start(void);
+int rproc_stop(void);
+
+#endif
diff --git a/qcom/rmtfs/rmtfs.service.in b/qcom/rmtfs/rmtfs.service.in
new file mode 100644
index 0000000..8384043
--- /dev/null
+++ b/qcom/rmtfs/rmtfs.service.in
@@ -0,0 +1,12 @@
+[Unit]
+Description=Qualcomm remotefs service
+Requires=qrtr-ns.service
+After=qrtr-ns.service
+
+[Service]
+ExecStart=RMTFS_PATH/rmtfs -r -P -s
+Restart=always
+RestartSec=1
+
+[Install]
+WantedBy=multi-user.target
diff --git a/qcom/rmtfs/rproc.c b/qcom/rmtfs/rproc.c
new file mode 100644
index 0000000..a471b3c
--- /dev/null
+++ b/qcom/rmtfs/rproc.c
@@ -0,0 +1,137 @@
+#include <sys/syscall.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <pthread.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "rmtfs.h"
+
+#define RPROC_BASE_PATH "/sys/bus/platform/drivers/qcom-q6v5-mss/"
+
+static pthread_t start_thread;
+static pthread_t stop_thread;
+static int rproc_state_fd;
+static int rproc_pipe[2];
+
+int rproc_init(void)
+{
+ struct dirent *device_de;
+ struct dirent *rproc_de;
+ int rproc_base_fd;
+ DIR *rproc_dir;
+ DIR *base_dir;
+ int device_fd;
+ int rproc_fd;
+ int base_fd;
+ int ret;
+
+ rproc_state_fd = -1;
+
+ base_fd = open(RPROC_BASE_PATH, O_RDONLY | O_DIRECTORY);
+ if (base_fd < 0)
+ return -1;
+
+ base_dir = fdopendir(base_fd);
+ if (!base_dir) {
+ fprintf(stderr, "failed to open mss driver path\n");
+ close(base_fd);
+ return -1;
+ }
+
+ while (rproc_state_fd < 0 && (device_de = readdir(base_dir)) != NULL) {
+ if (!strcmp(device_de->d_name, ".") ||
+ !strcmp(device_de->d_name, ".."))
+ continue;
+
+ device_fd = openat(base_fd, device_de->d_name, O_RDONLY | O_DIRECTORY);
+ if (device_fd < 0)
+ continue;
+
+ rproc_base_fd = openat(device_fd, "remoteproc", O_RDONLY | O_DIRECTORY);
+ if (rproc_base_fd < 0) {
+ close(device_fd);
+ continue;
+ }
+
+ rproc_dir = fdopendir(rproc_base_fd);
+ while (rproc_state_fd < 0 && (rproc_de = readdir(rproc_dir)) != NULL) {
+ if (!strcmp(rproc_de->d_name, ".") ||
+ !strcmp(rproc_de->d_name, ".."))
+ continue;
+
+ rproc_fd = openat(rproc_base_fd, rproc_de->d_name, O_RDONLY | O_DIRECTORY);
+ if (rproc_fd < 0)
+ continue;
+
+ rproc_state_fd = openat(rproc_fd, "state", O_WRONLY);
+ if (rproc_state_fd < 0) {
+ fprintf(stderr,
+ "unable to open remoteproc \"state\" control file of %s\n",
+ device_de->d_name);
+ }
+
+ close(rproc_fd);
+
+ }
+ closedir(rproc_dir);
+ close(rproc_base_fd);
+ close(device_fd);
+ }
+ closedir(base_dir);
+ close(base_fd);
+
+ if (rproc_state_fd < 0)
+ return -1;
+
+ ret = pipe(rproc_pipe);
+ if (ret < 0) {
+ close(rproc_state_fd);
+ return -1;
+ }
+
+ return rproc_pipe[0];
+}
+
+static void *do_rproc_start(void *unused)
+{
+ ssize_t ret;
+
+ ret = pwrite(rproc_state_fd, "start", 5, 0);
+ if (ret < 4)
+ fprintf(stderr, "failed to update start state\n");
+
+ return NULL;
+}
+
+int rproc_start()
+{
+ return pthread_create(&start_thread, NULL, do_rproc_start, NULL);
+}
+
+static void *do_rproc_stop(void *unused)
+{
+ ssize_t ret;
+
+ ret = pwrite(rproc_state_fd, "stop", 4, 0);
+ if (ret < 4)
+ fprintf(stderr, "failed to update stop state\n");
+
+ ret = write(rproc_pipe[1], "Y", 1);
+ if (ret != 1) {
+ fprintf(stderr, "failed to signal event loop about exit\n");
+ exit(0);
+ }
+
+ return NULL;
+}
+
+int rproc_stop(void)
+{
+ return pthread_create(&stop_thread, NULL, do_rproc_stop, NULL);
+}
diff --git a/qcom/rmtfs/sharedmem.c b/qcom/rmtfs/sharedmem.c
new file mode 100644
index 0000000..66bbd5d
--- /dev/null
+++ b/qcom/rmtfs/sharedmem.c
@@ -0,0 +1,460 @@
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <errno.h>
+#include <fcntl.h>
+#ifndef ANDROID
+#include <libudev.h>
+#else
+#include <sys/endian.h>
+#endif
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits.h>
+#include <string.h>
+#include <unistd.h>
+#include "rmtfs.h"
+
+static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem);
+
+struct rmtfs_mem {
+ uint64_t address;
+ uint64_t size;
+ void *base;
+ int fd;
+};
+
+#ifndef ANDROID
+
+static int parse_hex_sysattr(struct udev_device *dev, const char *name,
+ uint64_t *value)
+{
+ unsigned long long val;
+ const char *buf;
+ char *endptr;
+
+ buf = udev_device_get_sysattr_value(dev, name);
+ if (!buf)
+ return -ENOENT;
+
+ errno = 0;
+ val = strtoull(buf, &endptr, 16);
+ if ((val == ULLONG_MAX && errno == ERANGE) || endptr == buf) {
+ return -errno;
+ }
+
+ *value = val;
+
+ return 0;
+}
+
+static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id)
+{
+ struct udev_device *dev;
+ struct udev *udev;
+ int saved_errno;
+ struct stat sb;
+ char path[32];
+ int ret;
+ int fd;
+
+ sprintf(path, "/dev/qcom_rmtfs_mem%d", client_id);
+
+ fd = open(path, O_RDWR);
+ if (fd < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
+ return -saved_errno;
+ }
+ rmem->fd = fd;
+
+ ret = fstat(fd, &sb);
+ if (ret < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to stat %s: %s\n", path, strerror(errno));
+ close(fd);
+ goto err_close_fd;
+ }
+
+ udev = udev_new();
+ if (!udev) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to create udev context\n");
+ goto err_close_fd;
+ }
+
+ dev = udev_device_new_from_devnum(udev, 'c', sb.st_rdev);
+ if (!dev) {
+ saved_errno = errno;
+ fprintf(stderr, "unable to find udev device\n");
+ goto err_unref_udev;
+ }
+
+ ret = parse_hex_sysattr(dev, "phys_addr", &rmem->address);
+ if (ret < 0) {
+ fprintf(stderr, "failed to parse phys_addr of %s\n", path);
+ saved_errno = -ret;
+ goto err_unref_dev;
+ }
+
+ ret = parse_hex_sysattr(dev, "size", &rmem->size);
+ if (ret < 0) {
+ fprintf(stderr, "failed to parse size of %s\n", path);
+ saved_errno = -ret;
+ goto err_unref_dev;
+ }
+
+ udev_device_unref(dev);
+ udev_unref(udev);
+
+ return 0;
+
+err_unref_dev:
+ udev_device_unref(dev);
+err_unref_udev:
+ udev_unref(udev);
+err_close_fd:
+ close(fd);
+ return -saved_errno;
+}
+
+static int rmtfs_mem_open_uio(struct rmtfs_mem *rmem, int client_id)
+{
+ struct udev_device *dev;
+ struct udev *udev;
+ int saved_errno;
+ struct stat sb;
+ char path[32];
+ int ret;
+ int fd;
+
+ snprintf(path, sizeof(path), "/dev/qcom_rmtfs_uio%d", client_id);
+
+ fd = open(path, O_RDWR);
+ if (fd < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
+ return -saved_errno;
+ }
+ rmem->fd = fd;
+
+ ret = fstat(fd, &sb);
+ if (ret < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to stat %s: %s\n", path, strerror(errno));
+ close(fd);
+ goto err_close_fd;
+ }
+
+ udev = udev_new();
+ if (!udev) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to create udev context\n");
+ goto err_close_fd;
+ }
+
+ dev = udev_device_new_from_devnum(udev, 'c', sb.st_rdev);
+ if (!dev) {
+ saved_errno = errno;
+ fprintf(stderr, "unable to find udev device\n");
+ goto err_unref_udev;
+ }
+
+ ret = parse_hex_sysattr(dev, "maps/map0/addr", &rmem->address);
+ if (ret < 0) {
+ fprintf(stderr, "failed to parse phys_addr of %s\n", path);
+ saved_errno = -ret;
+ goto err_unref_dev;
+ }
+
+ ret = parse_hex_sysattr(dev, "maps/map0/size", &rmem->size);
+ if (ret < 0) {
+ fprintf(stderr, "failed to parse size of %s\n", path);
+ saved_errno = -ret;
+ goto err_unref_dev;
+ }
+
+ rmem->base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
+ if (rmem->base == MAP_FAILED) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to mmap: %s\n", strerror(errno));
+ goto err_unref_dev;
+ }
+
+ udev_device_unref(dev);
+ udev_unref(udev);
+
+ return 0;
+
+err_unref_dev:
+ udev_device_unref(dev);
+err_unref_udev:
+ udev_unref(udev);
+err_close_fd:
+ close(fd);
+ return -saved_errno;
+}
+
+#else
+
+#define PAGE_SIZE 4096
+
+static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id)
+{
+ int saved_errno;
+ int fd;
+ char path[PATH_MAX];
+ char val[PAGE_SIZE];
+ char *endptr;
+
+ errno = 0;
+
+ snprintf(path, sizeof(path), "/sys/class/rmtfs/qcom_rmtfs_mem%d/phys_addr", client_id);
+ fd = open(path, O_RDONLY);
+ if (fd < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
+ return -saved_errno;
+ }
+ read(fd, val, sizeof(val));
+ rmem->address = strtoull(val, &endptr, 16);
+ if ((rmem->address == ULLONG_MAX && errno == ERANGE) || endptr == val) {
+ saved_errno = errno;
+ goto err_close_fd;
+ }
+ close(fd);
+
+ snprintf(path, sizeof(path), "/sys/class/rmtfs/qcom_rmtfs_mem%d/size", client_id);
+ fd = open(path, O_RDONLY);
+ if (fd < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
+ return -saved_errno;
+ }
+ read(fd, val, sizeof(val));
+ rmem->size = strtoull(val, &endptr, 16);
+ if ((rmem->size == ULLONG_MAX && errno == ERANGE) || endptr == val) {
+ saved_errno = errno;
+ goto err_close_fd;
+ }
+ close(fd);
+
+ return 0;
+
+err_close_fd:
+ close(fd);
+ return -saved_errno;
+}
+
+static int rmtfs_mem_open_uio(struct rmtfs_mem *rmem, int client_id)
+{
+ fprintf(stderr, "uio access is not supported on ANDROID yet\n");
+ return -EINVAL;
+}
+
+#endif
+
+struct rmtfs_mem *rmtfs_mem_open(void)
+{
+ struct rmtfs_mem *rmem;
+ void *base;
+ int ret;
+ int fd;
+
+ rmem = malloc(sizeof(*rmem));
+ if (!rmem)
+ return NULL;
+
+ memset(rmem, 0, sizeof(*rmem));
+
+ ret = rmtfs_mem_open_rfsa(rmem, 1);
+ if (ret < 0 && ret != -ENOENT) {
+ goto err;
+ } else if (ret < 0) {
+ fprintf(stderr, "falling back to uio access\n");
+ ret = rmtfs_mem_open_uio(rmem, 1);
+ if (ret < 0 && ret != -ENOENT) {
+ goto err;
+ } else if (ret < 0) {
+ fprintf(stderr, "falling back to /dev/mem access\n");
+
+ ret = rmtfs_mem_enumerate(rmem);
+ if (ret < 0)
+ goto err;
+
+ fd = open("/dev/mem", O_RDWR|O_SYNC);
+ if (fd < 0) {
+ fprintf(stderr, "failed to open /dev/mem\n");
+ goto err;
+ }
+
+ base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, rmem->address);
+ if (base == MAP_FAILED) {
+ fprintf(stderr, "failed to mmap: %s\n", strerror(errno));
+ goto err_close_fd;
+ }
+
+ rmem->base = base;
+ rmem->fd = fd;
+ }
+ }
+
+ return rmem;
+
+err_close_fd:
+ close(fd);
+err:
+ free(rmem);
+ return NULL;
+}
+
+int64_t rmtfs_mem_alloc(struct rmtfs_mem *rmem, size_t alloc_size)
+{
+ if (alloc_size > rmem->size) {
+ fprintf(stderr,
+ "[RMTFS] rmtfs shared memory not large enough for allocation request 0x%zx vs 0x%lx\n",
+ alloc_size, rmem->size);
+ return -EINVAL;
+ }
+
+ return rmem->address;
+}
+
+void rmtfs_mem_free(struct rmtfs_mem *rmem)
+{
+}
+
+static void *rmtfs_mem_ptr(struct rmtfs_mem *rmem, unsigned long phys_address, ssize_t len)
+{
+ uint64_t start;
+ uint64_t end;
+
+ if (len < 0)
+ return NULL;
+
+ start = phys_address;
+ end = start + len;
+
+ if (start < rmem->address || end > rmem->address + rmem->size)
+ return NULL;
+
+ return rmem->base + phys_address - rmem->address;
+}
+
+ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len)
+{
+ off_t offset;
+ void *ptr;
+
+ if (rmem->base) {
+ ptr = rmtfs_mem_ptr(rmem, phys_address, len);
+ if (!ptr)
+ return -EINVAL;
+
+ memcpy(buf, ptr, len);
+ } else {
+ offset = phys_address - rmem->address;
+ len = pread(rmem->fd, buf, len, offset);
+ }
+
+ return len;
+}
+
+ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len)
+{
+ off_t offset;
+ void *ptr;
+
+ if (rmem->base) {
+ ptr = rmtfs_mem_ptr(rmem, phys_address, len);
+ if (!ptr)
+ return -EINVAL;
+
+ memcpy(ptr, buf, len);
+ } else {
+ offset = phys_address - rmem->address;
+ len = pwrite(rmem->fd, buf, len, offset);
+ }
+
+ return len;
+}
+
+void rmtfs_mem_close(struct rmtfs_mem *rmem)
+{
+ if (rmem->base)
+ munmap(rmem->base, rmem->size);
+
+ close(rmem->fd);
+
+ free(rmem);
+}
+
+static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem)
+{
+ union {
+ uint32_t dw[2];
+ uint64_t qw[2];
+ } reg;
+ struct dirent *de;
+ int basefd;
+ int dirfd;
+ int regfd;
+ DIR *dir;
+ int ret = 0;
+ int n;
+
+ basefd = open("/proc/device-tree/reserved-memory/", O_DIRECTORY);
+ dir = fdopendir(basefd);
+ if (!dir) {
+ fprintf(stderr,
+ "Unable to open reserved-memory device tree node: %s\n",
+ strerror(-errno));
+ close(basefd);
+ return -1;
+ }
+
+ while ((de = readdir(dir)) != NULL) {
+ if (strncmp(de->d_name, "rmtfs", 5) != 0)
+ continue;
+
+ dirfd = openat(basefd, de->d_name, O_DIRECTORY);
+ if (dirfd < 0) {
+ fprintf(stderr, "failed to open %s: %s\n",
+ de->d_name, strerror(-errno));
+ ret = -1;
+ goto out;
+ }
+
+ regfd = openat(dirfd, "reg", O_RDONLY);
+ if (regfd < 0) {
+ fprintf(stderr, "failed to open reg of %s: %s\n",
+ de->d_name, strerror(-errno));
+ ret = -1;
+ goto out;
+ }
+
+ n = read(regfd, ®, sizeof(reg));
+ if (n == 2 * sizeof(uint32_t)) {
+ rmem->address = be32toh(reg.dw[0]);
+ rmem->size = be32toh(reg.dw[1]);
+ } else if (n == 2 * sizeof(uint64_t)) {
+ rmem->address = be64toh(reg.qw[0]);
+ rmem->size = be64toh(reg.qw[1]);
+ } else {
+ fprintf(stderr, "failed to read reg of %s: %s\n",
+ de->d_name, strerror(-errno));
+ ret = -1;
+ }
+
+ close(regfd);
+ close(dirfd);
+ break;
+ }
+
+out:
+ closedir(dir);
+ close(basefd);
+ return ret;
+}
diff --git a/qcom/rmtfs/storage.c b/qcom/rmtfs/storage.c
new file mode 100644
index 0000000..d31f757
--- /dev/null
+++ b/qcom/rmtfs/storage.c
@@ -0,0 +1,291 @@
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include "rmtfs.h"
+
+#define MAX_CALLERS 10
+#define STORAGE_MAX_SIZE (16 * 1024 * 1024)
+
+#ifndef ANDROID
+#define BY_PARTLABEL_PATH "/dev/disk/by-partlabel"
+#else
+#define BY_PARTLABEL_PATH "/dev/block/by-name"
+#endif
+
+#define MIN(x, y) ((x) < (y) ? (x) : (y))
+
+struct partition {
+ const char *path;
+ const char *actual;
+ const char *partlabel;
+};
+
+struct rmtfd {
+ unsigned id;
+ unsigned node;
+ int fd;
+ unsigned dev_error;
+ const struct partition *partition;
+
+ void *shadow_buf;
+ size_t shadow_len;
+};
+
+static const char *storage_dir = "/boot";
+static int storage_read_only;
+static int storage_use_partitions;
+
+static const struct partition partition_table[] = {
+ { "/boot/modem_fs1", "modem_fs1", "modemst1" },
+ { "/boot/modem_fs2", "modem_fs2", "modemst2" },
+ { "/boot/modem_fsc", "modem_fsc", "fsc" },
+ { "/boot/modem_fsg", "modem_fsg", "fsg" },
+ {}
+};
+
+static struct rmtfd rmtfds[MAX_CALLERS];
+
+static int storage_populate_shadow_buf(struct rmtfd *rmtfd, const char *file);
+
+int storage_init(const char *storage_root, bool read_only, bool use_partitions)
+{
+ int i;
+
+ if (storage_root)
+ storage_dir = storage_root;
+
+ if (use_partitions) {
+ storage_dir = BY_PARTLABEL_PATH;
+ storage_use_partitions = true;
+ }
+
+ storage_read_only = read_only;
+
+ for (i = 0; i < MAX_CALLERS; i++) {
+ rmtfds[i].id = i;
+ rmtfds[i].fd = -1;
+ rmtfds[i].shadow_buf = NULL;
+ }
+
+ return 0;
+}
+
+struct rmtfd *storage_open(unsigned node, const char *path)
+{
+ char *fspath;
+ const struct partition *part;
+ struct rmtfd *rmtfd = NULL;
+ const char *file;
+ size_t pathlen;
+ int saved_errno;
+ int ret;
+ int fd;
+ int i;
+
+ for (part = partition_table; part->path; part++) {
+ if (strcmp(part->path, path) == 0)
+ goto found;
+ }
+
+ fprintf(stderr, "[RMTFS storage] request for unknown partition '%s', rejecting\n", path);
+ return NULL;
+
+found:
+ /* Check if this node already has the requested path open */
+ for (i = 0; i < MAX_CALLERS; i++) {
+ if ((rmtfds[i].fd != -1 || rmtfds[i].shadow_buf) &&
+ rmtfds[i].node == node &&
+ rmtfds[i].partition == part)
+ return &rmtfds[i];
+ }
+
+ for (i = 0; i < MAX_CALLERS; i++) {
+ if (rmtfds[i].fd == -1 && !rmtfds[i].shadow_buf) {
+ rmtfd = &rmtfds[i];
+ break;
+ }
+ }
+ if (!rmtfd) {
+ fprintf(stderr, "[storage] out of free rmtfd handles\n");
+ return NULL;
+ }
+
+ if (storage_use_partitions)
+ file = part->partlabel;
+ else
+ file = part->actual;
+
+ pathlen = strlen(storage_dir) + strlen(file) + 2;
+ fspath = alloca(pathlen);
+ snprintf(fspath, pathlen, "%s/%s", storage_dir, file);
+ if (!storage_read_only) {
+ fd = open(fspath, O_RDWR);
+ if (fd < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "[storage] failed to open '%s' (requested '%s'): %s\n",
+ fspath, part->path, strerror(saved_errno));
+ errno = saved_errno;
+ return NULL;
+ }
+ rmtfd->fd = fd;
+ rmtfd->shadow_len = 0;
+ } else {
+ ret = storage_populate_shadow_buf(rmtfd, fspath);
+ if (ret < 0) {
+ saved_errno = errno;
+ fprintf(stderr, "[storage] failed to open '%s' (requested '%s'): %s\n",
+ fspath, part->path, strerror(saved_errno));
+ errno = saved_errno;
+ return NULL;
+ }
+ }
+
+ rmtfd->node = node;
+ rmtfd->partition = part;
+
+ return rmtfd;
+}
+
+void storage_close(struct rmtfd *rmtfd)
+{
+ close(rmtfd->fd);
+ rmtfd->fd = -1;
+
+ free(rmtfd->shadow_buf);
+ rmtfd->shadow_buf = NULL;
+ rmtfd->shadow_len = 0;
+
+ rmtfd->partition = NULL;
+}
+
+struct rmtfd *storage_get(unsigned node, int caller_id)
+{
+ struct rmtfd *rmtfd;
+
+ if (caller_id >= MAX_CALLERS)
+ return NULL;
+
+ rmtfd = &rmtfds[caller_id];
+ if (rmtfd->node != node)
+ return NULL;
+
+ return rmtfd;
+}
+
+int storage_get_caller_id(const struct rmtfd *rmtfd)
+{
+ return rmtfd->id;
+}
+
+int storage_get_error(const struct rmtfd *rmtfd)
+{
+ return rmtfd->dev_error;
+}
+
+void storage_exit(void)
+{
+ int i;
+
+ for (i = 0; i < MAX_CALLERS; i++) {
+ if (rmtfds[i].fd >= 0)
+ close(rmtfds[i].fd);
+ }
+}
+
+ssize_t storage_pread(const struct rmtfd *rmtfd, void *buf, size_t nbyte, off_t offset)
+{
+ ssize_t n;
+
+ if (!storage_read_only) {
+ n = pread(rmtfd->fd, buf, nbyte, offset);
+ } else {
+ n = MIN(nbyte, rmtfd->shadow_len - offset);
+ if (n > 0)
+ memcpy(buf, rmtfd->shadow_buf + offset, n);
+ else
+ n = 0;
+ }
+
+ if (n < nbyte)
+ memset(buf + n, 0, nbyte - n);
+
+ return nbyte;
+}
+
+ssize_t storage_pwrite(struct rmtfd *rmtfd, const void *buf, size_t nbyte, off_t offset)
+{
+ size_t new_len = offset + nbyte;
+ void *new_buf;
+
+ if (!storage_read_only)
+ return pwrite(rmtfd->fd, buf, nbyte, offset);
+
+ if (new_len >= STORAGE_MAX_SIZE) {
+ fprintf(stderr, "write to %zd bytes exceededs max size\n", new_len);
+ errno = -EINVAL;
+ return -1;
+ }
+
+ if (new_len > rmtfd->shadow_len) {
+ new_buf = realloc(rmtfd->shadow_buf, new_len);
+ if (!new_buf) {
+ errno = -ENOMEM;
+ return -1;
+ }
+
+ rmtfd->shadow_buf = new_buf;
+ rmtfd->shadow_len = new_len;
+ }
+
+ memcpy(rmtfd->shadow_buf + offset, buf, nbyte);
+
+ return nbyte;
+}
+
+static int storage_populate_shadow_buf(struct rmtfd *rmtfd, const char *file)
+{
+ ssize_t len;
+ ssize_t n;
+ void *buf;
+ int ret;
+ int fd;
+
+ fd = open(file, O_RDONLY);
+ if (fd < 0)
+ return -1;
+
+ len = lseek(fd, 0, SEEK_END);
+ if (len < 0) {
+ ret = -1;
+ goto err_close_fd;
+ }
+
+ lseek(fd, 0, SEEK_SET);
+
+ buf = calloc(1, len);
+ if (!buf) {
+ ret = -1;
+ goto err_close_fd;
+ }
+
+ n = read(fd, buf, len);
+ if (n < 0) {
+ ret = -1;
+ goto err_close_fd;
+ }
+
+ rmtfd->shadow_buf = buf;
+ rmtfd->shadow_len = n;
+
+ ret = 0;
+
+err_close_fd:
+ close(fd);
+
+ return ret;
+}
diff --git a/qcom/rmtfs/util.c b/qcom/rmtfs/util.c
new file mode 100644
index 0000000..5ca5bba
--- /dev/null
+++ b/qcom/rmtfs/util.c
@@ -0,0 +1,48 @@
+#include <ctype.h>
+#include <stdint.h>
+#include <stdio.h>
+#include "util.h"
+
+static uint8_t to_hex(uint8_t ch)
+{
+ ch &= 0xf;
+ return ch <= 9 ? '0' + ch : 'a' + ch - 10;
+}
+
+void print_hex_dump(const char *prefix, const void *buf, size_t len)
+{
+ const uint8_t *ptr = buf;
+ size_t linelen;
+ uint8_t ch;
+ char line[16 * 3 + 16 + 1];
+ int li;
+ int i;
+ int j;
+
+ for (i = 0; i < len; i += 16) {
+ linelen = MIN(16, len - i);
+ li = 0;
+
+ for (j = 0; j < linelen; j++) {
+ ch = ptr[i + j];
+ line[li++] = to_hex(ch >> 4);
+ line[li++] = to_hex(ch);
+ line[li++] = ' ';
+ }
+
+ for (; j < 16; j++) {
+ line[li++] = ' ';
+ line[li++] = ' ';
+ line[li++] = ' ';
+ }
+
+ for (j = 0; j < linelen; j++) {
+ ch = ptr[i + j];
+ line[li++] = isprint(ch) ? ch : '.';
+ }
+
+ line[li] = '\0';
+
+ printf("%s %04x: %s\n", prefix, i, line);
+ }
+}
diff --git a/qcom/rmtfs/util.h b/qcom/rmtfs/util.h
new file mode 100644
index 0000000..3a9ec3e
--- /dev/null
+++ b/qcom/rmtfs/util.h
@@ -0,0 +1,9 @@
+#ifndef __UTIL_H__
+#define __UTIL_H__
+
+#define MIN(x, y) ((x) < (y) ? (x) : (y))
+#define MAX(x, y) ((x) > (y) ? (x) : (y))
+
+void print_hex_dump(const char *prefix, const void *buf, size_t len);
+
+#endif
diff --git a/qcom/tqftpserv/Android.bp b/qcom/tqftpserv/Android.bp
new file mode 100644
index 0000000..b2c5090
--- /dev/null
+++ b/qcom/tqftpserv/Android.bp
@@ -0,0 +1,9 @@
+cc_binary {
+ name: "tqftpserv",
+ vendor: true,
+ srcs: [
+ "tqftpserv.c",
+ "translate.c",
+ ],
+ shared_libs: ["libqrtr"],
+}
diff --git a/qcom/tqftpserv/LICENSE b/qcom/tqftpserv/LICENSE
new file mode 100644
index 0000000..cd92517
--- /dev/null
+++ b/qcom/tqftpserv/LICENSE
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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/tqftpserv/Makefile b/qcom/tqftpserv/Makefile
new file mode 100644
index 0000000..60ad687
--- /dev/null
+++ b/qcom/tqftpserv/Makefile
@@ -0,0 +1,25 @@
+TQFTPSERV := tqftpserv
+
+CFLAGS := -Wall -g -O2
+LDFLAGS := -lqrtr
+
+prefix ?= /usr/local
+bindir := $(prefix)/bin
+servicedir := $(prefix)/lib/systemd/system
+
+SRCS := tqftpserv.c translate.c
+
+OBJS := $(SRCS:.c=.o)
+
+$(TQFTPSERV): $(OBJS)
+ $(CC) -o $@ $^ $(LDFLAGS)
+
+tqftpserv.service: tqftpserv.service.in
+ @sed 's+TQFTPSERV_PATH+$(bindir)+g' $< > $@
+
+install: $(TQFTPSERV) tqftpserv.service
+ @install -D -m 755 $(TQFTPSERV) $(DESTDIR)$(bindir)/$(TQFTPSERV)
+ @install -D -m 644 tqftpserv.service $(DESTDIR)$(servicedir)/tqftpserv.service
+
+clean:
+ rm -f $(TQFTPSERV) $(OBJS) tqftpserv.service
diff --git a/qcom/tqftpserv/list.h b/qcom/tqftpserv/list.h
new file mode 100644
index 0000000..c3488e3
--- /dev/null
+++ b/qcom/tqftpserv/list.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2016, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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.
+ */
+#ifndef __LIST_H__
+#define __LIST_H__
+
+#include <stdbool.h>
+#include <stddef.h>
+
+#ifndef container_of
+#define container_of(ptr, type, member) ({ \
+ const typeof(((type *)0)->member)*__mptr = (ptr); \
+ (type *)((char *)__mptr - offsetof(type, member)); \
+ })
+#endif
+
+struct list_head {
+ struct list_head *prev;
+ struct list_head *next;
+};
+
+#define LIST_INIT(list) { &(list), &(list) }
+
+static inline void list_init(struct list_head *list)
+{
+ list->prev = list->next = list;
+}
+
+static inline bool list_empty(struct list_head *list)
+{
+ return list->next == list;
+}
+
+static inline void list_add(struct list_head *list, struct list_head *item)
+{
+ struct list_head *prev = list->prev;
+
+ item->next = list;
+ item->prev = prev;
+
+ prev->next = list->prev = item;
+}
+
+static inline void list_del(struct list_head *item)
+{
+ item->prev->next = item->next;
+ item->next->prev = item->prev;
+}
+
+#define list_for_each(item, list) \
+ for (item = (list)->next; item != list; item = item->next)
+
+#define list_for_each_safe(item, next, list) \
+ for (item = (list)->next, next = item->next; item != list; item = next, next = item->next)
+
+#define list_entry(item, type, member) \
+ container_of(item, type, member)
+
+#define list_entry_first(list, type, member) \
+ container_of((list)->next, type, member)
+
+#define list_entry_next(item, member) \
+ container_of((item)->member.next, typeof(*(item)), member)
+
+#define list_for_each_entry(item, list, member) \
+ for (item = list_entry_first(list, typeof(*(item)), member); \
+ &item->member != list; \
+ item = list_entry_next(item, member))
+
+#define list_for_each_entry_safe(item, next, list, member) \
+ for (item = list_entry_first(list, typeof(*(item)), member), \
+ next = list_entry_next(item, member); \
+ &item->member != list; \
+ item = next, \
+ next = list_entry_next(item, member)) \
+
+#endif
diff --git a/qcom/tqftpserv/tqftpserv.c b/qcom/tqftpserv/tqftpserv.c
new file mode 100644
index 0000000..55b667f
--- /dev/null
+++ b/qcom/tqftpserv/tqftpserv.c
@@ -0,0 +1,609 @@
+/*
+ * Copyright (c) 2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 <arpa/inet.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <libgen.h>
+#include <libqrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "list.h"
+#include "translate.h"
+
+#define MAX(x, y) ((x) > (y) ? (x) : (y))
+
+enum {
+ OP_RRQ = 1,
+ OP_WRQ,
+ OP_DATA,
+ OP_ACK,
+ OP_ERROR,
+ OP_OACK,
+};
+
+struct tftp_client {
+ struct list_head node;
+
+ struct sockaddr_qrtr sq;
+
+ int sock;
+ int fd;
+
+ size_t block;
+
+ size_t blksize;
+ size_t rsize;
+ size_t wsize;
+ unsigned int timeoutms;
+};
+
+static struct list_head readers = LIST_INIT(readers);
+static struct list_head writers = LIST_INIT(writers);
+
+static ssize_t tftp_send_data(struct tftp_client *client,
+ unsigned int block, size_t offset)
+{
+ ssize_t len;
+ char *buf;
+ char *p;
+
+ buf = malloc(4 + client->blksize);
+ p = buf;
+
+ *p++ = 0;
+ *p++ = OP_DATA;
+
+ *p++ = (block >> 8) & 0xff;
+ *p++ = block & 0xff;
+
+ len = pread(client->fd, p, client->blksize, offset);
+ if (len <= 0) {
+ if (len < 0)
+ printf("[TQFTP] failed to read data\n");
+ free(buf);
+ return len;
+ }
+
+ p += len;
+
+ // printf("[TQFTP] Sending %zd bytes of DATA\n", p - buf);
+ len = send(client->sock, buf, p - buf, 0);
+
+ free(buf);
+
+ return len;
+}
+
+
+static int tftp_send_ack(int sock, int block)
+{
+ struct {
+ uint16_t opcode;
+ uint16_t block;
+ } ack = { htons(OP_ACK), htons(block) };
+
+ return send(sock, &ack, sizeof(ack), 0);
+}
+
+static int tftp_send_oack(int sock, size_t *blocksize, size_t *tsize,
+ size_t *wsize, unsigned int *timeoutms, size_t *rsize)
+{
+ char buf[512];
+ char *p = buf;
+ int n;
+
+ *p++ = 0;
+ *p++ = OP_OACK;
+
+ if (blocksize) {
+ strcpy(p, "blksize");
+ p += 8;
+
+ n = sprintf(p, "%zd", *blocksize);
+ p += n;
+ *p++ = '\0';
+ }
+
+ if (timeoutms) {
+ strcpy(p, "timeoutms");
+ p += 10;
+
+ n = sprintf(p, "%d", *timeoutms);
+ p += n;
+ *p++ = '\0';
+ }
+
+ if (tsize && *tsize != -1) {
+ strcpy(p, "tsize");
+ p += 6;
+
+ n = sprintf(p, "%zd", *tsize);
+ p += n;
+ *p++ = '\0';
+ }
+
+ if (wsize) {
+ strcpy(p, "wsize");
+ p += 6;
+
+ n = sprintf(p, "%zd", *wsize);
+ p += n;
+ *p++ = '\0';
+ }
+
+ if (rsize) {
+ strcpy(p, "rsize");
+ p += 6;
+
+ n = sprintf(p, "%zd", *rsize);
+ p += n;
+ *p++ = '\0';
+ }
+
+ return send(sock, buf, p - buf, 0);
+}
+
+static int tftp_send_error(int sock, int code, const char *msg)
+{
+ size_t len;
+ char *buf;
+ int rc;
+
+ len = 4 + strlen(msg) + 1;
+
+ buf = calloc(1, len);
+ if (!buf)
+ return -1;
+
+ *(uint16_t*)buf = htons(OP_ERROR);
+ *(uint16_t*)(buf + 2) = htons(code);
+ strcpy(buf + 4, msg);
+
+ rc = send(sock, buf, len, 0);
+ free(buf);
+ return rc;
+}
+
+static void handle_rrq(const char *buf, size_t len, struct sockaddr_qrtr *sq)
+{
+ struct tftp_client *client;
+ const char *filename;
+ const char *value;
+ const char *mode;
+ const char *opt;
+ struct stat sb;
+ const char *p;
+ ssize_t tsize = -1;
+ size_t blksize = 512;
+ unsigned int timeoutms = 1000;
+ size_t rsize = 0;
+ size_t wsize = 0;
+ bool do_oack = false;
+ int sock;
+ int ret;
+ int fd;
+
+ p = buf + 2;
+
+ filename = p;
+ p += strlen(p) + 1;
+
+ mode = p;
+ p += strlen(p) + 1;
+
+ if (strcasecmp(mode, "octet")) {
+ /* XXX: error */
+ printf("[TQFTP] not octet, reject\n");
+ return;
+ }
+
+ printf("[TQFTP] RRQ: %s (%s)\n", filename, mode);
+
+ if (p < buf + len) {
+ do_oack = true;
+
+ while (p < buf + len) {
+ /* XXX: ensure we're not running off the end */
+ opt = p;
+ p += strlen(p) + 1;
+
+ /* XXX: ensure we're not running off the end */
+ value = p;
+ p += strlen(p) + 1;
+
+ if (!strcmp(opt, "blksize")) {
+ blksize = atoi(value);
+ } else if (!strcmp(opt, "timeoutms")) {
+ timeoutms = atoi(value);
+ } else if (!strcmp(opt, "tsize")) {
+ tsize = atoi(value);
+ } else if (!strcmp(opt, "rsize")) {
+ rsize = atoi(value);
+ } else if (!strcmp(opt, "wsize")) {
+ wsize = atoi(value);
+ } else {
+ printf("[TQFTP] Ignoring unknown option '%s'\n", opt);
+ }
+ }
+ }
+
+ sock = qrtr_open(0);
+ if (sock < 0) {
+ /* XXX: error */
+ printf("[TQFTP] unable to create new qrtr socket, reject\n");
+ return;
+ }
+
+ ret = connect(sock, (struct sockaddr *)sq, sizeof(*sq));
+ if (ret < 0) {
+ /* XXX: error */
+ printf("[TQFTP] unable to connect new qrtr socket to remote\n");
+ return;
+ }
+
+ fd = translate_open(filename, O_RDONLY);
+ if (fd < 0) {
+ printf("[TQFTP] unable to open %s (%d), reject\n", filename, errno);
+ tftp_send_error(sock, 1, "file not found");
+ return;
+ }
+
+ if (tsize != -1) {
+ fstat(fd, &sb);
+ tsize = sb.st_size;
+ }
+
+ client = calloc(1, sizeof(*client));
+ client->sq = *sq;
+ client->sock = sock;
+ client->fd = fd;
+ client->blksize = blksize;
+ client->rsize = rsize;
+ client->wsize = wsize;
+ client->timeoutms = timeoutms;
+
+ // printf("[TQFTP] new reader added\n");
+
+ list_add(&readers, &client->node);
+
+ if (do_oack) {
+ tftp_send_oack(client->sock, &blksize,
+ tsize ? (size_t*)&tsize : NULL,
+ wsize ? &wsize : NULL,
+ &client->timeoutms,
+ rsize ? &rsize: NULL);
+ } else {
+ tftp_send_data(client, 1, 0);
+ }
+}
+
+static void handle_wrq(const char *buf, size_t len, struct sockaddr_qrtr *sq)
+{
+ struct tftp_client *client;
+ const char *filename;
+ const char *mode;
+ int sock;
+ int ret;
+ int fd;
+
+ filename = buf + 2;
+ mode = buf + 2 + strlen(filename) + 1;
+
+ if (strcasecmp(mode, "octet")) {
+ /* XXX: error */
+ printf("[TQFTP] not octet, reject\n");
+ return;
+ }
+
+ printf("[TQFTP] WRQ: %s (%s)\n", filename, mode);
+
+ fd = translate_open(filename, O_WRONLY | O_CREAT);
+ if (fd < 0) {
+ /* XXX: error */
+ printf("[TQFTP] unable to open %s (%d), reject\n", filename, errno);
+ return;
+ }
+
+ sock = qrtr_open(0);
+ if (sock < 0) {
+ /* XXX: error */
+ printf("[TQFTP] unable to create new qrtr socket, reject\n");
+ return;
+ }
+
+ ret = connect(sock, (struct sockaddr *)sq, sizeof(*sq));
+ if (ret < 0) {
+ /* XXX: error */
+ printf("[TQFTP] unable to connect new qrtr socket to remote\n");
+ return;
+ }
+
+ client = calloc(1, sizeof(*client));
+ client->sq = *sq;
+ client->sock = sock;
+ client->fd = fd;
+
+ ret = tftp_send_ack(client->sock, 0);
+ if (ret < 0) {
+ printf("[TQFTP] unable to send ack\n");
+ close(sock);
+ close(fd);
+ free(client);
+ return;
+ }
+
+ // printf("[TQFTP] new writer added\n");
+
+ list_add(&writers, &client->node);
+}
+
+static int handle_reader(struct tftp_client *client)
+{
+ struct sockaddr_qrtr sq;
+ uint16_t block;
+ uint16_t last;
+ char buf[128];
+ socklen_t sl;
+ ssize_t len;
+ ssize_t n = 0;
+ int opcode;
+ int ret;
+
+ sl = sizeof(sq);
+ len = recvfrom(client->sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (len < 0) {
+ ret = -errno;
+ if (ret != -ENETRESET)
+ fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
+ return -1;
+ }
+
+ /* Drop unsolicited messages */
+ if (sq.sq_node != client->sq.sq_node ||
+ sq.sq_port != client->sq.sq_port) {
+ printf("[TQFTP] Discarding spoofed message\n");
+ return -1;
+ }
+
+ opcode = buf[0] << 8 | buf[1];
+ if (opcode == OP_ERROR) {
+ buf[len] = '\0';
+ printf("[TQFTP] Remote returned an error: %s\n", buf + 4);
+ return -1;
+ } else if (opcode != OP_ACK) {
+ printf("[TQFTP] Expected ACK, got %d\n", opcode);
+ return -1;
+ }
+
+ last = buf[2] << 8 | buf[3];
+ // printf("[TQFTP] Got ack for %d\n", last);
+
+ for (block = last; block < last + client->wsize; block++) {
+ n = tftp_send_data(client, block + 1,
+ block * client->blksize);
+ if (n < 0) {
+ printf("[TQFTP] Sent block %d failed: %zd\n", block + 1, n);
+ break;
+ }
+ // printf("[TQFTP] Sent block %d of %zd\n", block + 1, n);
+ if (n == 0)
+ break;
+ }
+
+ return 1;
+}
+
+static int handle_writer(struct tftp_client *client)
+{
+ struct sockaddr_qrtr sq;
+ uint16_t block;
+ size_t payload;
+ char buf[516];
+ socklen_t sl;
+ ssize_t len;
+ int opcode;
+ int ret;
+
+ sl = sizeof(sq);
+ len = recvfrom(client->sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (len < 0) {
+ ret = -errno;
+ if (ret != -ENETRESET)
+ fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
+ return -1;
+ }
+
+ /* Drop unsolicited messages */
+ if (sq.sq_node != client->sq.sq_node ||
+ sq.sq_port != client->sq.sq_port)
+ return -1;
+
+ opcode = buf[0] << 8 | buf[1];
+ block = buf[2] << 8 | buf[3];
+ if (opcode != OP_DATA) {
+ printf("[TQFTP] Expected DATA opcode, got %d\n", opcode);
+ tftp_send_error(client->sock, 4, "Expected DATA opcode");
+ return -1;
+ }
+
+ payload = len - 4;
+
+ ret = write(client->fd, buf + 4, payload);
+ if (ret < 0) {
+ /* XXX: report error */
+ printf("[TQFTP] failed to write data\n");
+ return -1;
+ }
+
+ tftp_send_ack(client->sock, block);
+
+ return payload == 512 ? 1 : 0;
+}
+
+static void client_close_and_free(struct tftp_client *client)
+{
+ list_del(&client->node);
+ close(client->sock);
+ close(client->fd);
+ free(client);
+}
+
+int main(int argc, char **argv)
+{
+ struct tftp_client *client;
+ struct tftp_client *next;
+ struct sockaddr_qrtr sq;
+ struct qrtr_packet pkt;
+ socklen_t sl;
+ ssize_t len;
+ char buf[4096];
+ fd_set rfds;
+ int nfds;
+ int opcode;
+ int ret;
+ int fd;
+
+ fd = qrtr_open(0);
+ if (fd < 0) {
+ fprintf(stderr, "failed to open qrtr socket\n");
+ exit(1);
+ }
+
+ ret = qrtr_publish(fd, 4096, 1, 0);
+ if (ret < 0) {
+ fprintf(stderr, "failed to publish service registry service\n");
+ exit(1);
+ }
+
+ for (;;) {
+ FD_ZERO(&rfds);
+ FD_SET(fd, &rfds);
+ nfds = fd;
+
+ list_for_each_entry(client, &writers, node) {
+ FD_SET(client->sock, &rfds);
+ nfds = MAX(nfds, client->sock);
+ }
+
+ list_for_each_entry(client, &readers, node) {
+ FD_SET(client->sock, &rfds);
+ nfds = MAX(nfds, client->sock);
+ }
+
+ ret = select(nfds + 1, &rfds, NULL, NULL, NULL);
+ if (ret < 0) {
+ if (errno == EINTR) {
+ continue;
+ } else {
+ fprintf(stderr, "select failed\n");
+ break;
+ }
+ }
+
+ list_for_each_entry_safe(client, next, &writers, node) {
+ if (FD_ISSET(client->sock, &rfds)) {
+ ret = handle_writer(client);
+ if (ret <= 0)
+ client_close_and_free(client);
+ }
+ }
+
+ list_for_each_entry_safe(client, next, &readers, node) {
+ if (FD_ISSET(client->sock, &rfds)) {
+ ret = handle_reader(client);
+ if (ret <= 0)
+ client_close_and_free(client);
+ }
+ }
+
+ if (FD_ISSET(fd, &rfds)) {
+ sl = sizeof(sq);
+ len = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
+ if (len < 0) {
+ ret = -errno;
+ if (ret != -ENETRESET)
+ fprintf(stderr, "[TQFTP] recvfrom failed: %d\n", ret);
+ return ret;
+ }
+
+ /* Ignore control messages */
+ if (sq.sq_port == QRTR_PORT_CTRL) {
+ ret = qrtr_decode(&pkt, buf, len, &sq);
+ if (ret < 0) {
+ fprintf(stderr, "[TQFTP] unable to decode qrtr packet\n");
+ return ret;
+ }
+
+ switch (pkt.type) {
+ case QRTR_TYPE_BYE:
+ // fprintf(stderr, "[TQFTP] got bye\n");
+ list_for_each_entry_safe(client, next, &writers, node) {
+ if (client->sq.sq_node == sq.sq_node)
+ client_close_and_free(client);
+ }
+ break;
+ case QRTR_TYPE_DEL_CLIENT:
+ // fprintf(stderr, "[TQFTP] got del_client\n");
+ list_for_each_entry_safe(client, next, &writers, node) {
+ if (!memcmp(&client->sq, &sq, sizeof(sq)))
+ client_close_and_free(client);
+ }
+ break;
+ }
+ } else {
+ if (len < 2)
+ continue;
+
+ opcode = buf[0] << 8 | buf[1];
+ switch (opcode) {
+ case OP_RRQ:
+ handle_rrq(buf, len, &sq);
+ break;
+ case OP_WRQ:
+ // printf("[TQFTP] write\n");
+ handle_wrq(buf, len, &sq);
+ break;
+ default:
+ printf("[TQFTP] unhandled op %d\n", opcode);
+ break;
+ }
+ }
+ }
+ }
+
+ close(fd);
+
+ return 0;
+}
diff --git a/qcom/tqftpserv/tqftpserv.service.in b/qcom/tqftpserv/tqftpserv.service.in
new file mode 100644
index 0000000..2cf828c
--- /dev/null
+++ b/qcom/tqftpserv/tqftpserv.service.in
@@ -0,0 +1,12 @@
+[Unit]
+Description=QRTR TFTP service
+Requires=qrtr-ns.service
+After=qrtr-ns.service
+
+[Service]
+ExecStart=TQFTPSERV_PATH/tqftpserv
+Restart=always
+
+[Install]
+WantedBy=multi-user.target
+
diff --git a/qcom/tqftpserv/translate.c b/qcom/tqftpserv/translate.c
new file mode 100644
index 0000000..e95dee5
--- /dev/null
+++ b/qcom/tqftpserv/translate.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2019, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 <sys/stat.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <err.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <libgen.h>
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "translate.h"
+
+#define READONLY_PATH "/readonly/firmware/image/"
+#define READWRITE_PATH "/readwrite/"
+
+#define FIRMWARE_BASE "/lib/firmware/"
+
+/**
+ * translate_readonly() - open "file" residing with remoteproc firmware
+ * @file: file requested, stripped of "/readonly/image/" prefix
+ *
+ * It is assumed that the readonly files requested by the client resides under
+ * /lib/firmware in the same place as its associated remoteproc firmware. This
+ * function scans through all entries under /sys/class/remoteproc and read the
+ * dirname of each "firmware" file in an attempt to find, and open(2), the
+ * requested file.
+ *
+ * As these files are readonly, it's not possible to pass flags to open(2).
+ *
+ * Return: opened fd on success, -1 otherwise
+ */
+static int translate_readonly(const char *file)
+{
+ char firmware_value[PATH_MAX];
+ char firmware_attr[32];
+ char path[PATH_MAX];
+ struct dirent *de;
+ int firmware_fd;
+ DIR *class_dir;
+ int class_fd;
+ ssize_t n;
+ int fd = -1;
+
+ class_fd = open("/sys/class/remoteproc", O_RDONLY | O_DIRECTORY);
+ if (class_fd < 0) {
+ warn("failed to open remoteproc class");
+ return -1;
+ }
+
+ class_dir = fdopendir(class_fd);
+ if (!class_dir) {
+ warn("failed to opendir");
+ goto close_class;
+ }
+
+ while ((de = readdir(class_dir)) != NULL) {
+ if (!strcmp(de->d_name, ".") || !strcmp(de->d_name, ".."))
+ continue;
+
+ if (strlen(de->d_name) + sizeof("/firmware") > sizeof(firmware_attr))
+ continue;
+ strcpy(firmware_attr, de->d_name);
+ strcat(firmware_attr, "/firmware");
+
+ firmware_fd = openat(class_fd, firmware_attr, O_RDONLY);
+ if (firmware_fd < 0)
+ continue;
+
+ n = read(firmware_fd, firmware_value, sizeof(firmware_value));
+ close(firmware_fd);
+ if (n < 0) {
+ continue;
+ }
+
+ if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 +
+ strlen(file) + 1 > sizeof(path))
+ continue;
+
+ strcpy(path, FIRMWARE_BASE);
+ strcat(path, dirname(firmware_value));
+ strcat(path, "/");
+ strcat(path, file);
+
+ fd = open(path, O_RDONLY);
+ if (fd >= 0)
+ break;
+
+ if (errno != ENOENT)
+ warn("failed to open %s", path);
+ }
+
+ closedir(class_dir);
+
+close_class:
+ close(class_fd);
+
+ return fd;
+}
+
+/**
+ * translate_readwrite() - open "file" from a temporary directory
+ * @file: relative path of the requested file, with /readwrite/ stripped
+ * @flags: flags to be passed to open(2)
+ *
+ * Return: opened fd on success, -1 otherwise
+ */
+static int translate_readwrite(const char *file, int flags)
+{
+ int base;
+ int ret;
+ int fd;
+
+ ret = mkdir("/tmp/tqftpserv", 0700);
+ if (ret < 0 && errno != EEXIST) {
+ warn("failed to create /tmp/tqftpserv");
+ return -1;
+ }
+
+ base = open("/tmp/tqftpserv", O_RDONLY | O_DIRECTORY);
+ if (base < 0) {
+ warn("failed top open /tmp/tqftpserv");
+ return -1;
+ }
+
+ fd = openat(base, file, flags, 0600);
+ close(base);
+ if (fd < 0)
+ warn("failed to open %s", file);
+
+ return fd;
+}
+
+/**
+ * translate_open() - open file after translating path
+ *
+
+ * Strips /readonly/firmware/image and search among remoteproc firmware.
+ * Replaces /readwrite with a temporary directory.
+
+ */
+int translate_open(const char *path, int flags)
+{
+ if (!strncmp(path, READONLY_PATH, strlen(READONLY_PATH)))
+ return translate_readonly(path + strlen(READONLY_PATH));
+ else if (!strncmp(path, READWRITE_PATH, strlen(READWRITE_PATH)))
+ return translate_readwrite(path + strlen(READWRITE_PATH), flags);
+
+ fprintf(stderr, "invalid path %s, rejecting\n", path);
+ errno = ENOENT;
+ return -1;
+}
diff --git a/qcom/tqftpserv/translate.h b/qcom/tqftpserv/translate.h
new file mode 100644
index 0000000..ade2d09
--- /dev/null
+++ b/qcom/tqftpserv/translate.h
@@ -0,0 +1,6 @@
+#ifndef __TRANSLATE_H__
+#define __TRANSLATE_H__
+
+int translate_open(const char *path, int flags);
+
+#endif
diff --git a/sepolicy/file.te b/sepolicy/file.te
index 4d9988f..3e31092 100644
--- a/sepolicy/file.te
+++ b/sepolicy/file.te
@@ -1,2 +1,7 @@
type sysfs_gpu, fs_type, sysfs_type;
+type sysfs_rmtfs, fs_type, sysfs_type;
+type sysfs_tqftpserv, fs_type, sysfs_type;
type dri_device, dev_type;
+type rmtfs_device, dev_type;
+type modem_block_device, dev_type;
+type tqftpserv_vendor_data_file, file_type, data_file_type, mlstrustedobject;
diff --git a/sepolicy/file_contexts b/sepolicy/file_contexts
index f1d323c..165f3b3 100644
--- a/sepolicy/file_contexts
+++ b/sepolicy/file_contexts
@@ -1,19 +1,40 @@
-/dev/block/by-name/metadata u:object_r:metadata_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/fsc u:object_r:modem_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/fsg u:object_r:modem_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/modemst[12] u:object_r:modem_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/metadata u:object_r:metadata_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/super u:object_r:super_block_device:s0
+/dev/block/platform/soc@0/1d84000\.ufshc/by-name/userdata u:object_r:userdata_block_device:s0
+
/dev/dri u:object_r:dri_device:s0
/dev/dri/card0 u:object_r:graphics_device:s0
/dev/dri/renderD128 u:object_r:gpu_device:s0
+/dev/qcom_rmtfs_mem1 u:object_r:rmtfs_device:s0
/dev/ttyMSM0 u:object_r:console_device:s0
+/sys/bus/platform/drivers/qcom-q6v5-mss u:object_r:sysfs_rmtfs:s0
+/sys/devices/platform/88f00000.memory/rmtfs u:object_r:sysfs_rmtfs:s0
+
/sys/devices/platform/soc/ae00000.mdss u:object_r:sysfs_gpu:s0
/sys/devices/platform/soc/c440000.spmi/spmi-0/0-00/c440000.spmi:pmic@0:rtc@6000/rtc u:object_r:sysfs_rtc:s0
# sysfs path changed in v5.4+ kernel for sdm845 devices
+/sys/devices/platform/soc@0/4080000.remoteproc u:object_r:sysfs_rmtfs:s0
/sys/devices/platform/soc@0/ae00000.mdss u:object_r:sysfs_gpu:s0
/sys/devices/platform/soc@0/c440000.spmi/spmi-0/0-00/c440000.spmi:pmic@0:rtc@6000/rtc u:object_r:sysfs_rtc:s0
-/dev/block/platform/soc@0/1d84000\.ufshc/by-name/userdata u:object_r:userdata_block_device:s0
+/sys/class/remoteproc u:object_r:sysfs_tqftpserv:s0
+/sys/devices/platform/remoteproc-cdsp/remoteproc/remoteproc2/firmware u:object_r:sysfs_tqftpserv:s0
+
+/data/vendor/tmp(/.*)? u:object_r:tqftpserv_vendor_data_file:s0
+/data/vendor/readwrite(/.*)? u:object_r:tqftpserv_vendor_data_file:s0
+/data/vendor/readonly(/.*)? u:object_r:tqftpserv_vendor_data_file:s0
/vendor/bin/hw/android\.hardware\.gatekeeper@1\.0-service\.software u:object_r:hal_gatekeeper_default_exec:s0
+/vendor/bin/pd-mapper u:object_r:pd_mapper_exec:s0
+/vendor/bin/qrtr-cfg u:object_r:qrtr_exec:s0
+/vendor/bin/qrtr-ns u:object_r:qrtr_exec:s0
+/vendor/bin/rmtfs u:object_r:rmtfs_exec:s0
+/vendor/bin/tqftpserv u:object_r:tqftpserv_exec:s0
/vendor/lib(64)?/dri/.* u:object_r:same_process_hal_file:s0
/vendor/lib(64)?/hw/gralloc\.gbm\.so u:object_r:same_process_hal_file:s0
@@ -21,3 +42,4 @@
/vendor/lib(64)?/libdrm_freedreno\.so u:object_r:same_process_hal_file:s0
/vendor/lib(64)?/libgbm\.so u:object_r:same_process_hal_file:s0
/vendor/lib(64)?/libglapi\.so u:object_r:same_process_hal_file:s0
+/vendor/lib(64)?/libqrtr\.so u:object_r:same_process_hal_file:s0
diff --git a/sepolicy/genfs_contexts b/sepolicy/genfs_contexts
index 0816df9..caed6c0 100644
--- a/sepolicy/genfs_contexts
+++ b/sepolicy/genfs_contexts
@@ -1,6 +1,9 @@
-genfscon sysfs /devices/platform/soc/ae00000.mdss u:object_r:sysfs_gpu:s0
+genfscon sysfs /devices/platform/88f00000.memory/rmtfs u:object_r:sysfs_rmtfs:s0
+genfscon sysfs /devices/platform/soc/ae00000.mdss u:object_r:sysfs_gpu:s0
genfscon sysfs /devices/platform/soc/c440000.spmi/spmi-0/0-00/c440000.spmi:pmic@0:rtc@6000 u:object_r:sysfs_rtc:s0
+genfscon sysfs /devices/platform/remoteproc-cdsp/remoteproc/remoteproc2/firmware u:object_r:sysfs_tqftpserv:s0
# sysfs path changed in v5.4+ kernel for sdm845 devices
-genfscon sysfs /devices/platform/soc@0/ae00000.mdss u:object_r:sysfs_gpu:s0
+genfscon sysfs /devices/platform/soc@0/4080000.remoteproc u:object_r:sysfs_rmtfs:s0
+genfscon sysfs /devices/platform/soc@0/ae00000.mdss u:object_r:sysfs_gpu:s0
genfscon sysfs /devices/platform/soc@0/c440000.spmi/spmi-0/0-00/c440000.spmi:pmic@0:rtc@6000 u:object_r:sysfs_rtc:s0
diff --git a/sepolicy/pd_mapper.te b/sepolicy/pd_mapper.te
new file mode 100644
index 0000000..6eb348e
--- /dev/null
+++ b/sepolicy/pd_mapper.te
@@ -0,0 +1,6 @@
+type pd_mapper, domain;
+
+type pd_mapper_exec, exec_type, vendor_file_type, file_type;
+init_daemon_domain(pd_mapper);
+
+allow pd_mapper self:qipcrtr_socket { create getattr read setopt write };
diff --git a/sepolicy/qrtr.te b/sepolicy/qrtr.te
new file mode 100644
index 0000000..8344398
--- /dev/null
+++ b/sepolicy/qrtr.te
@@ -0,0 +1,8 @@
+type qrtr, domain;
+type qrtr_exec, exec_type, vendor_file_type, file_type;
+init_daemon_domain(qrtr)
+
+allow qrtr self:capability net_admin;
+allow qrtr self:capability sys_admin;
+allow qrtr self:qipcrtr_socket create_socket_perms_no_ioctl;
+allow qrtr self:netlink_route_socket { create nlmsg_write read write };
diff --git a/sepolicy/rmtfs.te b/sepolicy/rmtfs.te
new file mode 100644
index 0000000..fff2e64
--- /dev/null
+++ b/sepolicy/rmtfs.te
@@ -0,0 +1,13 @@
+type rmtfs, domain;
+type rmtfs_exec, exec_type, vendor_file_type, file_type;
+
+init_daemon_domain(rmtfs)
+
+allow rmtfs block_device:dir search;
+allow rmtfs modem_block_device:blk_file { open read };
+allow rmtfs rmtfs_device:chr_file { open read write };
+allow rmtfs self:capability net_admin;
+allow rmtfs self:qipcrtr_socket { bind create getattr read setopt write };
+allow rmtfs sysfs_rmtfs:dir { open read search};
+allow rmtfs sysfs_rmtfs:file r_file_perms;
+allow rmtfs sysfs_rmtfs:file write;
diff --git a/sepolicy/tqftpserv.te b/sepolicy/tqftpserv.te
new file mode 100644
index 0000000..46f3926
--- /dev/null
+++ b/sepolicy/tqftpserv.te
@@ -0,0 +1,10 @@
+type tqftpserv, domain;
+
+type tqftpserv_exec, exec_type, vendor_file_type, file_type;
+init_daemon_domain(tqftpserv);
+
+allow tqftpserv self:qipcrtr_socket { connect create getattr read setopt write };
+allow tqftpserv sysfs_tqftpserv:dir { open read search };
+allow tqftpserv sysfs_tqftpserv:file { open read };
+allow tqftpserv tqftpserv_vendor_data_file:dir { add_name create open read search write };
+allow tqftpserv tqftpserv_vendor_data_file:file { create open write };