Merge pull request #17 from lumag/fix-sysfs-path

Fix the code handling the firmware path set in sysfs

Android.bp

@@ -0,0 +1,11 @@
+cc_binary {
+    name: "pd-mapper",
+    vendor: true,
+    srcs: [
+	"pd-mapper.c",
+	"assoc.c",
+	"json.c",
+	"servreg_loc.c",
+    ],
+    shared_libs: ["libqrtr"],
+}

Makefile

@@ -1,18 +1,29 @@
 PD_MAPPER := pd-mapper
 
-CFLAGS := -Wall -g -O2
+CFLAGS += -Wall -g -O2
-LDFLAGS := -lqrtr
+LDFLAGS += -lqrtr -llzma
+prefix ?= /usr/local
+
+bindir := $(prefix)/bin
+servicedir := $(prefix)/lib/systemd/system
 
 SRCS := pd-mapper.c \
-	servreg_loc.c
+        assoc.c \
+        json.c \
+	servreg_loc.c \
+	lzma_decomp.c
 
 OBJS := $(SRCS:.c=.o)
 
 $(PD_MAPPER): $(OBJS)
 	$(CC) -o $@ $^ $(LDFLAGS)
 
-install: $(PD_MAPPER)
+pd-mapper.service: pd-mapper.service.in
-	install -D -m 755 $< $(DESTDIR)$(prefix)/bin/$<
+	@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)
+	rm -f $(PD_MAPPER) $(OBJS) pd-mapper.service

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;
+}

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

json.c

