libsel4camkes/src/io.c

/*
 * Copyright 2017, Data61
 * Commonwealth Scientific and Industrial Research Organisation (CSIRO)
 * ABN 41 687 119 230.
 *
 * This software may be distributed and modified according to the terms of
 * the BSD 2-Clause license. Note that NO WARRANTY is provided.
 * See "LICENSE_BSD2.txt" for details.
 *
 * @TAG(DATA61_BSD)
 */

/* IO port/device functionality. This is meant for interaction with
 * libplatsupport infrastructure.
 */

#include <assert.h>
#include <libfdt.h>
#include <camkes/dataport.h>
#include <camkes/dma.h>
#include <camkes/io.h>
#include <camkes/interface_registration.h>
#include <camkes/irq.h>
#include <camkes/arch/io.h>
#include <platsupport/io.h>
#include <platsupport/driver_module.h>
#include <stdint.h>
#include <stdlib.h>
#include <utils/util.h>

/* Force the _dataport_frames  section to be created even if no modules are defined. */
static USED SECTION("_dataport_frames") struct {} dummy_dataport_frame;
/* Definitions so that we can find the exposed dataport frames */
extern dataport_frame_t __start__dataport_frames[];
extern dataport_frame_t __stop__dataport_frames[];

typedef struct camkes_defer_token {
    char *device_path;
    ps_driver_init_fn_t init_func;
} camkes_defer_token_t;

/* Basic linked-list implementation. */
typedef struct ll_ {
    void *data;
    struct ll_ *next;
} ll_t;

#ifndef NDEBUG
bool malloc_ops_initialised = false;
static ps_malloc_ops_t io_mapper_malloc_ops = {0};
#endif

static UNUSED int ll_prepend(ps_malloc_ops_t *malloc_ops, ll_t **list, const void *data)
{
    ll_t *node = NULL;
    int error = ps_calloc(malloc_ops, 1, sizeof * node, (void **) &node);
    if (error) {
        return -1;
    }
    node->data = (void *)data;
    node->next = *list;
    *list = node;
    return 0;
}

static UNUSED int ll_append(ps_malloc_ops_t *malloc_ops, ll_t **list, const void *data)
{
    ll_t *node = NULL;
    int error = ps_calloc(malloc_ops, 1, sizeof * node, (void **) &node);
    if (error) {
        return -1;
    }
    node->data = (void *)data;
    node->next = NULL;
    if (*list == NULL) {
        *list = node;
        return 0;
    }
    ll_t *curr = NULL;
    for (curr = *list; curr->next != NULL; curr = curr->next);
    curr->next = node;
    return 0;
}

static UNUSED int ll_remove(ps_malloc_ops_t *malloc_ops, ll_t **list, const void *data)
{
    for (ll_t **l = list; *l != NULL; l = &(*l)->next) {
        if ((*l)->data == data) {
            /* found it */
            ll_t *temp = *l;
            *l = (*l)->next;
            ps_free(malloc_ops, sizeof(*temp), temp);
            return 0;
        }
    }
    return -1;
}

typedef struct {
    ps_malloc_ops_t *malloc_ops;
    ps_io_map_fn_t map;
    ll_t *mapped;
} cookie_t;

/* Debug wrapper for IO map. This function calls the underlying map function
 * and tracks results for the purpose of catching illegal unmapping operations.
 * Note that this function is unused when NDEBUG is defined.
 */
static UNUSED void *io_map(void *cookie, uintptr_t paddr, size_t size,
                           int cached, ps_mem_flags_t flags)
{

    /* Call the real IO map function. */
    cookie_t *c = cookie;
    void *p = c->map(NULL, paddr, size, cached, flags);

    if (p != NULL) {
        /* The IO map function gave us a successful result; track this pointer
         * to lookup during unmapping.
         */
        if (ll_prepend(c->malloc_ops, &c->mapped, p) != 0) {
            LOG_ERROR("failed to track mapped IO pointer %p\n", p);
        }
    }

    return p;
}

static int UNUSED pointer_compare(void *a, void *b)
{
    uintptr_t p = (uintptr_t)a;
    uintptr_t q = (uintptr_t)b;
    if (p > q) {
        return 1;
    } else if (p < q) {
        return -1;
    } else {
        return 0;
    }
}

