1 /*
2 * Copyright (c) 2018, Linaro Ltd.
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 *
11 * 2. Redistributions in binary form must reproduce the above copyright notice,
12 * this list of conditions and the following disclaimer in the documentation
13 * and/or other materials provided with the distribution.
14 *
15 * 3. Neither the name of the copyright holder nor the names of its contributors
16 * may be used to endorse or promote products derived from this software without
17 * specific prior written permission.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31 #include <sys/stat.h>
32 #include <sys/types.h>
33 #include <err.h>
34 #include <errno.h>
35 #include <dirent.h>
36 #include <fcntl.h>
37 #include <libgen.h>
38 #include <libqrtr.h>
39 #include <limits.h>
40 #include <stdio.h>
41 #include <stdlib.h>
42 #include <string.h>
43 #include <unistd.h>
44
45 #include "assoc.h"
46 #include "json.h"
47 #include "servreg_loc.h"
48
49 struct pd_map {
50 const char *service;
51 const char *domain;
52 int instance;
53 };
54
55 static struct pd_map *pd_maps;
56
handle_get_domain_list(int sock,const struct qrtr_packet * pkt)57 static void handle_get_domain_list(int sock, const struct qrtr_packet *pkt)
58 {
59 struct servreg_loc_get_domain_list_resp resp = {};
60 struct servreg_loc_get_domain_list_req req = {};
61 struct servreg_loc_domain_list_entry *entry;
62 DEFINE_QRTR_PACKET(resp_buf, 256);
63 const struct pd_map *pd_map = pd_maps;
64 unsigned int txn;
65 ssize_t len;
66 int ret;
67
68 ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
69 SERVREG_LOC_GET_DOMAIN_LIST,
70 servreg_loc_get_domain_list_req_ei);
71 if (ret < 0) {
72 resp.result.result = QMI_RESULT_FAILURE;
73 resp.result.error = QMI_ERR_MALFORMED_MSG;
74 goto respond;
75 }
76
77 req.name[sizeof(req.name)-1] = '\0';
78
79 resp.result.result = QMI_RESULT_SUCCESS;
80 resp.db_revision_valid = 1;
81 resp.db_revision = 1;
82
83 while (pd_map->service) {
84 if (!strcmp(pd_map->service, req.name)) {
85 entry = &resp.domain_list[resp.domain_list_len++];
86
87 strcpy(entry->name, pd_map->domain);
88 entry->name_len = strlen(pd_map->domain);
89 entry->instance_id = pd_map->instance;
90 }
91
92 pd_map++;
93 }
94
95 if (resp.domain_list_len)
96 resp.domain_list_valid = 1;
97
98 resp.total_domains_valid = 1;
99 resp.total_domains = resp.domain_list_len;
100
101 respond:
102 len = qmi_encode_message(&resp_buf,
103 QMI_RESPONSE, SERVREG_LOC_GET_DOMAIN_LIST,
104 txn, &resp,
105 servreg_loc_get_domain_list_resp_ei);
106 if (len < 0) {
107 fprintf(stderr,
108 "[PD-MAPPER] failed to encode get_domain_list response: %s\n",
109 strerror(-len));
110 return;
111 }
112
113 ret = qrtr_sendto(sock, pkt->node, pkt->port,
114 resp_buf.data, resp_buf.data_len);
115 if (ret < 0) {
116 fprintf(stderr,
117 "[PD-MAPPER] failed to send get_domain_list response: %s\n",
118 strerror(-ret));
119 }
120 }
121
pd_load_map(const char * file)122 static int pd_load_map(const char *file)
123 {
124 static int num_pd_maps;
125 struct json_value *sr_service;
126 struct json_value *sr_domain;
127 struct json_value *root;
128 struct json_value *it;
129 const char *subdomain;
130 const char *provider;
131 const char *service;
132 const char *domain;
133 const char *soc;
134 struct pd_map *newp;
135 struct pd_map *map;
136 double number;
137 int count;
138 int ret;
139
140 root = json_parse_file(file);
141 if (!root)
142 return -1;
143
144 sr_domain = json_get_child(root, "sr_domain");
145 soc = json_get_string(sr_domain, "soc");
146 domain = json_get_string(sr_domain, "domain");
147 subdomain = json_get_string(sr_domain, "subdomain");
148 ret = json_get_number(sr_domain, "qmi_instance_id", &number);
149 if (ret)
150 return ret;
151
152 if (!soc || !domain || !subdomain) {
153 fprintf(stderr, "failed to parse sr_domain\n");
154 return -1;
155 }
156
157 sr_service = json_get_child(root, "sr_service");
158 count = json_count_children(sr_service);
159 if (count < 0)
160 return count;
161
162 newp = realloc(pd_maps, (num_pd_maps + count + 1) * sizeof(*newp));
163 if (!newp)
164 return -1;
165 pd_maps = newp;
166
167 for (it = sr_service->u.value; it; it = it->next) {
168 provider = json_get_string(it, "provider");
169 service = json_get_string(it, "service");
170
171 if (!provider || !service) {
172 fprintf(stderr,
173 "failed to parse provdider or service from %s\n",
174 file);
175 return -1;
176 }
177
178 map = &pd_maps[num_pd_maps++];
179
180 map->service = malloc(strlen(provider) + strlen(service) + 2);
181 sprintf((char *)map->service, "%s/%s", provider, service);
182
183 map->domain = malloc(strlen(soc) + strlen(domain) + strlen(subdomain) + 3);
184 sprintf((char *)map->domain, "%s/%s/%s", soc, domain, subdomain);
185
186 map->instance = number;
187 }
188
189 pd_maps[num_pd_maps].service = NULL;
190
191 json_free(root);
192
193 return 0;
194 }
195
196 #ifndef ANDROID
197 #define FIRMWARE_BASE "/lib/firmware/"
198 #else
199 #define FIRMWARE_BASE "/vendor/firmware/"
200 #endif
201
pd_enumerate_jsons(struct assoc * json_set)202 static int pd_enumerate_jsons(struct assoc *json_set)
203 {
204 char firmware_value[PATH_MAX];
205 char json_path[PATH_MAX];
206 char firmware_attr[32];
207 struct dirent *fw_de;
208 char path[PATH_MAX];
209 struct dirent *de;
210 int firmware_fd;
211 DIR *class_dir;
212 int class_fd;
213 DIR *fw_dir;
214 size_t len;
215 size_t n;
216
217 class_fd = open("/sys/class/remoteproc", O_RDONLY | O_DIRECTORY);
218 if (class_fd < 0) {
219 warn("failed to open remoteproc class");
220 return -1;
221 }
222
223 class_dir = fdopendir(class_fd);
224 if (!class_dir) {
225 warn("failed to opendir");
226 goto close_class;
227 }
228
229 while ((de = readdir(class_dir)) != NULL) {
230 if (!strcmp(de->d_name, ".") || !strcmp(de->d_name, ".."))
231 continue;
232
233 if (strlen(de->d_name) + sizeof("/firmware") > sizeof(firmware_attr))
234 continue;
235
236 strcpy(firmware_attr, de->d_name);
237 strcat(firmware_attr, "/firmware");
238
239 firmware_fd = openat(class_fd, firmware_attr, O_RDONLY);
240 if (firmware_fd < 0)
241 continue;
242
243 n = read(firmware_fd, firmware_value, sizeof(firmware_value));
244 close(firmware_fd);
245 if (n < 0) {
246 continue;
247 }
248
249 firmware_value[n] = '\0';
250
251 if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 > sizeof(path))
252 continue;
253
254 strcpy(path, FIRMWARE_BASE);
255 strcat(path, dirname(firmware_value));
256
257 fw_dir = opendir(path);
258 while ((fw_de = readdir(fw_dir)) != NULL) {
259 if (!strcmp(fw_de->d_name, ".") || !strcmp(fw_de->d_name, ".."))
260 continue;
261
262 len = strlen(fw_de->d_name);
263 if (len < 5 || strcmp(&fw_de->d_name[len - 4], ".jsn"))
264 continue;
265
266 if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 +
267 strlen(fw_de->d_name) + 1 > sizeof(path))
268 continue;
269
270 strcpy(json_path, path);
271 strcat(json_path, "/");
272 strcat(json_path, fw_de->d_name);
273
274 assoc_set(json_set, json_path, NULL);
275 }
276
277 closedir(fw_dir);
278 }
279
280 closedir(class_dir);
281 close_class:
282 close(class_fd);
283
284 return 0;
285 }
286
pd_load_maps(void)287 static int pd_load_maps(void)
288 {
289 struct assoc json_set;
290 unsigned long it;
291 const char *jsn;
292 int ret = 0;
293
294 assoc_init(&json_set, 20);
295
296 pd_enumerate_jsons(&json_set);
297
298 assoc_foreach(jsn, NULL, &json_set, it) {
299 ret = pd_load_map(jsn);
300 if (ret < 0)
301 break;
302 }
303
304 assoc_destroy(&json_set);
305
306 return ret;
307 }
308
main(int argc __unused,char ** argv __unused)309 int main(int argc __unused, char **argv __unused)
310 {
311 struct sockaddr_qrtr sq;
312 struct qrtr_packet pkt;
313 unsigned int msg_id;
314 socklen_t sl;
315 char buf[4096];
316 int ret;
317 int fd;
318
319 ret = pd_load_maps();
320 if (ret)
321 exit(1);
322
323 if (!pd_maps) {
324 fprintf(stderr, "no pd maps available\n");
325 exit(1);
326 }
327
328 fd = qrtr_open(0);
329 if (fd < 0) {
330 fprintf(stderr, "failed to open qrtr socket\n");
331 exit(1);
332 }
333
334 ret = qrtr_publish(fd, SERVREG_QMI_SERVICE,
335 SERVREG_QMI_VERSION, SERVREG_QMI_INSTANCE);
336 if (ret < 0) {
337 fprintf(stderr, "failed to publish service registry service\n");
338 exit(1);
339 }
340
341 for (;;) {
342 ret = qrtr_poll(fd, -1);
343 if (ret < 0) {
344 if (errno == EINTR) {
345 continue;
346 } else {
347 fprintf(stderr, "qrtr_poll failed\n");
348 break;
349 }
350 }
351
352 sl = sizeof(sq);
353 ret = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
354 if (ret < 0) {
355 ret = -errno;
356 if (ret != -ENETRESET)
357 fprintf(stderr, "[PD-MAPPER] recvfrom failed: %d\n", ret);
358 return ret;
359 }
360
361 ret = qrtr_decode(&pkt, buf, ret, &sq);
362 if (ret < 0) {
363 fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
364 return ret;
365 }
366
367 switch (pkt.type) {
368 case QRTR_TYPE_DATA:
369 ret = qmi_decode_header(&pkt, &msg_id);
370 if (ret < 0)
371 continue;
372
373 switch (msg_id) {
374 case SERVREG_LOC_GET_DOMAIN_LIST:
375 handle_get_domain_list(fd, &pkt);
376 break;
377 case SERVREG_LOC_PFR:
378 printf("[PD-MAPPER] pfr\n");
379 break;
380 };
381 break;
382 };
383 }
384
385 close(fd);
386
387 return 0;
388 }
389