[PATCH v6 27/27] ARM: copy DTB appended to zImage

[Stefano Stabellini <stefano.stabellini@eu.citrix.com>]

From: David Vrabel <david.vrabel@citrix.com>

When copying a zImage from flash, also copy any appended device tree
blob.

Signed-off-by: David Vrabel <david.vrabel@citrix.com>
---
 xen/arch/arm/kernel.c |   63 ++++++++++++++++++++++++++++++++++--------------
 1 files changed, 44 insertions(+), 19 deletions(-)

diff --git a/xen/arch/arm/kernel.c b/xen/arch/arm/kernel.c
index 5fb2ba0..d4ffa4f 100644
--- a/xen/arch/arm/kernel.c
+++ b/xen/arch/arm/kernel.c
@@ -10,6 +10,7 @@
 #include <xen/mm.h>
 #include <xen/domain_page.h>
 #include <xen/sched.h>
+#include <asm/byteorder.h>
 
 #include "kernel.h"
 
@@ -23,6 +24,40 @@
 
 #define ZIMAGE_MAGIC 0x016f2818
 
+struct minimal_dtb_header {
+    uint32_t magic;
+    uint32_t total_size;
+    /* There are other fields but we don't use them yet. */
+};
+
+#define DTB_MAGIC 0xd00dfeed
+
+static void copy_from_flash(void *dst, paddr_t flash, unsigned long len)
+{
+    void *src = (void *)FIXMAP_ADDR(FIXMAP_MISC);
+
+    printk("Copying %#lx bytes from flash %"PRIpaddr" to %p",
+           len, flash, dst);
+
+    while (len) {
+        paddr_t p;
+        unsigned long l, s;
+
+        p = flash >> PAGE_SHIFT;
+        s = flash & (PAGE_SIZE-1);
+        l = min(PAGE_SIZE - s, len);
+
+        set_fixmap(FIXMAP_MISC, p, DEV_SHARED);
+        memcpy(dst, src + s, l);
+
+        flash += l;
+        dst += l;
+        len -= l;
+    }
+
+    clear_fixmap(FIXMAP_MISC);
+}
+
 static void kernel_zimage_load(struct kernel_info *info)
 {
     paddr_t load_addr = info->zimage.load_addr;
@@ -58,6 +93,7 @@ static int kernel_try_zimage_prepare(struct kernel_info *info)
 {
     uint32_t *zimage = (void *)FIXMAP_ADDR(FIXMAP_MISC);
     uint32_t start, end;
+    struct minimal_dtb_header dtb_hdr;
 
     set_fixmap(FIXMAP_MISC, KERNEL_FLASH_ADDRESS >> PAGE_SHIFT, DEV_SHARED);
 
@@ -69,6 +105,14 @@ static int kernel_try_zimage_prepare(struct kernel_info *info)
 
     clear_fixmap(FIXMAP_MISC);
 
+    /*
+     * Check for an appended DTB.
+     */
+    copy_from_flash(&dtb_hdr, KERNEL_FLASH_ADDRESS + end - start, sizeof(dtb_hdr));
+    if (be32_to_cpu(dtb_hdr.magic) == DTB_MAGIC) {
+        end += be32_to_cpu(dtb_hdr.total_size);
+    }
+
     /* FIXME: get RAM location from appended DTB (if there is one)? */
 
     /*
@@ -97,25 +141,6 @@ static void kernel_elf_load(struct kernel_info *info)
     free_xenheap_pages(info->kernel_img, info->kernel_order);
 }
 
-static void copy_from_flash(void *dst, paddr_t flash, unsigned long len)
-{
-    void *src = (void *)FIXMAP_ADDR(FIXMAP_MISC);
-    unsigned long offs;
-
-    printk("Copying %#lx bytes from flash %"PRIpaddr" to %p-%p: [",
-           len, flash, dst, dst+(1<<23));
-    for ( offs = 0; offs < len ; offs += PAGE_SIZE )
-    {
-        if ( ( offs % (1<<20) ) == 0 )
-            printk(".");
-        set_fixmap(FIXMAP_MISC, (flash+offs) >> PAGE_SHIFT, DEV_SHARED);
-        memcpy(dst+offs, src, PAGE_SIZE);
-    }
-    printk("]\n");
-
-    clear_fixmap(FIXMAP_MISC);
-}
-
 static int kernel_try_elf_prepare(struct kernel_info *info)
 {
     int rc;
-- 
1.7.2.5