qdl: Implement sparse image flashing using libsparse

Extend XML with the optional "sparse" tag. If sparse is true the file
is interpreted as sparse file and only non "Don't Care" Sections are
flashed.

Signed-off-by: Maximilian Blenk <[email protected]>

Author: Maximilian Blenk

Makefile

@@ -1,17 +1,21 @@
 OUT := qdl
 
-CFLAGS := -O2 -Wall -g `xml2-config --cflags`
-LDFLAGS := `xml2-config --libs` -ludev
+CFLAGS := -O2 -Wall -g `xml2-config --cflags` -I libsparse/include
+LDFLAGS := `xml2-config --libs` -ludev -lz
 prefix := /usr/local
 
 SRCS := firehose.c qdl.c sahara.c util.c patch.c program.c ufs.c
 OBJS := $(SRCS:.c=.o)
 
-$(OUT): $(OBJS)
-	$(CC) -o $@ $^ $(LDFLAGS)
+$(OUT): $(OBJS) libsparse/libsparse.a
+	$(CXX) -o $@ $^ $(LDFLAGS)
+
+libsparse/libsparse.a:
+	$(MAKE) -C libsparse
 
 clean:
 	rm -f $(OUT) $(OBJS)
+	$(MAKE) -C libsparse clean
 
 install: $(OUT)
 	install -D -m 755 $< $(DESTDIR)$(prefix)/bin/$<

firehose.c

@@ -48,9 +48,12 @@
 #include <unistd.h>
 #include <libxml/parser.h>
 #include <libxml/tree.h>
+#include <sparse/sparse.h>
 #include "qdl.h"
 #include "ufs.h"
 
