From a427ad436edb96d553e8a6a7917a7791cd663c4d Mon Sep 17 00:00:00 2001 From: Bjorn Andersson Date: Fri, 29 Jun 2018 15:47:57 -0700 Subject: [PATCH] pd-mapper: Initial implementation This implements the minimum scope of pd-mapper in order to trigger loading of wlan firmware on SDM845. Signed-off-by: Bjorn Andersson --- Makefile | 18 +++++ pd-mapper.c | 186 ++++++++++++++++++++++++++++++++++++++++++++++++ servreg_loc.c | 169 +++++++++++++++++++++++++++++++++++++++++++ servreg_loc.h | 67 +++++++++++++++++ servreg_loc.qmi | 48 +++++++++++++ 5 files changed, 488 insertions(+) create mode 100644 Makefile create mode 100644 pd-mapper.c create mode 100644 servreg_loc.c create mode 100644 servreg_loc.h create mode 100644 servreg_loc.qmi diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..78090c6 --- /dev/null +++ b/Makefile @@ -0,0 +1,18 @@ +PD_MAPPER := pd-mapper + +CFLAGS := -Wall -g -O2 +LDFLAGS := -lqrtr + +SRCS := pd-mapper.c \ + servreg_loc.c + +OBJS := $(SRCS:.c=.o) + +$(PD_MAPPER): $(OBJS) + $(CC) -o $@ $^ $(LDFLAGS) + +install: $(PD_MAPPER) + install -D -m 755 $< $(DESTDIR)$(prefix)/bin/$< + +clean: + rm -f $(PD_MAPPER) $(OBJS) diff --git a/pd-mapper.c b/pd-mapper.c new file mode 100644 index 0000000..bf2c25b --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#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_maps->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/servreg_loc.c b/servreg_loc.c new file mode 100644 index 0000000..6309498 --- /dev/null +++ b/servreg_loc.c @@ -0,0 +1,169 @@ +#include +#include +#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/servreg_loc.h b/servreg_loc.h new file mode 100644 index 0000000..2ac5faa --- /dev/null +++ b/servreg_loc.h @@ -0,0 +1,67 @@ +#ifndef __QMI_SERVREG_LOC_H__ +#define __QMI_SERVREG_LOC_H__ + +#include +#include + +#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/servreg_loc.qmi b/servreg_loc.qmi new file mode 100644 index 0000000..4dc04e6 --- /dev/null +++ b/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;