@@ -0,0 +1,458 @@
+/*
+ * 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 <lzma.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;
+}
+
+extern int lzma_decomp(const char *file);
+
+struct json_value *json_parse_file(const char *file)
+{
+	struct json_value *root;
+	struct stat sb;
+	int ret;
+	int fd;
+
+	if ((strlen(file) > 3) && !strcmp(&file[strlen(file)-3], ".xz"))
+	        fd = lzma_decomp(file);
+	else
+	        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);
+}

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

lzma_decomp.c

@@ -0,0 +1,287 @@
+/*
+ *  Original Author:	 Lasse Collin
+ *   LZMA boilerplate decompression example.
+ *
+ *  Imported to pd-mapper by Jeremy Linton
+ *   who reworked the main() into lzma_decomp()
+ *   which returns a FD to a decompressed/unlinked
+ *   file.
+ *
+ *  This file has been put into the public domain.
+ *  You can do whatever you want with this file.
+ *
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <lzma.h>
+
+
+static bool
+init_decoder(lzma_stream *strm)
+{
+	// Initialize a .xz decoder. The decoder supports a memory usage limit
+	// and a set of flags.
+	//
+	// The memory usage of the decompressor depends on the settings used
+	// to compress a .xz file. It can vary from less than a megabyte to
+	// a few gigabytes, but in practice (at least for now) it rarely
+	// exceeds 65 MiB because that's how much memory is required to
+	// decompress files created with "xz -9". Settings requiring more
+	// memory take extra effort to use and don't (at least for now)
+	// provide significantly better compression in most cases.
+	//
+	// Memory usage limit is useful if it is important that the
+	// decompressor won't consume gigabytes of memory. The need
+	// for limiting depends on the application. In this example,
+	// no memory usage limiting is used. This is done by setting
+	// the limit to UINT64_MAX.
+	//
+	// The .xz format allows concatenating compressed files as is:
+	//
+	//     echo foo | xz > foobar.xz
+	//     echo bar | xz >> foobar.xz
+	//
+	// When decompressing normal standalone .xz files, LZMA_CONCATENATED
+	// should always be used to support decompression of concatenated
+	// .xz files. If LZMA_CONCATENATED isn't used, the decoder will stop
+	// after the first .xz stream. This can be useful when .xz data has
+	// been embedded inside another file format.
+	//
+	// Flags other than LZMA_CONCATENATED are supported too, and can
+	// be combined with bitwise-or. See lzma/container.h
+	// (src/liblzma/api/lzma/container.h in the source package or e.g.
+	// /usr/include/lzma/container.h depending on the install prefix)
+	// for details.
+	lzma_ret ret = lzma_stream_decoder(
+			strm, UINT64_MAX, LZMA_CONCATENATED);
+
+	// Return successfully if the initialization went fine.
+	if (ret == LZMA_OK)
+		return true;
+
+	// Something went wrong. The possible errors are documented in
+	// lzma/container.h (src/liblzma/api/lzma/container.h in the source
+	// package or e.g. /usr/include/lzma/container.h depending on the
+	// install prefix).
+	//
+	// Note that LZMA_MEMLIMIT_ERROR is never possible here. If you
+	// specify a very tiny limit, the error will be delayed until
+	// the first headers have been parsed by a call to lzma_code().
+	const char *msg;
+	switch (ret) {
+	case LZMA_MEM_ERROR:
+		msg = "Memory allocation failed";
+		break;
+
+	case LZMA_OPTIONS_ERROR:
+		msg = "Unsupported decompressor flags";
+		break;
+
+	default:
+		// This is most likely LZMA_PROG_ERROR indicating a bug in
+		// this program or in liblzma. It is inconvenient to have a
+		// separate error message for errors that should be impossible
+		// to occur, but knowing the error code is important for
+		// debugging. That's why it is good to print the error code
+		// at least when there is no good error message to show.
+		msg = "Unknown error, possibly a bug";
+		break;
+	}
+
+	fprintf(stderr, "Error initializing the decoder: %s (error code %u)\n",
+			msg, ret);
+	return false;
+}
+
+
+static bool
+decompress(lzma_stream *strm, const char *inname, FILE *infile, int outfile)
+{
+	// When LZMA_CONCATENATED flag was used when initializing the decoder,
+	// we need to tell lzma_code() when there will be no more input.
+	// This is done by setting action to LZMA_FINISH instead of LZMA_RUN
+	// in the same way as it is done when encoding.
+	//
+	// When LZMA_CONCATENATED isn't used, there is no need to use
+	// LZMA_FINISH to tell when all the input has been read, but it
+	// is still OK to use it if you want. When LZMA_CONCATENATED isn't
+	// used, the decoder will stop after the first .xz stream. In that
+	// case some unused data may be left in strm->next_in.
+	lzma_action action = LZMA_RUN;
+
+	uint8_t inbuf[BUFSIZ];
+	uint8_t outbuf[BUFSIZ];
+
+	strm->next_in = NULL;
+	strm->avail_in = 0;
+	strm->next_out = outbuf;
+	strm->avail_out = sizeof(outbuf);
+
+	while (true) {
+		if (strm->avail_in == 0 && !feof(infile)) {
+			strm->next_in = inbuf;
+			strm->avail_in = fread(inbuf, 1, sizeof(inbuf),
+					infile);
+
+			if (ferror(infile)) {
+				fprintf(stderr, "%s: Read error: %s\n",
+						inname, strerror(errno));
+				return false;
+			}
+
+			// Once the end of the input file has been reached,
+			// we need to tell lzma_code() that no more input
+			// will be coming. As said before, this isn't required
+			// if the LZMA_CONCATENATED flag isn't used when
+			// initializing the decoder.
+			if (feof(infile))
+				action = LZMA_FINISH;
+		}
+
+		lzma_ret ret = lzma_code(strm, action);
+
+		if (strm->avail_out == 0 || ret == LZMA_STREAM_END) {
+			size_t write_size = sizeof(outbuf) - strm->avail_out;
+
+			if (write(outfile, outbuf, write_size) != write_size) {
+				fprintf(stderr, "Write error: %s\n",
+						strerror(errno));
+				return false;
+			}
+
+			strm->next_out = outbuf;
+			strm->avail_out = sizeof(outbuf);
+		}
+
+		if (ret != LZMA_OK) {
+			// Once everything has been decoded successfully, the
+			// return value of lzma_code() will be LZMA_STREAM_END.
+			//
+			// It is important to check for LZMA_STREAM_END. Do not
+			// assume that getting ret != LZMA_OK would mean that
+			// everything has gone well or that when you aren't
+			// getting more output it must have successfully
+			// decoded everything.
+			if (ret == LZMA_STREAM_END)
+				return true;
+
+			// It's not LZMA_OK nor LZMA_STREAM_END,
+			// so it must be an error code. See lzma/base.h
+			// (src/liblzma/api/lzma/base.h in the source package
+			// or e.g. /usr/include/lzma/base.h depending on the
+			// install prefix) for the list and documentation of
+			// possible values. Many values listen in lzma_ret
+			// enumeration aren't possible in this example, but
+			// can be made possible by enabling memory usage limit
+			// or adding flags to the decoder initialization.
+			const char *msg;
+			switch (ret) {
+			case LZMA_MEM_ERROR:
+				msg = "Memory allocation failed";
+				break;
+
+			case LZMA_FORMAT_ERROR:
+				// .xz magic bytes weren't found.
+				msg = "The input is not in the .xz format";
+				break;
+
+			case LZMA_OPTIONS_ERROR:
+				// For example, the headers specify a filter
+				// that isn't supported by this liblzma
+				// version (or it hasn't been enabled when
+				// building liblzma, but no-one sane does
+				// that unless building liblzma for an
+				// embedded system). Upgrading to a newer
+				// liblzma might help.
+				//
+				// Note that it is unlikely that the file has
+				// accidentally became corrupt if you get this
+				// error. The integrity of the .xz headers is
+				// always verified with a CRC32, so
+				// unintentionally corrupt files can be
+				// distinguished from unsupported files.
+				msg = "Unsupported compression options";
+				break;
+
+			case LZMA_DATA_ERROR:
+				msg = "Compressed file is corrupt";
+				break;
+
+			case LZMA_BUF_ERROR:
+				// Typically this error means that a valid
+				// file has got truncated, but it might also
+				// be a damaged part in the file that makes
+				// the decoder think the file is truncated.
+				// If you prefer, you can use the same error
+				// message for this as for LZMA_DATA_ERROR.
+				msg = "Compressed file is truncated or "
+						"otherwise corrupt";
+				break;
+
+			default:
+				// This is most likely LZMA_PROG_ERROR.
+				msg = "Unknown error, possibly a bug";
+				break;
+			}
+
+			fprintf(stderr, "%s: Decoder error: "
+					"%s (error code %u)\n",
+					inname, msg, ret);
+			return false;
+		}
+	}
+}
+
+#define TEMP_TEMPLATE "/tmp/pd-mapperXXXXXX"
+
+
+int lzma_decomp(const char *file)
+{
+	int return_fd;
+	char temp_file[sizeof(TEMP_TEMPLATE)];
+	lzma_stream strm = LZMA_STREAM_INIT;
+
+	strcpy(temp_file, TEMP_TEMPLATE);
+
+	return_fd = mkstemp(temp_file);
+	if (return_fd < 0)
+		return return_fd;
+
+	unlink(temp_file);
+
+	// Try to decompress all files.
+	if (!init_decoder(&strm)) {
+		// Decoder initialization failed. There's no point
+		// to retry it so we need to exit.
+		close(return_fd);
+		return -1;
+	}
+
+	FILE *infile = fopen(file, "rb");
+
+	if (infile == NULL) {
+	       fprintf(stderr, "%s: Error opening the input file: %s\n",
+		       file, strerror(errno));
+	       close(return_fd);
+	       return_fd = -1;
+	} else {
+		if (!decompress(&strm, file, infile, return_fd)) {
+			close(return_fd);
+			return_fd = -1;
+		} else {
+		        lseek(return_fd, 0, SEEK_SET);
+		}
+		fclose(infile);
+	}
+
+	// Free the memory allocated for the decoder. This only needs to be
+	// done after the last file.
+	lzma_end(&strm);
+
+	return return_fd;
+}

pd-mapper.c

@@ -28,13 +28,22 @@
  * 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 {
@@ -43,10 +52,7 @@ struct pd_map {
 	int instance;
 };
 
-static const struct pd_map pd_maps[] = {
+static 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)
 {
@@ -113,6 +119,281 @@ respond:
 	}
 }
 
+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;
+}
+
+static int concat_path(char *base_path, char *firmware_path, char *path, size_t path_len)
+{
+	if (strlen(base_path) + 1 + strlen(firmware_path) + 1 >= path_len) {
+		warn("Path length exceeded %lu\n", path_len);
+		return -1;
+	}
+
+	strcpy(path, base_path);
+	strcat(path, "/");
+	strcat(path, firmware_path);
+
+	return 0;
+}
+
+#ifndef ANDROID
+#define FIRMWARE_BASE	"/lib/firmware/"
+#define FIRMWARE_PARAM_PATH	"/sys/module/firmware_class/parameters/path"
+
+static DIR *opendir_firmware(char *firmware_path, char *out_path_opened, size_t out_path_size)
+{
+	int ret = 0, n;
+	DIR *fw_dir = NULL;
+	int fw_param_path = open(FIRMWARE_PARAM_PATH, O_RDONLY);
+	char fw_sysfs_path[PATH_MAX];
+
+	if (fw_param_path < 0) {
+		warn("Cannot open sysfs path: %s", FIRMWARE_PARAM_PATH);
+		close(fw_param_path);
+		goto err;
+	}
+
+	n = read(fw_param_path, fw_sysfs_path, sizeof(char) * PATH_MAX);
+	close(fw_param_path);
+	if (n < 0) {
+		warn("Cannot read sysfs path: %s", FIRMWARE_PARAM_PATH);
+		goto err;
+	}
+
+	/* path not set in sysfs */
+	if (n <= 1)
+		goto err;
+
+	fw_sysfs_path[n - 1] = '\0';
+
+	ret = concat_path(fw_sysfs_path, firmware_path, out_path_opened, out_path_size);
+	if (ret)
+		goto err;
+
+	fw_dir = opendir(out_path_opened);
+	if (!fw_dir)
+		goto err;
+
+	return fw_dir;
+err:
+	ret = concat_path(FIRMWARE_BASE, firmware_path, out_path_opened, out_path_size);
+	if (ret)
+		return fw_dir;
+
+	fw_dir = opendir(out_path_opened);
+	if (!fw_dir)
+		warn("Cannot open firmware path: %s", fw_sysfs_path);
+
+	return fw_dir;
+}
+#else
+#define FIRMWARE_BASE	"/vendor/firmware/"
+
+DIR opendir_firmware(char *firmware_path)
+{
+	return open_concat_path("/vendor/firmware/", firmware_path);
+}
+#endif
+
+static char *known_extensions[] = {
+  ".jsn.xz",
+  ".jsn",
+  NULL,
+};
+
+static int pd_enumerate_jsons(struct assoc *json_set)
+{
+	char firmware_value[PATH_MAX];
+	char *firmware_value_copy = NULL;
+	char *firmware_path = NULL;
+	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';
+		/* dirname() function can (and will) modify the parameter, so make a copy */
+		firmware_value_copy = strdup(firmware_value);
+		firmware_path = dirname(firmware_value_copy);
+
+		fw_dir = opendir_firmware(firmware_path, path, sizeof(path));
+		if (!fw_dir)
+			continue;
+
+		while ((fw_de = readdir(fw_dir)) != NULL) {
+			int extens_index;
+			bool found = false;
+
+			if (!strcmp(fw_de->d_name, ".") || !strcmp(fw_de->d_name, ".."))
+				continue;
+
+			len = strlen(fw_de->d_name);
+
+			for (extens_index = 0; known_extensions[extens_index] != NULL; extens_index++) {
+				int extens_len = strlen(known_extensions[extens_index]);
+				if (len > extens_len &&
+				    !strcmp(&fw_de->d_name[len - extens_len], known_extensions[extens_index])) {
+					found = true;
+					break;
+				}
+			}
+
+			if (!found)
+				continue;
+
+			if (strlen(path) + 1 + strlen(fw_de->d_name) + 1 > sizeof(json_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);
+		free(firmware_value_copy);
+	}
+
+	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, char **argv)
 {
 	struct sockaddr_qrtr sq;
@@ -123,6 +404,15 @@ int main(int argc, char **argv)
 	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");

pd-mapper.service.in

@@ -0,0 +1,9 @@
+[Unit]
+Description=Qualcomm PD mapper service
+
+[Service]
+ExecStart=PD_MAPPER_PATH/pd-mapper
+Restart=always
+
+[Install]
+WantedBy=multi-user.target