libsel4platsupport: Add IO FDT ops implementation
This commit adds an implementation of the IO FDT ops interface in
libplatsupport.
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
*
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>
