diff options
author | Amit Pundir <amit.pundir@linaro.org> | 2020-02-07 22:26:08 +0530 |
---|---|---|
committer | John Stultz <john.stultz@linaro.org> | 2020-03-17 04:10:56 +0000 |
commit | d477f821bb485acbb232cb896e06b99ed98ba25c (patch) | |
tree | 2cb8babdc907b5eb23c85cedb70742e360beeba5 /qcom | |
parent | 4789995b56420cfbd17b71dd242b54d36fca6361 (diff) |
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>
Diffstat (limited to 'qcom')
56 files changed, 7741 insertions, 0 deletions
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 |