+#define SPARSE_BUFFER_SIZE 8
+
 static void xml_setpropf(xmlNode *node, const char *attr, const char *fmt, ...)
 {
 	xmlChar buf[128];
@@ -288,14 +291,47 @@ static int firehose_configure(struct qdl_device *qdl, bool skip_storage_init, co
 #define MIN(x, y) ((x) < (y) ? (x) : (y))
 #define ROUND_UP(x, a) (((x) + (a) - 1) & ~((a) - 1))
 
+static int firehose_program_init(struct qdl_device *qdl, unsigned int sector_size,
+				 unsigned int num_sectors, unsigned int partition_number,
+				 const char *start_sector, const char *filename) {
+	xmlNode *root;
+	xmlNode *node;
+	xmlDoc *doc;
+	int ret;
+
+	doc = xmlNewDoc((xmlChar*)"1.0");
+	root = xmlNewNode(NULL, (xmlChar*)"data");
+	xmlDocSetRootElement(doc, root);
+
+	node = xmlNewChild(root, NULL, (xmlChar*)"program", NULL);
+	xml_setpropf(node, "SECTOR_SIZE_IN_BYTES", "%d", sector_size);
+	xml_setpropf(node, "num_partition_sectors", "%d", num_sectors);
+	xml_setpropf(node, "physical_partition_number", "%d", partition_number);
+	xml_setpropf(node, "start_sector", "%s", start_sector);
+	if (filename)
+		xml_setpropf(node, "filename", "%s", filename);
+
+	ret = firehose_write(qdl, doc);
+	if (ret < 0) {
+		fprintf(stderr, "[PROGRAM] failed to write program command\n");
+		goto out;
+	}
+
+	ret = firehose_read(qdl, -1, firehose_nop_parser);
+	if (ret) {
+		fprintf(stderr, "[PROGRAM] failed to setup programming\n");
+		goto out;
+	}
+out:
+	xmlFreeDoc(doc);
+	return ret;
+}
+
 static int firehose_program(struct qdl_device *qdl, struct program *program, int fd)
 {
 	unsigned num_sectors;
 	struct stat sb;
 	size_t chunk_size;
-	xmlNode *root;
-	xmlNode *node;
-	xmlDoc *doc;
 	void *buf;
 	time_t t0;
 	time_t t;
@@ -322,27 +358,10 @@ static int firehose_program(struct qdl_device *qdl, struct program *program, int
 	if (!buf)
 		err(1, "failed to allocate sector buffer");
 
-	doc = xmlNewDoc((xmlChar*)"1.0");
-	root = xmlNewNode(NULL, (xmlChar*)"data");
-	xmlDocSetRootElement(doc, root);
-
-	node = xmlNewChild(root, NULL, (xmlChar*)"program", NULL);
-	xml_setpropf(node, "SECTOR_SIZE_IN_BYTES", "%d", program->sector_size);
-	xml_setpropf(node, "num_partition_sectors", "%d", num_sectors);
-	xml_setpropf(node, "physical_partition_number", "%d", program->partition);
-	xml_setpropf(node, "start_sector", "%s", program->start_sector);
-	if (program->filename)
-		xml_setpropf(node, "filename", "%s", program->filename);
-
-	ret = firehose_write(qdl, doc);
-	if (ret < 0) {
-		fprintf(stderr, "[PROGRAM] failed to write program command\n");
-		goto out;
-	}
+	ret = firehose_program_init(qdl, program->sector_size, num_sectors, program->partition,
+					program->start_sector, program->filename);
 
-	ret = firehose_read(qdl, -1, firehose_nop_parser);
 	if (ret) {
-		fprintf(stderr, "[PROGRAM] failed to setup programming\n");
 		goto out;
 	}
 
@@ -386,10 +405,178 @@ static int firehose_program(struct qdl_device *qdl, struct program *program, int
 	}
 
 out:
-	xmlFreeDoc(doc);
+	free(buf);
 	return ret;
 }
 
+struct Data {
+	struct program* program;
+	long unsigned int offset;
+	struct qdl_device *qdl;
+	void *data_blob;
+	int data_blob_size;
+	int data_blob_count;
+};
+
+int firehose_program_sparse_do_flash(struct qdl_device *qdl, struct program* program, const void *data, long unsigned int num_sectors, long unsigned int start_sector_offset) {
+	int ret;
+	int chunk_size;
+	int n;
+	int left;
+	int offset;
+	long long start_sector;
+	// 2^64 requires 20 chars at max (+ an additional one for '\0')
+	char start_sector_str[21];
+
+	// Calculate new start sector
+	start_sector = atoll(program->start_sector);
+	start_sector += start_sector_offset;
+	snprintf(start_sector_str, sizeof(start_sector_str), "%lld", start_sector);
+
+	ret = firehose_program_init(qdl, program->sector_size, num_sectors, program->partition,
+					start_sector_str, program->filename);
+	if (ret) {
+		return ret;
+	}
+
+	// Send given data to the target
+	left = num_sectors;
+	offset = 0;
+	while (left > 0) {
+		chunk_size = MIN(max_payload_size / program->sector_size, left);
+
+		n = qdl_write(qdl, data + offset, chunk_size * program->sector_size, true);
+		if (n < 0)
+			err(1, "failed to write");
+
+		if (n != chunk_size * program->sector_size)
+			err(1, "failed to write full sector");
+
+		left -= chunk_size;
+		offset += n;
+	}
+
+	ret = firehose_read(qdl, -1, firehose_nop_parser);
+	if (ret) {
+		fprintf(stderr, "[PROGRAM] failed\n");
+	}
+	return ret;
+}
+
+int firehose_program_sparse_callback(void *priv, const void *data, long unsigned int len) {
+	int ret = 0;
+	long unsigned int already_flashed = 0;
+	struct Data *d = priv;
+	unsigned num_sectors = 0;
+
+	if (data == NULL) {
+		// Reached "Don't care" part --> Flash what is currently buffered
+		if (d->data_blob) {
+			num_sectors = (d->data_blob_count + d->program->sector_size - 1) / d->program->sector_size;
+
+			memset(d->data_blob + d->data_blob_count, 0, d->data_blob_size - d->data_blob_count);
+
+			ret = firehose_program_sparse_do_flash(d->qdl, d->program, d->data_blob, num_sectors, d->offset);
+			if (ret) {
+				goto out;
+			}
+
+			d->data_blob_count = 0;
+		}
+
+		d->offset += num_sectors;
+		d->offset += (len + d->program->sector_size - 1) / d->program->sector_size;
+		goto out;
+	}
+	else if ((d->data_blob_size - d->data_blob_count) < len) {
+		// Reach part that contains data --> fill up buffer and flash if neccessary
+		// Fill up current buffer and flash it
+		already_flashed = d->data_blob_size - d->data_blob_count;
+		memcpy(d->data_blob + d->data_blob_count, data, already_flashed);
+		d->data_blob_count += already_flashed;
+		ret = firehose_program_sparse_callback(priv, NULL, 0);
+
+		// Flash the rest of the given data
+		if ((len - already_flashed) > d->data_blob_size) {
+			num_sectors = ((len - already_flashed) / d->program->sector_size);
+
+			ret = firehose_program_sparse_do_flash(d->qdl, d->program, data + already_flashed, num_sectors, d->offset);
+			if (ret) {
+				goto out;
+			}
+			already_flashed += num_sectors * d->program->sector_size;
+			d->data_blob_count = 0;
+			d->offset += num_sectors;
+		}
+	}
+
+	memcpy(d->data_blob + d->data_blob_count, data + already_flashed, len - already_flashed);
+	d->data_blob_count += len - already_flashed;
+
+out:
+	return ret;
+}
+
+static int firehose_program_sparse(struct qdl_device *qdl, struct program *program, struct sparse_file *sparse)
+{
+	unsigned num_sectors;
+	time_t t0;
+	time_t t;
+	int ret;
+	struct Data d;
+
+	num_sectors = program->num_sectors;
+	t0 = time(NULL);
+
+	memset(&d, 0, sizeof(struct Data));
+	d.qdl = qdl;
+	d.program = program;
+	d.data_blob_size = SPARSE_BUFFER_SIZE * max_payload_size;
+	d.data_blob = malloc(d.data_blob_size);
+	if (!d.data_blob) {
+		return -1;
+	}
+
+	// Process the sparse file
+	ret = sparse_file_callback(sparse, false, false, firehose_program_sparse_callback, &d);
+	if (!ret) {
+		// Call once more (pretending a "Don't care" block) to flash current buffer
+		ret = firehose_program_sparse_callback(&d, NULL, 0);
+	}
+	free(d.data_blob);
+
+	t = time(NULL) - t0;
+	if (t) {
+		fprintf(stderr,
+			"[PROGRAM] flashed \"%s\" successfully at %ldkB/s\n",
+			program->label,
+			program->sector_size * num_sectors / t / 1024);
+	} else {
+		fprintf(stderr, "[PROGRAM] flashed \"%s\" successfully\n",
+			program->label);
+	}
+
+	return ret;
+}
+
+static int firehose_program_maybe_sparse(struct qdl_device *qdl, struct program *program, int fd)
+{
+	int ret;
+	struct sparse_file *sparse;
+
+	if (!program->sparse) {
+		return firehose_program(qdl, program, fd);
+	} else {
+		sparse = sparse_file_import(fd, false, false);
+		if (!sparse) {
+			return -1;
+		}
+		ret = firehose_program_sparse(qdl, program, sparse);
+		sparse_file_destroy(sparse);
+		return ret;
+	}
+}
+
 static int firehose_apply_patch(struct qdl_device *qdl, struct patch *patch)
 {
 	xmlNode *root;
@@ -597,7 +784,7 @@ int firehose_run(struct qdl_device *qdl, const char *incdir, const char *storage
 	if (ret)
 		return ret;
 
-	ret = program_execute(qdl, firehose_program, incdir);
+	ret = program_execute(qdl, firehose_program_maybe_sparse, incdir);
 	if (ret)
 		return ret;
 

libsparse/Makefile

@@ -1,5 +1,5 @@
 OUT := libsparse.a
-CXXFLAGS := -O2 -Wall -I include
+CXXFLAGS := -O2 -Wall -I include -std=c++11
 
 SRCS := backed_block.cpp output_file.cpp sparse.cpp sparse_crc32.cpp sparse_err.cpp sparse_read.cpp stringprintf.cpp
 OBJS := $(SRCS:.cpp=.o)

program.c

@@ -37,7 +37,7 @@
 
 #include "program.h"
 #include "qdl.h"
-		
+
 static struct program *programes;
 static struct program *programes_last;
 
@@ -76,6 +76,11 @@ int program_load(const char *program_file)
 		program->num_sectors = attr_as_unsigned(node, "num_partition_sectors", &errors);
 		program->partition = attr_as_unsigned(node, "physical_partition_number", &errors);
 		program->start_sector = attr_as_string(node, "start_sector", &errors);
+		int ignore_errors = 0;
+		const char *sparse = attr_as_string(node, "sparse", &ignore_errors);
+		if (sparse) {
+			program->sparse = strcmp(sparse, "true") == 0;
+		}
 
 		if (errors) {
 			fprintf(stderr, "[PROGRAM] errors while parsing program\n");
@@ -96,7 +101,7 @@ int program_load(const char *program_file)
 
 	return 0;
 }
-	
+
 int program_execute(struct qdl_device *qdl, int (*apply)(struct qdl_device *qdl, struct program *program, int fd),
 		    const char *incdir)
 {

program.h

@@ -12,6 +12,7 @@ struct program {
 	unsigned num_sectors;
 	unsigned partition;
 	const char *start_sector;
+	bool sparse;
 
 	struct program *next;
 };