diff options
Diffstat (limited to 'shared/utils/pd-mapper')
| -rw-r--r-- | shared/utils/pd-mapper/Android.bp | 30 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/LICENSE | 60 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/Makefile | 28 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/assoc.c | 158 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/assoc.h | 54 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/json.c | 451 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/json.h | 67 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/pd-mapper.c | 388 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/pd-mapper.service.in | 11 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/servreg_loc.c | 169 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/servreg_loc.h | 67 | ||||
| -rw-r--r-- | shared/utils/pd-mapper/servreg_loc.qmi | 48 |
12 files changed, 1531 insertions, 0 deletions
diff --git a/shared/utils/pd-mapper/Android.bp b/shared/utils/pd-mapper/Android.bp new file mode 100644 index 0000000..7e86231 --- /dev/null +++ b/shared/utils/pd-mapper/Android.bp @@ -0,0 +1,30 @@ +package { + default_applicable_licenses: [ + "device_linaro_dragonboard_qcom_pd-mapper_license", + ], +} + +// Added automatically by a large-scale-change +// See: http://go/android-license-faq +license { + name: "device_linaro_dragonboard_qcom_pd-mapper_license", + visibility: [":__subpackages__"], + license_kinds: [ + "SPDX-license-identifier-BSD", + ], + license_text: [ + "LICENSE", + ], +} + +cc_binary { + name: "pd-mapper", + vendor: true, + srcs: [ + "pd-mapper.c", + "assoc.c", + "json.c", + "servreg_loc.c", + ], + shared_libs: ["libqrtr"], +} diff --git a/shared/utils/pd-mapper/LICENSE b/shared/utils/pd-mapper/LICENSE new file mode 100644 index 0000000..aa8cb35 --- /dev/null +++ b/shared/utils/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/shared/utils/pd-mapper/Makefile b/shared/utils/pd-mapper/Makefile new file mode 100644 index 0000000..908dbfd --- /dev/null +++ b/shared/utils/pd-mapper/Makefile @@ -0,0 +1,28 @@ +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 \ + assoc.c \ + json.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/shared/utils/pd-mapper/assoc.c b/shared/utils/pd-mapper/assoc.c new file mode 100644 index 0000000..460f0d0 --- /dev/null +++ b/shared/utils/pd-mapper/assoc.c @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2013, 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. + */ + +#include <assert.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> + +#include "assoc.h" + +static unsigned long assoc_hash(const char *value) +{ + unsigned long hash = 0; + unsigned long g; + const char *v = value; + + while (*v) { + hash = (hash << 4) + *(v++); + g = hash & 0xF0000000L; + if (g) + hash ^= g >> 24; + hash &= ~g; + } + + return hash; +} + +void assoc_init(struct assoc *assoc, unsigned long size) +{ + assert(size > 0); + + assoc->size = size; + assoc->fill = 0; + assoc->keys = calloc(size, sizeof(const char *)); + assoc->values = malloc(size * sizeof(void *)); +} + +void *assoc_get(struct assoc *assoc, const char *key) +{ + unsigned long hash; + + hash = assoc_hash(key) % assoc->size; + while (assoc->keys[hash]) { + if (!strcmp(assoc->keys[hash], key)) + return assoc->values[hash]; + + hash = (hash + 1) % assoc->size; + } + + return NULL; +} + +static void _assoc_set(struct assoc *assoc, const char *key, void *value) +{ + struct assoc new_set; + unsigned long hash; + unsigned long i; + + assert(assoc->fill < assoc->size); + + /* Grow set at 80% utilization */ + if (5 * assoc->fill > 4 * assoc->size) { + assoc_init(&new_set, assoc->size * 5 / 4); + + for (i = 0; i < assoc->size; i++) + if (assoc->keys[i]) + assoc_set(&new_set, assoc->keys[i], + assoc->values[i]); + + free(assoc->keys); + free(assoc->values); + + assoc->keys = new_set.keys; + assoc->values = new_set.values; + assoc->fill = new_set.fill; + assoc->size = new_set.size; + } + + hash = assoc_hash(key) % assoc->size; + while (assoc->keys[hash]) { + if (!strcmp(assoc->keys[hash], key)) { + assoc->values[hash] = value; + return; + } + + hash = (hash + 1) % assoc->size; + } + + assoc->keys[hash] = key; + assoc->values[hash] = value; + assoc->fill++; +} + +void assoc_set(struct assoc *assoc, const char *key, void *value) +{ + _assoc_set(assoc, strdup(key), value); +} + +const char *assoc_next(struct assoc *assoc, void **value, unsigned long *iter) +{ + unsigned long it = *iter; + + while (it < assoc->size && !assoc->keys[it]) + it++; + + if (it == assoc->size) + return NULL; + + *iter = it + 1; + + if (it < assoc->size) { + if (value) + *value = assoc->values[it]; + return assoc->keys[it]; + } else { + return NULL; + } +} + +void assoc_destroy(struct assoc *assoc) +{ + unsigned long i; + + for (i = 0; i < assoc->size; i++) + free((void*)assoc->keys[i]); + + free(assoc->keys); + free(assoc->values); + assoc->size = 0; +} diff --git a/shared/utils/pd-mapper/assoc.h b/shared/utils/pd-mapper/assoc.h new file mode 100644 index 0000000..25d00fa --- /dev/null +++ b/shared/utils/pd-mapper/assoc.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2013, 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. + */ + +#ifndef __ASSOC_H__ +#define __ASSOC_H__ + +struct assoc { + unsigned long size; + unsigned long fill; + + const char **keys; + void **values; +}; + +void assoc_init(struct assoc *assoc, unsigned long size); +void *assoc_get(struct assoc *assoc, const char *key); +void assoc_set(struct assoc *assoc, const char *key, void *value); +const char *assoc_next(struct assoc *assoc, void **value, unsigned long *iter); +void assoc_destroy(struct assoc *assoc); + +#define assoc_foreach(key, value, assoc, iter) \ + for ((iter) = 0, (key) = assoc_next((assoc), (value), &(iter)); \ + (key); \ + (key) = assoc_next((assoc), (value), &(iter))) + +#endif diff --git a/shared/utils/pd-mapper/json.c b/shared/utils/pd-mapper/json.c new file mode 100644 index 0000000..e08afa4 --- /dev/null +++ b/shared/utils/pd-mapper/json.c @@ -0,0 +1,451 @@ +/* + * Copyright (c) 2018-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 <ctype.h> +#include <errno.h> +#include <fcntl.h> +#include <stdarg.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include "json.h" + +static const char *input_buf; +static int input_pos; +static int input_len; + +static int json_parse_array(struct json_value *array); +static int json_parse_object(struct json_value *object); +static int json_parse_property(struct json_value *value); + +static int input(void) +{ + if (input_pos >= input_len) + return 0; + + return input_buf[input_pos++]; +} + +static void unput(void) +{ + input_pos--; +} + +static void json_skip_whitespace(void) +{ + int ch; + + while ((ch = input()) && isspace(ch)) + ; + unput(); +} + +static int json_parse_string(struct json_value *value) +{ + char buf[128]; + char *b = buf; + int ch; + + ch = input(); + if (ch != '"') { + unput(); + return 0; + } + + while ((ch = input()) && ch != '"' && b - buf < sizeof(buf) - 1) + *b++ = ch; + *b = '\0'; + + if (!ch) + return -1; + + value->type = JSON_TYPE_STRING; + value->u.string = strdup(buf); + + return 1; +} + +static int json_parse_number(struct json_value *value) +{ + char buf[20]; + char *b = buf; + int ch; + + while ((ch = input()) && isdigit(ch) && b - buf < sizeof(buf) - 1) + *b++ = ch; + *b = '\0'; + unput(); + + if (b == buf) + return 0; + + value->type = JSON_TYPE_NUMBER; + value->u.number = strtod(buf, NULL); + + return 1; +} + +static int json_parse_keyword(struct json_value *value) +{ + const char *match; + const char *m; + int ch; + + ch = input(); + switch (ch) { + case 't': + match = "true"; + value->type = JSON_TYPE_TRUE; + break; + case 'f': + match = "false"; + value->type = JSON_TYPE_FALSE; + break; + case 'n': + match = "null"; + value->type = JSON_TYPE_NULL; + break; + default: + unput(); + return 0; + } + + m = match; + while (*m && *m++ == ch) + ch = input(); + unput(); + + return *m == '\0' ? 1 : -1; +} + +static int json_parse_value(struct json_value *value) +{ + int ret; + + json_skip_whitespace(); + + ret = json_parse_object(value); + if (ret) + goto out; + + ret = json_parse_array(value); + if (ret) + goto out; + + ret = json_parse_string(value); + if (ret) + goto out; + + ret = json_parse_number(value); + if (ret) + goto out; + + ret = json_parse_keyword(value); + if (ret) + goto out; + + fprintf(stderr, "unable to match a value\n"); + return -1; + +out: + json_skip_whitespace(); + return ret; +} + +static int json_parse_array(struct json_value *array) +{ + struct json_value *value; + struct json_value *last = NULL; + int ret; + int ch; + + ch = input(); + if (ch != '[') { + unput(); + return 0; + } + + array->type = JSON_TYPE_ARRAY; + do { + value = calloc(1, sizeof(*value)); + if (!value) + return -1; + + ret = json_parse_value(value); + if (ret <= 0) { + free(value); + return -1; + } + + if (!array->u.value) + array->u.value = value; + if (last) + last->next = value; + last = value; + + ch = input(); + if (ch == ']') { + return 1; + } + + } while (ch == ','); + + fprintf(stderr, "expected ',' got '%c'\n", ch); + + return -1; +} + +static int json_parse_object(struct json_value *object) +{ + struct json_value *value; + struct json_value *last = NULL; + int ret; + int ch; + + ch = input(); + if (ch != '{') { + unput(); + return 0; + } + + object->type = JSON_TYPE_OBJECT; + + do { + value = calloc(1, sizeof(*value)); + if (!value) + return -1; + + ret = json_parse_property(value); + if (ret <= 0) { + free(value); + return -1; + } + + if (!object->u.value) + object->u.value = value; + if (last) + last->next = value; + last = value; + + ch = input(); + if (ch == '}') { + return 1; + } + } while (ch == ','); + + return -1; +} + +static int json_parse_property(struct json_value *value) +{ + struct json_value key; + int ret; + int ch; + + json_skip_whitespace(); + + ret = json_parse_string(&key); + if (ret <= 0) + return -1; + + value->key = key.u.string; + + json_skip_whitespace(); + + ch = input(); + if (ch != ':') + return -1; + + ret = json_parse_value(value); + if (ret <= 0) + return -1; + + return 1; +} + +struct json_value *json_parse(const char *json) +{ + struct json_value *root; + int ret; + + input_buf = json; + input_pos = 0; + input_len = strlen(input_buf); + + root = calloc(1, sizeof(*root)); + if (!root) + return NULL; + + ret = json_parse_value(root); + if (ret != 1) { + free(root); + return NULL; + } + + return root; +} + +struct json_value *json_parse_file(const char *file) +{ + struct json_value *root; + struct stat sb; + int ret; + int fd; + + fd = open(file, O_RDONLY); + if (fd < 0) { + fprintf(stderr, "failed to open %s: %s\n", file, strerror(errno)); + return NULL; + } + + ret = fstat(fd, &sb); + if (ret < 0) + return NULL; + + input_pos = 0; + input_len = sb.st_size; + input_buf = malloc(sb.st_size); + + ret = read(fd, (char *)input_buf, input_len); + + close(fd); + + if (ret != input_len) { + fprintf(stderr, "failed to read %d bytes form %s\n", input_len, file); + return NULL; + } + + root = calloc(1, sizeof(*root)); + if (!root) + return NULL; + + ret = json_parse_value(root); + if (ret != 1) { + json_free(root); + return NULL; + } + + return root; +} + +struct json_value *json_get_child(struct json_value *object, const char *key) +{ + struct json_value *it; + + if(object->type != JSON_TYPE_OBJECT) + return NULL; + + for (it = object->u.value; it; it = it->next) { + if (!strcmp(it->key, key)) + return it; + } + + return NULL; +} + +int json_count_children(struct json_value *array) +{ + struct json_value *it; + int count = 0; + + if (!array || array->type != JSON_TYPE_ARRAY) + return -1; + + for (it = array->u.value; it; it = it->next) + count++; + + return count; +} + +int json_get_number(struct json_value *object, const char *key, double *number) +{ + struct json_value *it; + + if (!object || object->type != JSON_TYPE_OBJECT) + return -1; + + for (it = object->u.value; it; it = it->next) { + if (!strcmp(it->key, key)) { + if (it->type != JSON_TYPE_NUMBER) + return -1; + + *number = it->u.number; + return 0; + } + } + + return -1; +} + +const char *json_get_string(struct json_value *object, const char *key) +{ + struct json_value *it; + + if (!object || object->type != JSON_TYPE_OBJECT) + return NULL; + + for (it = object->u.value; it; it = it->next) { + if (!strcmp(it->key, key)) { + if (it->type != JSON_TYPE_STRING) + return NULL; + + return it->u.string; + } + } + + return NULL; +} + +void json_free(struct json_value *value) +{ + struct json_value *next; + struct json_value *it; + + free((char *)value->key); + + switch (value->type) { + case JSON_TYPE_OBJECT: + case JSON_TYPE_ARRAY: + it = value->u.value; + while (it) { + next = it->next; + json_free(it); + it = next; + } + break; + case JSON_TYPE_STRING: + free((char *)value->u.string); + break; + } + + free(value); +} diff --git a/shared/utils/pd-mapper/json.h b/shared/utils/pd-mapper/json.h new file mode 100644 index 0000000..91790a0 --- /dev/null +++ b/shared/utils/pd-mapper/json.h @@ -0,0 +1,67 @@ +/* + * 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. + */ +#ifndef __JSON_H__ +#define __JSON_H__ + + +enum { + JSON_TYPE_UNKNOWN, + JSON_TYPE_TRUE, + JSON_TYPE_FALSE, + JSON_TYPE_NULL, + JSON_TYPE_NUMBER, + JSON_TYPE_STRING, + JSON_TYPE_ARRAY, + JSON_TYPE_OBJECT, +}; + +struct json_value { + const char *key; + + int type; + union { + double number; + const char *string; + struct json_value *value; + } u; + + struct json_value *next; +}; + +struct json_value *json_parse(const char *json); +struct json_value *json_parse_file(const char *file); +int json_count_children(struct json_value *array); +struct json_value *json_get_child(struct json_value *object, const char *key); +int json_get_number(struct json_value *object, const char *key, double *number); +const char *json_get_string(struct json_value *object, const char *key); +void json_free(struct json_value *value); + +#endif diff --git a/shared/utils/pd-mapper/pd-mapper.c b/shared/utils/pd-mapper/pd-mapper.c new file mode 100644 index 0000000..376d9fe --- /dev/null +++ b/shared/utils/pd-mapper/pd-mapper.c @@ -0,0 +1,388 @@ +/* + * 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 <sys/stat.h> +#include <sys/types.h> +#include <err.h> +#include <errno.h> +#include <dirent.h> +#include <fcntl.h> +#include <libgen.h> +#include <libqrtr.h> +#include <limits.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> + +#include "assoc.h" +#include "json.h" +#include "servreg_loc.h" + +struct pd_map { + const char *service; + const char *domain; + int instance; +}; + +static struct pd_map *pd_maps; + +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)); + } +} + +static int pd_load_map(const char *file) +{ + static int num_pd_maps; + struct json_value *sr_service; + struct json_value *sr_domain; + struct json_value *root; + struct json_value *it; + const char *subdomain; + const char *provider; + const char *service; + const char *domain; + const char *soc; + struct pd_map *newp; + struct pd_map *map; + double number; + int count; + int ret; + + root = json_parse_file(file); + if (!root) + return -1; + + sr_domain = json_get_child(root, "sr_domain"); + soc = json_get_string(sr_domain, "soc"); + domain = json_get_string(sr_domain, "domain"); + subdomain = json_get_string(sr_domain, "subdomain"); + ret = json_get_number(sr_domain, "qmi_instance_id", &number); + if (ret) + return ret; + + if (!soc || !domain || !subdomain) { + fprintf(stderr, "failed to parse sr_domain\n"); + return -1; + } + + sr_service = json_get_child(root, "sr_service"); + count = json_count_children(sr_service); + if (count < 0) + return count; + + newp = realloc(pd_maps, (num_pd_maps + count + 1) * sizeof(*newp)); + if (!newp) + return -1; + pd_maps = newp; + + for (it = sr_service->u.value; it; it = it->next) { + provider = json_get_string(it, "provider"); + service = json_get_string(it, "service"); + + if (!provider || !service) { + fprintf(stderr, + "failed to parse provdider or service from %s\n", + file); + return -1; + } + + map = &pd_maps[num_pd_maps++]; + + map->service = malloc(strlen(provider) + strlen(service) + 2); + sprintf((char *)map->service, "%s/%s", provider, service); + + map->domain = malloc(strlen(soc) + strlen(domain) + strlen(subdomain) + 3); + sprintf((char *)map->domain, "%s/%s/%s", soc, domain, subdomain); + + map->instance = number; + } + + pd_maps[num_pd_maps].service = NULL; + + json_free(root); + + return 0; +} + +#ifndef ANDROID +#define FIRMWARE_BASE "/lib/firmware/" +#else +#define FIRMWARE_BASE "/vendor/firmware/" +#endif + +static int pd_enumerate_jsons(struct assoc *json_set) +{ + char firmware_value[PATH_MAX]; + char json_path[PATH_MAX]; + char firmware_attr[32]; + struct dirent *fw_de; + char path[PATH_MAX]; + struct dirent *de; + int firmware_fd; + DIR *class_dir; + int class_fd; + DIR *fw_dir; + size_t len; + size_t n; + + 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; + } + + firmware_value[n] = '\0'; + + if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 > sizeof(path)) + continue; + + strcpy(path, FIRMWARE_BASE); + strcat(path, dirname(firmware_value)); + + fw_dir = opendir(path); + while ((fw_de = readdir(fw_dir)) != NULL) { + if (!strcmp(fw_de->d_name, ".") || !strcmp(fw_de->d_name, "..")) + continue; + + len = strlen(fw_de->d_name); + if (len < 5 || strcmp(&fw_de->d_name[len - 4], ".jsn")) + continue; + + if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 + + strlen(fw_de->d_name) + 1 > sizeof(path)) + continue; + + strcpy(json_path, path); + strcat(json_path, "/"); + strcat(json_path, fw_de->d_name); + + assoc_set(json_set, json_path, NULL); + } + + closedir(fw_dir); + } + + closedir(class_dir); +close_class: + close(class_fd); + + return 0; +} + +static int pd_load_maps(void) +{ + struct assoc json_set; + unsigned long it; + const char *jsn; + int ret = 0; + + assoc_init(&json_set, 20); + + pd_enumerate_jsons(&json_set); + + assoc_foreach(jsn, NULL, &json_set, it) { + ret = pd_load_map(jsn); + if (ret < 0) + break; + } + + assoc_destroy(&json_set); + + return ret; +} + +int main(int argc __unused, char **argv __unused) +{ + struct sockaddr_qrtr sq; + struct qrtr_packet pkt; + unsigned int msg_id; + socklen_t sl; + char buf[4096]; + int ret; + int fd; + + ret = pd_load_maps(); + if (ret) + exit(1); + + if (!pd_maps) { + fprintf(stderr, "no pd maps available\n"); + exit(1); + } + + 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/shared/utils/pd-mapper/pd-mapper.service.in b/shared/utils/pd-mapper/pd-mapper.service.in new file mode 100644 index 0000000..09b594b --- /dev/null +++ b/shared/utils/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/shared/utils/pd-mapper/servreg_loc.c b/shared/utils/pd-mapper/servreg_loc.c new file mode 100644 index 0000000..6309498 --- /dev/null +++ b/shared/utils/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/shared/utils/pd-mapper/servreg_loc.h b/shared/utils/pd-mapper/servreg_loc.h new file mode 100644 index 0000000..2ac5faa --- /dev/null +++ b/shared/utils/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/shared/utils/pd-mapper/servreg_loc.qmi b/shared/utils/pd-mapper/servreg_loc.qmi new file mode 100644 index 0000000..4dc04e6 --- /dev/null +++ b/shared/utils/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; |