static void *camkes_io_map(void *cookie UNUSED, uintptr_t paddr,
                           size_t size, int cached UNUSED, ps_mem_flags_t flags UNUSED)
{
    if (paddr % PAGE_SIZE_4K != 0 && size % PAGE_SIZE_4K != 0) {
        ZF_LOGE("paddr or size has incorrect alignment: (%p, 0x%zx)", (void *) paddr, size);
        return NULL;
    }

    /* Given a base paddr and size, we try to find a region of mapped memory that
     * is a superset of the given parameters. */
    size_t size_counter = 0;
    bool counting_frames = false;
    uintptr_t base_vaddr = 0;
    for (dataport_frame_t *frame = __start__dataport_frames;
         frame < __stop__dataport_frames; frame++) {
        if (counting_frames) {
            if (paddr == (frame->paddr - size_counter)) {
                size_counter += frame->size;
            } else {
                /* We've encountered a different region of physical memory that does
                   not match what we want, reset the counters */
                counting_frames = false;
                base_vaddr = 0;
                size_counter = 0;
            }
        } else {
            if (paddr >= frame->paddr && (frame->paddr + frame->size) > paddr) {
                /* We've found the first frame of the mapped region,
                   start counting from here */
                counting_frames = true;
                base_vaddr = frame->vaddr + (paddr - frame->paddr);
                size_counter += (frame->vaddr + frame->size) - base_vaddr;
            }
        }

        if (size_counter >= size) {
            /* We've found all the frames that cover the desired region */
            return (void *)base_vaddr;
        }
    }

    /* Not found. */
    return NULL;
}

/* We never unmap anything. */
static void io_unmap(void *cookie UNUSED, void *vaddr UNUSED, size_t size UNUSED)
{
#ifndef NDEBUG
    cookie_t *c = cookie;
    /* Make sure we previously mapped the pointer the caller gave us. */
    if (ll_remove(c->malloc_ops, &c->mapped, vaddr) != 0) {
        LOG_ERROR("unmapping an IO pointer that was not previously mapped: %p\n",
                  vaddr);
    }
#endif
}

int camkes_io_mapper(ps_io_mapper_t *mapper)
{
    if (mapper == NULL) {
        ZF_LOGE("mapper is NULL");
        return -1;
    }
#ifdef NDEBUG
    mapper->cookie = NULL;
    mapper->io_map_fn = camkes_io_map;
#else
    if (!malloc_ops_initialised) {
        ZF_LOGF_IF(camkes_ps_malloc_ops(&io_mapper_malloc_ops),
                   "Failed to get malloc_ops for DEBUG mode io mapper");
        malloc_ops_initialised = true;
    }
    cookie_t *c = NULL;
    int error = ps_calloc(&io_mapper_malloc_ops, 1, sizeof(*c), (void **) &c);
    if (error) {
        return -1;
    }
    c->malloc_ops = &io_mapper_malloc_ops;
    c->map = camkes_io_map;
    c->mapped = NULL;
    mapper->cookie = c;
    mapper->io_map_fn = io_map;
#endif
    mapper->io_unmap_fn = io_unmap;
    return 0;
}

static int camkes_io_port_in(void *cookie UNUSED, uint32_t port, int io_size, uint32_t *result)
{
    return camkes_arch_io_port_in(port, io_size, result);
}

static int camkes_io_port_out(void *cookie UNUSED, uint32_t port, int io_size, uint32_t val)
{
    return camkes_arch_io_port_out(port, io_size, val);
}

int camkes_io_port_ops(ps_io_port_ops_t *ops)
{
    if (ops == NULL) {
        ZF_LOGE("ops is NULL");
        return -1;
    }
    ops->io_port_in_fn = camkes_io_port_in;
    ops->io_port_out_fn = camkes_io_port_out;
    return 0;
}

int camkes_ps_malloc_ops(ps_malloc_ops_t *ops)
{
    if (ops == NULL) {
        ZF_LOGE("ops is NULL");
        return -1;
    }

    int ret = ps_new_stdlib_malloc_ops(ops);
    if (ret) {
        return ret;
    }

#ifndef NDEBUG
    /* This works as malloc_ops contains pointers */
    malloc_ops_initialised = true;
    io_mapper_malloc_ops = (ps_malloc_ops_t) * ops;
#endif

    return 0;
}

