kwbimage_v1: add support to boot a mvebu image

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@pengutronix.de>

2 files changed, 85 insertions, 0 deletions

diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index 2cf0dfed47..fb5c4e6bcd 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 0000000000..8d364ceb7b
--- /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);