/*
 *
 * Copyright (C) 2012-2019, Samsung Electronics Co., Ltd.
 *
 * RMI4 Bus level functions
 */

#include "rmi.h"

rmi_function_container_t *rmi_bus_get_function(rmi_device_t *rmi_dev, int id)
{
    int i = -1;
    rmi_function_container_t *entry;

    for (i = 0; i < RMI_FUNCTIONS_COUNT; i++) {
        entry = rmi_dev->rmi_functions[i];
        if (entry && entry->func == id)
            return entry;
    }
    return NULL;
}

int rmi_bus_register_function(rmi_device_t *rmi_dev, rmi_function_container_t *fc, rmi_function_descriptor_t *fd)
{
    int i;
    rmi_function_container_t *entry;

    entry = rmi_bus_get_function(rmi_dev, fc->func);
    if (entry) {
        RMI_ERR("Function f%02x already registered!", fc->func);
        return ERROR_RMI_INVAL;
    }
    entry = TEE_Malloc(sizeof(rmi_function_container_t), 0);
    if (!entry)
        return ERROR_RMI_NOMEM;

    entry->func = fc->func;
    entry->rmi_dev = rmi_dev;
    entry->init = fc->init;
    entry->attention = fc->attention;
    entry->remove = fc->remove;
    entry->data = NULL;
    TEE_MemMove(&entry->fd, fd, sizeof(rmi_function_descriptor_t));

    for (i = 0; i < RMI_FUNCTIONS_COUNT; i++) {
        if (!rmi_dev->rmi_functions[i]) {
            rmi_dev->rmi_functions[i] = entry;
            if (entry->init)
                entry->init(entry);

            return 0;
        }
    }
    TEE_Free(entry);
    return ERROR_RMI_FAILED;
}

int rmi_bus_unregister_function(rmi_device_t *rmi_dev, rmi_function_container_t *fc)
{
    int i = -1;
    int id;
    rmi_function_container_t *entry;

    id = fc->func;
    entry = rmi_bus_get_function(rmi_dev, id);
    if (entry) {
        if (entry->remove)
            entry->remove(entry);

        TEE_Free(entry);
        for (i = 0; i < RMI_FUNCTIONS_COUNT; i++) {
            entry = rmi_dev->rmi_functions[i];
            if (entry && entry->func == id) {
                rmi_dev->rmi_functions[i] = NULL;
                break;
            }
        }
        return 0;
    }
    return ERROR_RMI_INVAL;
}

void rmi_bus_unregister_all(rmi_device_t *rmi_dev)
{
    int i;
    rmi_function_container_t *entry;

    for (i = 0; i < RMI_FUNCTIONS_COUNT; i++) {
        entry = rmi_dev->rmi_functions[i];
        if (entry)
            rmi_bus_unregister_function(rmi_dev, entry);
    }
}