static char *camkes_io_fdt_get(void *cookie)
{
    return (char *)(cookie ? cookie : NULL);
}

int camkes_io_fdt(ps_io_fdt_t *io_fdt)
{
    if (io_fdt == NULL) {
        ZF_LOGE("io_fdt is NULL");
        return -1;
    }

    extern char *dtb_symbol WEAK;

    if (!&dtb_symbol) {
        io_fdt->cookie = NULL;
    } else {
        /* the buffer contains the bootinfo header, so we skip it */
        io_fdt->cookie = (void *) &dtb_symbol + sizeof(seL4_BootInfoHeader);
    }

    io_fdt->get_fn = camkes_io_fdt_get;

    return 0;
}

/* Force the _driver_modules section to be created even if no modules are defined. */
static USED SECTION("_driver_modules") struct {} dummy_driver_module;
/* Definitions so that we can find the list of driver modules that can be initialised */
extern ps_driver_module_t *__start__driver_modules[];
extern ps_driver_module_t *__stop__driver_modules[];

static ll_t *driver_defer_list = NULL;

static int defer_driver_init(ps_io_ops_t *ops, char *device_path, ps_driver_init_fn_t init_func)
{
    if (ops == NULL) {
        ZF_LOGE("ops is NULL");
        return -1;
    }

    if (device_path == NULL) {
        ZF_LOGE("device path is NULL");
        return -1;
    }

    if (init_func == NULL) {
        ZF_LOGE("init_func is NULL");
        return -1;
    }

    camkes_defer_token_t *defer_token = NULL;
    int error = ps_calloc(&ops->malloc_ops, 1, sizeof(*defer_token), (void **) &defer_token);
    if (error) {
        ZF_LOGE("Failed to allocate memory for a bookkeeping structure for the driver defer list");
        return -1;
    }

    defer_token->device_path = device_path;
    defer_token->init_func = init_func;

    error = ll_append(&ops->malloc_ops, &driver_defer_list, defer_token);
    if (error) {
        ZF_LOGE("Failed to add the driver init function to the defer list");
    }

    return 0;
}

static int find_compatible_driver_module(ps_io_ops_t *ops, int node_offset, char *device_path)
{
    if (ops == NULL) {
        ZF_LOGF("ops is NULL");
    }

    char *dtb_blob = ps_io_fdt_get(&ops->io_fdt);
    if (!dtb_blob) {
        ZF_LOGF("No DTB supplied!");
    }

    /* Look through the list of hardware modules that are registered and
     * pick the most suitable one by comparing the module's compatible
     * strings against the device node's */
    for (ps_driver_module_t **module = __start__driver_modules; module < __stop__driver_modules; module++) {
        for (const char **curr_str = (*module)->compatible_list; *curr_str != NULL; curr_str++) {
            int match_ret = fdt_node_check_compatible(dtb_blob, node_offset, *curr_str);
            if (match_ret == 0) {
                /* Found a match! */
                int ret = (*module)->init(ops, device_path);
                if (ret == PS_DRIVER_INIT_DEFER) {
                    ZF_LOGF_IF(defer_driver_init(ops, device_path, (*module)->init),
                               "Failed to defer the initialisation of node %s", device_path);
                    return 0;
                } else if (ret < 0) {
                    ZF_LOGE("Node pointed to by path %s failed to have a driver initialise properly, ignoring", device_path);
                    return -1;
                } else if (ret == PS_DRIVER_INIT_SUCCESS) {
                    return 0;
                } else {
                    ZF_LOGE("Initialisation function for node pointed to by path %s returned an unexpected code", device_path);
                    return -1;
                }
            } else if (match_ret < 0) {
                ZF_LOGE("Node pointed to by path %s is malformed, ignoring", device_path);
                return -1;
            }
            /* Ignore the no match case */
        }
    }

    /* No suitable module was found for this device node */
    ZF_LOGE("No suitable driver was found for path %s, ignoring", device_path);
    return 0;
}

