libsel4platsupport: Add IO FDT ops implementation

This commit adds an implementation of the IO FDT ops interface in
libplatsupport.

libsel4platsupport/include/sel4platsupport/io.h

diff --git a/libsel4platsupport/include/sel4platsupport/io.h b/libsel4platsupport/include/sel4platsupport/io.h
index 9da11b2..f6d4986 100644
--- a/libsel4platsupport/include/sel4platsupport/io.h
+++ b/libsel4platsupport/include/sel4platsupport/io.h
@@ -42,6 +42,16 @@
 int sel4platsupport_new_malloc_ops(ps_malloc_ops_t *ops);
 
 /**
+ * Create a new FDT ops structure using a provided simple
+ *
+ * @param io_fdt Interface to fill in
+ * @param simple An initialised simple interface
+ *
+ * @return returns 0 on success
+ */
+int sel4platsupport_new_fdt_ops(ps_io_fdt_t *io_fdt, simple_t *simple, ps_malloc_ops_t *malloc_ops);
+
+/**
  * Creates a new implementation of the platsupport io_ops interface using a
  * provided vspace and vka
  *

libsel4platsupport/src/io.c

diff --git a/libsel4platsupport/src/io.c b/libsel4platsupport/src/io.c
index 90a78e4..500bf16 100644
--- a/libsel4platsupport/src/io.c
+++ b/libsel4platsupport/src/io.c
@@ -269,6 +269,51 @@
     return 0;
 }
 
+static char *sel4platsupport_io_fdt_get(void *cookie)
+{
+    return cookie != NULL ? (char *) cookie : NULL;
+}
+
+int sel4platsupport_new_fdt_ops(ps_io_fdt_t *io_fdt, simple_t *simple, ps_malloc_ops_t *malloc_ops)
+{
+    if (!io_fdt || !simple || !malloc_ops) {
+        ZF_LOGE("arguments are NULL");
+        return -1;
+    }
+
+    ssize_t block_size = simple_get_extended_bootinfo_length(simple, SEL4_BOOTINFO_HEADER_FDT);
+
+    int error = ps_calloc(malloc_ops, 1, block_size, &io_fdt->cookie);
+    if (error) {
+        ZF_LOGE("Failed to allocate %zu bytes for the FDT", block_size);
+        return -1;
+    }
+
+    /* Copy the FDT from the extended bootinfo */
+    ssize_t copied_size = simple_get_extended_bootinfo(simple, SEL4_BOOTINFO_HEADER_FDT,
+                                                       io_fdt->cookie, block_size);
+    if (copied_size != block_size) {
+        ZF_LOGE("Failed to copy the FDT");
+        ZF_LOGF_IF(ps_free(malloc_ops, block_size, io_fdt->cookie),
+                   "Failed to clean-up after a failed operation!");
+        return -1;
+    }
+
+    /* Cut off the bootinfo header from the start of the buffer */
+    ssize_t fdt_size = block_size - sizeof(seL4_BootInfoHeader);
+    void *fdt_start = io_fdt->cookie + sizeof(seL4_BootInfoHeader);
+    memmove(io_fdt->cookie, fdt_start, fdt_size);
+
+    /* Trim off the extra bytes at the end of the FDT */
+    void *fdt_end = io_fdt->cookie + fdt_size;
+    memset(fdt_end, 0, sizeof(seL4_BootInfoHeader));
+
+    /* Set the function pointer inside the io_fdt interface */
+    io_fdt->get_fn = sel4platsupport_io_fdt_get;
+
+    return 0;
+}
+
 #ifdef CONFIG_PLAT_TK1
 #include <platsupport/gpio.h>