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

Fix the code handling the firmware path set in sysfs

Android.bp

@@ -3,6 +3,8 @@ cc_binary {
     vendor: true,
     srcs: [
 	"pd-mapper.c",
+	"assoc.c",
+	"json.c",
 	"servreg_loc.c",
     ],
     shared_libs: ["libqrtr"],

Makefile

@@ -1,16 +1,17 @@
 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 \
         assoc.c \
         json.c \
-	servreg_loc.c
+	servreg_loc.c \
+	lzma_decomp.c
 
 OBJS := $(SRCS:.c=.o)
 

json.c

@@ -32,6 +32,7 @@
 #include <ctype.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <lzma.h>
 #include <stdarg.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -314,6 +315,8 @@ struct json_value *json_parse(const char *json)
 	return root;
 }
 
+extern int lzma_decomp(const char *file);
+
 struct json_value *json_parse_file(const char *file)
 {
 	struct json_value *root;
@@ -321,7 +324,11 @@ struct json_value *json_parse_file(const char *file)
 	int ret;
 	int fd;
 
-	fd = open(file, O_RDONLY);
+	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;

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

@@ -193,11 +193,90 @@ static int pd_load_map(const char *file)
 	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;
@@ -243,25 +322,37 @@ static int pd_enumerate_jsons(struct assoc *json_set)
 		}
 
 		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);
 
-		if (strlen(FIRMWARE_BASE) + strlen(firmware_value) + 1 > sizeof(path))
+		fw_dir = opendir_firmware(firmware_path, path, sizeof(path));
+		if (!fw_dir)
 			continue;
 
-		strcpy(path, FIRMWARE_BASE);
-		strcat(path, dirname(firmware_value));
-
-		fw_dir = opendir(path);
 		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);
-			if (len < 5 || strcmp(&fw_de->d_name[len - 4], ".jsn"))
+
+			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(FIRMWARE_BASE) + strlen(firmware_value) + 1 +
+			if (strlen(path) + 1 + strlen(fw_de->d_name) + 1 > sizeof(json_path))
-			    strlen(fw_de->d_name) + 1 > sizeof(path))
+				continue;
-					continue;
 
 			strcpy(json_path, path);
 			strcat(json_path, "/");
@@ -271,6 +362,7 @@ static int pd_enumerate_jsons(struct assoc *json_set)
 		}
 
 		closedir(fw_dir);
+		free(firmware_value_copy);
 	}
 
 	closedir(class_dir);

pd-mapper.service.in

@@ -1,7 +1,5 @@
 [Unit]
 Description=Qualcomm PD mapper service
-Requires=qrtr-ns.service
-After=qrtr-ns.service
 
 [Service]
 ExecStart=PD_MAPPER_PATH/pd-mapper