[PATCH 3/4] kwbimage_v1: add support to boot a mvebu image

[Uwe Kleine-König]

This just starts the main image of the mvebu image assuming that the
header images just setup the RAM. The position of the internal register
window is provided in the header as introduced in the previous commit.

Signed-off-by: Uwe Kleine-König <u.kleine-koenig at pengutronix.de>
---
 arch/arm/mach-mvebu/Makefile      |  1 +
 arch/arm/mach-mvebu/kwbootimage.c | 84 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 85 insertions(+)
 create mode 100644 arch/arm/mach-mvebu/kwbootimage.c

diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index 2cf0dfed4725..fb5c4e6bcd66 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -3,3 +3,4 @@ obj-$(CONFIG_ARCH_ARMADA_370)	+= armada-370-xp.o
 obj-$(CONFIG_ARCH_ARMADA_XP)	+= armada-370-xp.o
 obj-$(CONFIG_ARCH_DOVE)		+= dove.o
 obj-$(CONFIG_ARCH_KIRKWOOD)	+= kirkwood.o
+obj-y				+= kwbootimage.o
diff --git a/arch/arm/mach-mvebu/kwbootimage.c b/arch/arm/mach-mvebu/kwbootimage.c
new file mode 100644
index 000000000000..8d364ceb7b31
--- /dev/null
+++ b/arch/arm/mach-mvebu/kwbootimage.c
@@ -0,0 +1,84 @@
+#include <bootm.h>
+#include <common.h>
+#include <fcntl.h>
+#include <filetype.h>
+#include <fs.h>
+#include <init.h>
+#include <libfile.h>
+#include <restart.h>
+#include <unistd.h>
+#include <asm/unaligned.h>
+#include <mach/common.h>
+
+static int do_bootm_kwbimage_v1(struct image_data *data)
+{
+	int fd, ret;
+	loff_t offset;
+	char header[0x20];
+	u32 image_size, image_source;
+	void (*barebox)(void);
+
+	fd = open(data->os_file, O_RDONLY);
+	if (fd < 0)
+		return fd;
+
+	ret = read_full(fd, header, 0x20);
+	if (ret < 0x20) {
+		pr_err("Failed to read header\n");
+		if (ret >= 0)
+			return -EINVAL;
+		return -errno;
+	}
+
+	image_size = header[4] | header[5] << 8 | header[6] << 16 | header[7] << 24;
+	image_source = header[0xc] | header[0xd] << 8 |
+		header[0xe] << 16 | header[0xf] << 24;
+
+	if (data->verbose)
+		pr_info("size: %u\noffset: %u\n", image_size, image_source);
+
+	offset = lseek(fd, image_source, SEEK_SET);
+	if (offset < 0) {
+		pr_err("Failed to seek to image (%lld, %d)\n", offset, errno);
+		return -errno;
+	}
+
+	barebox = xzalloc(image_size);
+
+	ret = read_full(fd, barebox, image_size);
+	if (ret < image_size) {
+		pr_err("Failed to read image\n");
+		if (ret >= 0)
+			ret = -EINVAL;
+		else
+			ret = -errno;
+		goto out_free;
+	}
+
+	if (is_barebox_arm_head((void *)barebox))
+		put_unaligned_le32(MVEBU_REMAP_INT_REG_BASE, barebox + 0x30);
+
+	shutdown_barebox();
+
+	barebox();
+
+	restart_machine();
+
+out_free:
+	free(barebox);
+	return ret;
+}
+
+static struct image_handler image_handler_kwbimage_v1_handler = {
+	.name = "MVEBU kwbimage v1",
+	.bootm = do_bootm_kwbimage_v1,
+	.filetype = filetype_kwbimage_v1,
+};
+
+static int mvebu_register_kwbimage_image_handler(void)
+{
+	register_image_handler(&image_handler_kwbimage_v1_handler);
+
+	return 0;
+}
+late_initcall(mvebu_register_kwbimage_image_handler);
-- 
2.11.0