static int handle_defered_modules(ps_io_ops_t *ops)
{
    if (ops == NULL) {
        ZF_LOGF("ops is NULL");
    }

    /* The set of modules that have deferred and that we need to work through */
    ll_t *working_set = driver_defer_list;
    /* The set of modules that have deferred again */
    ll_t *deferred_set = NULL;
    ll_t *deferred_tail = NULL;

    while (working_set != NULL) {
        ll_t *curr = working_set;
        while (curr != NULL) {
            camkes_defer_token_t *defer_token = curr->data;
            int ret = defer_token->init_func(ops, defer_token->device_path);
            if (ret == PS_DRIVER_INIT_DEFER) {
                /* Throw this into deferred set and also remove it from the
                 * working set */
                if (deferred_tail != NULL) {
                    deferred_tail->next = curr;
                }
                if (deferred_set == NULL) {
                    deferred_set = curr;
                }
                deferred_tail = curr;
                curr = curr->next;
                deferred_tail->next = NULL;
            } else {
                /* Diagnostics */
                if (ret < 0) {
                    ZF_LOGE("Node pointed to by path %s failed to have a driver initialise properly, ignoring",
                            defer_token->device_path);
                } else if (ret != PS_DRIVER_INIT_SUCCESS) {
                    ZF_LOGE("Initialisation function for node pointed to by path %s returned an unexpected code",
                            defer_token->device_path);
                }
                /* Remove this from the working set and deallocate memory */
                ll_t *temp = curr;
                curr = curr->next;
                ZF_LOGF_IF(ps_free(&ops->malloc_ops, sizeof(*defer_token), defer_token),
                           "Failed to deallocate a camkes_defer_token_t instance");
                ZF_LOGF_IF(ps_free(&ops->malloc_ops, sizeof(*temp), temp), "Failed to deallocate a ll_t node");
            }
        }
        /* Swap the two sets and continue on */
        working_set = deferred_set;
        deferred_set = NULL;
        deferred_tail = NULL;
    }

    return 0;
}

/* Force the _hardware_init section to be created even if no modules are defined. */
static USED SECTION("_hardware_init") struct {} dummy_init_module;
/* Definitions so that we can find the list of hardware modules that we need to initialise */
extern char **__start__hardware_init[];
extern char **__stop__hardware_init[];

int camkes_call_hardware_init_modules(ps_io_ops_t *ops)
{
    if (__start__hardware_init == __stop__hardware_init) {
        /* Exit early if there are no modules to initialise */
        return 0;
    }

    if (ops == NULL) {
        ZF_LOGE("ops is NULL");
        return -1;
    }

    char *dtb_blob = ps_io_fdt_get(&ops->io_fdt);
    if (!dtb_blob) {
        ZF_LOGE("No DTB supplied!");
        return -1;
    }

    for (char ***curr_path = __start__hardware_init; curr_path < __stop__hardware_init; curr_path++) {
        int node_offset = fdt_path_offset(dtb_blob, **curr_path);
        if (node_offset < 0) {
            ZF_LOGE("Path %s doesn't seem to be in the DTB, ignoring", **curr_path);
            continue;
        }

        /* Quick sanity check to see if the node has a compatible property */
        const void *dummy = fdt_getprop(dtb_blob, node_offset, "compatible", NULL);
        if (dummy == NULL) {
            ZF_LOGE("Node pointed to by path %s doesn't have a compatible string or is malformed, ignoring", **curr_path);
            continue;
        }

        /* Currently most of the errors in this function as they aren't fatal */
        find_compatible_driver_module(ops, node_offset, **curr_path);
    }

    /* Now take care of the deferred modules, again ignore most of the errors
     * in this function as they aren't fatal */
    handle_defered_modules(ops);

    return 0;
}

int camkes_io_ops(ps_io_ops_t *ops)
{
    if (ops == NULL) {
        ZF_LOGE("ops is NULL");
        return -1;
    }

    int ret = camkes_ps_malloc_ops(&ops->malloc_ops);
    if (ret) {
        return ret;
    }

    ret = camkes_io_mapper(&ops->io_mapper) ||
          camkes_io_port_ops(&ops->io_port_ops) ||
          camkes_dma_manager(&ops->dma_manager) ||
          camkes_io_fdt(&ops->io_fdt) ||
          camkes_irq_ops(&ops->irq_ops) ||
          camkes_interface_registration_ops(&ops->interface_registration_ops, &ops->malloc_ops);

    return ret;
}