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 <stdio.h>
40 #include <stdlib.h>
41 #include <string.h>
42 #include <unistd.h>
43
44 #include "assoc.h"
45 #include "json.h"
46 #include "servreg_loc.h"
47
48 struct pd_map {
49 const char *service;
50 const char *domain;
51 int instance;
52 };
53
54 static struct pd_map *pd_maps;
55
handle_get_domain_list(int sock,const struct qrtr_packet * pkt)56 static void handle_get_domain_list(int sock, const struct qrtr_packet *pkt)
57 {
58 struct servreg_loc_get_domain_list_resp resp = {};
59 struct servreg_loc_get_domain_list_req req = {};
60 struct servreg_loc_domain_list_entry *entry;
61 DEFINE_QRTR_PACKET(resp_buf, 256);
62 const struct pd_map *pd_map = pd_maps;
63 unsigned int txn;
64 ssize_t len;
65 int ret;
66
67 ret = qmi_decode_message(&req, &txn, pkt, QMI_REQUEST,
68 SERVREG_LOC_GET_DOMAIN_LIST,
69 servreg_loc_get_domain_list_req_ei);
70 if (ret < 0) {
71 resp.result.result = QMI_RESULT_FAILURE;
72 resp.result.error = QMI_ERR_MALFORMED_MSG;
73 goto respond;
74 }
75
76 req.name[sizeof(req.name)-1] = '\0';
77
78 resp.result.result = QMI_RESULT_SUCCESS;
79 resp.db_revision_valid = 1;
80 resp.db_revision = 1;
81
82 while (pd_map->service) {
83 if (!strcmp(pd_map->service, req.name)) {
84 entry = &resp.domain_list[resp.domain_list_len++];
85
86 strcpy(entry->name, pd_map->domain);
87 entry->name_len = strlen(pd_map->domain);
88 entry->instance_id = pd_map->instance;
89 }
90
91 pd_map++;
92 }
93
94 if (resp.domain_list_len)
95 resp.domain_list_valid = 1;
96
97 resp.total_domains_valid = 1;
98 resp.total_domains = resp.domain_list_len;
99
100 respond:
101 len = qmi_encode_message(&resp_buf,
102 QMI_RESPONSE, SERVREG_LOC_GET_DOMAIN_LIST,
103 txn, &resp,
104 servreg_loc_get_domain_list_resp_ei);
105 if (len < 0) {
106 fprintf(stderr,
107 "[PD-MAPPER] failed to encode get_domain_list response: %s\n",
108 strerror(-len));
109 return;
110 }
111
112 ret = qrtr_sendto(sock, pkt->node, pkt->port,
113 resp_buf.data, resp_buf.data_len);
114 if (ret < 0) {
115 fprintf(stderr,
116 "[PD-MAPPER] failed to send get_domain_list response: %s\n",
117 strerror(-ret));
118 }
119 }
120
pd_load_map(const char * file)121 static int pd_load_map(const char *file)
122 {
123 static int num_pd_maps;
124 struct json_value *sr_service;
125 struct json_value *sr_domain;
126 struct json_value *root;
127 struct json_value *it;
128 const char *subdomain;
129 const char *provider;
130 const char *service;
131 const char *domain;
132 const char *soc;
133 struct pd_map *newp;
134 struct pd_map *map;
135 double number;
136 int count;
137 int ret;
138
139 root = json_parse_file(file);
140 if (!root)
141 return -1;
142
143 sr_domain = json_get_child(root, "sr_domain");
144 soc = json_get_string(sr_domain, "soc");
145 domain = json_get_string(sr_domain, "domain");
146 subdomain = json_get_string(sr_domain, "subdomain");
147 ret = json_get_number(sr_domain, "qmi_instance_id", &number);
148 if (ret)
149 return ret;
150
151 if (!soc || !domain || !subdomain) {
152 fprintf(stderr, "failed to parse sr_domain\n");
153 return -1;
154 }
155
156 sr_service = json_get_child(root, "sr_service");
157 count = json_count_children(sr_service);
158 if (count < 0)
159 return count;
160
161 newp = realloc(pd_maps, (num_pd_maps + count + 1) * sizeof(*newp));
162 if (!newp)
163 return -1;
164 pd_maps = newp;
165
166 for (it = sr_service->u.value; it; it = it->next) {
167 provider = json_get_string(it, "provider");
168 service = json_get_string(it, "service");
169
170 if (!provider || !service) {
171 fprintf(stderr,
172 "failed to parse provdider or service from %s\n",
173 file);
174 return -1;
175 }
176
177 map = &pd_maps[num_pd_maps++];
178
179 map->service = malloc(strlen(provider) + strlen(service) + 2);
180 sprintf((char *)map->service, "%s/%s", provider, service);
181
182 map->domain = malloc(strlen(soc) + strlen(domain) + strlen(subdomain) + 3);
183 sprintf((char *)map->domain, "%s/%s/%s", soc, domain, subdomain);
184
185 map->instance = number;
186 }
187
188 pd_maps[num_pd_maps].service = NULL;
189
190 json_free(root);
191
192 return 0;
193 }
194
195 #ifndef ANDROID
196 #define FIRMWARE_BASE "/lib/firmware/"
197 #else
198 #define FIRMWARE_BASE "/vendor/firmware/"
199 #endif
200
pd_enumerate_jsons(struct assoc * json_set)201 static int pd_enumerate_jsons(struct assoc *json_set)
202 {
203 char firmware_value[PATH_MAX];
204 char json_path[PATH_MAX];
205 char firmware_attr[32];
206 struct dirent *fw_de;
207 char path[PATH_MAX];
208 struct dirent *de;
209 int firmware_fd;
210 DIR *class_dir;
211 int class_fd;
212 DIR *fw_dir;
213 size_t len;
214 size_t n;
215
216 class_fd = open("/sys/class/remoteproc", O_RDONLY | O_DIRECTORY);
217 if (class_fd < 0) {
218 warn("failed to open remoteproc class");
219 return -1;
220 }
221
222 class_dir = fdopendir(class_fd);
223 if (!class_dir) {
224 warn("failed to opendir");
225 goto close_class;
226 }
227
228 while ((de = readdir(class_dir)) != NULL) {
229 if (!strcmp(de->d_name, ".") || !strcmp(de->d_name, ".."))
230 continue;
231
232 if (strlen(de->d_name) + sizeof("/firmware") > sizeof(firmware_attr))
233 continue;
234
235 strcpy(firmware_attr, de->d_name);
236 strcat(firmware_attr, "/firmware");
237
238 firmware_fd = openat(class_fd, firmware_attr, O_RDONLY);
239 if (firmware_fd < 0)
240 continue;
241
242 n = read(firmware_fd, firmware_value, sizeof(firmware_value));
243 close(firmware_fd);
244 if (n < 0) {
245 continue;
246 }
247
248 firmware_value[n] = '\0';
249
250 if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 > sizeof(path))
251 continue;
252
253 strcpy(path, FIRMWARE_BASE);
254 strcat(path, dirname(firmware_value));
255
256 fw_dir = opendir(path);
257 while ((fw_de = readdir(fw_dir)) != NULL) {
258 if (!strcmp(fw_de->d_name, ".") || !strcmp(fw_de->d_name, ".."))
259 continue;
260
261 len = strlen(fw_de->d_name);
262 if (len < 5 || strcmp(&fw_de->d_name[len - 4], ".jsn"))
263 continue;
264
265 if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 +
266 strlen(fw_de->d_name) + 1 > sizeof(path))
267 continue;
268
269 strcpy(json_path, path);
270 strcat(json_path, "/");
271 strcat(json_path, fw_de->d_name);
272
273 assoc_set(json_set, json_path, NULL);
274 }
275
276 closedir(fw_dir);
277 }
278
279 closedir(class_dir);
280 close_class:
281 close(class_fd);
282
283 return 0;
284 }
285
pd_load_maps(void)286 static int pd_load_maps(void)
287 {
288 struct assoc json_set;
289 unsigned long it;
290 const char *jsn;
291 int ret = 0;
292
293 assoc_init(&json_set, 20);
294
295 pd_enumerate_jsons(&json_set);
296
297 assoc_foreach(jsn, NULL, &json_set, it) {
298 ret = pd_load_map(jsn);
299 if (ret < 0)
300 break;
301 }
302
303 assoc_destroy(&json_set);
304
305 return ret;
306 }
307
main(int argc __unused,char ** argv __unused)308 int main(int argc __unused, char **argv __unused)
309 {
310 struct sockaddr_qrtr sq;
311 struct qrtr_packet pkt;
312 unsigned int msg_id;
313 socklen_t sl;
314 char buf[4096];
315 int ret;
316 int fd;
317
318 ret = pd_load_maps();
319 if (ret)
320 exit(1);
321
322 if (!pd_maps) {
323 fprintf(stderr, "no pd maps available\n");
324 exit(1);
325 }
326
327 fd = qrtr_open(0);
328 if (fd < 0) {
329 fprintf(stderr, "failed to open qrtr socket\n");
330 exit(1);
331 }
332
333 ret = qrtr_publish(fd, SERVREG_QMI_SERVICE,
334 SERVREG_QMI_VERSION, SERVREG_QMI_INSTANCE);
335 if (ret < 0) {
336 fprintf(stderr, "failed to publish service registry service\n");
337 exit(1);
338 }
339
340 for (;;) {
341 ret = qrtr_poll(fd, -1);
342 if (ret < 0) {
343 if (errno == EINTR) {
344 continue;
345 } else {
346 fprintf(stderr, "qrtr_poll failed\n");
347 break;
348 }
349 }
350
351 sl = sizeof(sq);
352 ret = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
353 if (ret < 0) {
354 ret = -errno;
355 if (ret != -ENETRESET)
356 fprintf(stderr, "[PD-MAPPER] recvfrom failed: %d\n", ret);
357 return ret;
358 }
359
360 ret = qrtr_decode(&pkt, buf, ret, &sq);
361 if (ret < 0) {
362 fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
363 return ret;
364 }
365
366 switch (pkt.type) {
367 case QRTR_TYPE_DATA:
368 ret = qmi_decode_header(&pkt, &msg_id);
369 if (ret < 0)
370 continue;
371
372 switch (msg_id) {
373 case SERVREG_LOC_GET_DOMAIN_LIST:
374 handle_get_domain_list(fd, &pkt);
375 break;
376 case SERVREG_LOC_PFR:
377 printf("[PD-MAPPER] pfr\n");
378 break;
379 };
380 break;
381 };
382 }
383
384 close(fd);
385
386 return 0;
387 }
388