#include "test_TA_driver.h"

#include <tee_internal_api.h>
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <driver.h>
#include <string.h>
#include <core/page.h>

#ifdef DEBUG
#include <errno.h>
#define LOG_PREFIX                      "DRIVER_DBG: "
#define print_info(format, args...)     printf(LOG_PREFIX format, ##args)
#else
#define print_info(format, args...)
#endif

#define _ROUND_PGDOWN(n)         (((uintptr_t)(n)) & PAGE_MASK)
#define ROUND_PGUP(n)            (_ROUND_PGDOWN((n) + (PAGE_SIZE - 1)))
#define ROUND_PGDOWN(n)          (_ROUND_PGDOWN(n))

#define DEV_BUF_SZ 256

enum cmd_ioctl
{
    CMD_IOCTL_NORMAL,
    CMD_IOCTL_PROCESS_DATA,
    CMD_IOCTL_COMPLEX_DATA,
    CMD_IOCTL_MAX
};

struct ref_array {
    char *str;
    uint64_t val;
} __attribute__((packed));

struct ref_data {
    uint32_t val;
    char *buf;
    uint32_t cnt0;
    uint32_t cnt1;
    struct ref_array arr0[3];
    uint32_t arr1[3];
} __attribute__((packed));

static struct usr_drv_info *drv_info = NULL;

static char drv_name[] = "my_driver";

struct my_dev {
    char buf[DEV_BUF_SZ];
};

struct my_dev dev;

static int drv_open(struct drv_info *info, const char *drv_path, ...)
{
    (void) info;
    (void) drv_path;
    int ret = 0;

    print_info("drv_open\n");

    strncpy(dev.buf, "Driver initial data", DEV_BUF_SZ);

    print_info("drv_open success\n");

    return ret;
}

static int drv_close(struct drv_info *info)
{
    (void) info;
    int ret = 0;

    print_info("drv_close()\n");
    print_info("drv_close() exit\n");

    return ret;
}

static int drv_ioctl(struct drv_info *info, int ioctl_cmd, unsigned long ioctl_data)
{
    (void) info;
    int ret = 0;
    unsigned int i;
    struct ref_data *data;
    char *str;

    print_info("drv_ioctl()\n");

    switch (ioctl_cmd) {
        case CMD_IOCTL_NORMAL:
            /* Do nothing */
            print_info("ioctl() cmd '0', do nothing... \n");
            break;
        case CMD_IOCTL_PROCESS_DATA:
            str = (char *)ioctl_data;
            print_info("data len: %zu, data buf: %s\n", strlen(str), str);
            TEE_MemMove(str, "World!!!", 8);
            break;
        case CMD_IOCTL_COMPLEX_DATA:
            data = (struct ref_data *)ioctl_data;
            print_info("val: %#x, buf: %s[%p], cnt0: %d, cnt1: %d\n",
                    data->val, data->buf, data->buf, data->cnt0, data->cnt1);
            for (i = 0; i < data->cnt0; i++) {
#if defined(__arm__)
                print_info("ref_array[%d]: {%s, %#llx}\n", i, data->arr0[i].str, data->arr0[i].val);
#elif defined(__aarch64__)
#else
                print_info("ref_array[%d]: {%s, %#lx}\n", i, data->arr0[i].str, data->arr0[i].val);
#error "Unsupported architecture"
#endif
            }
            for (i = 0; i < data->cnt1; i++) {
                print_info("array[%d]: {%#x}\n", i, data->arr1[i]);
            }

            data->val |= 0xF0000000;
            for (i = 0; i < data->cnt0; i++) {
                data->arr0[i].val |= 0xF0000000;
            }
            for (i = 0; i < data->cnt1; i++) {
                data->arr1[i] |= 0xF0000000;
            }
            TEE_MemMove(data->buf, "World!!!", 8);
            break;
        default:
            return -EINVAL;
    }
    print_info("drv_ioctl() exit\n");

    return ret;
}

static ssize_t drv_read(struct drv_info *info, void *data, size_t data_len)
{
    (void) info;

    print_info("drv_read()\n");
    if (data_len > DEV_BUF_SZ)
        data_len = DEV_BUF_SZ;

    TEE_MemMove(data, dev.buf, data_len);
    print_info("drv_read() exit\n");

    return data_len;
}

static ssize_t drv_write(struct drv_info *info, void *data, size_t data_len)
{
    (void) info;

    print_info("drv_write()\n");
    if (data_len > DEV_BUF_SZ)
        data_len = DEV_BUF_SZ;

    TEE_MemMove(dev.buf, data, data_len);
    print_info("drv_write() received buf: '%s'\n", dev.buf);
    print_info("drv_write() exit\n");

    return data_len;
}


static struct fops file_ops;

TEE_Result TA_CreateEntryPoint(void)
{
    int ret = 0;
    struct ioctl_desc tpl;

    file_ops.open = drv_open;
    file_ops.close = drv_close;
    file_ops.read = drv_read;
    file_ops.write = drv_write;
    file_ops.ioctl  = drv_ioctl;

    ret = TEES_InitDriver(drv_name, &file_ops, ACC_PERM_SEC_DRV, &drv_info);
    if (ret) {
        print_info("TEES_InitDriver failed, ret = %d (%d)\n", ret, errno);
        return TEE_ERROR_GENERIC;
    }

    tpl = (struct ioctl_desc){
        .cnt = 9,
        .type = IOCTL_DESC_STRUCT,
        .tpl = { {.type = DESC_ATOM_VAL, .len = sizeof(uint32_t) },
                 {.type = DESC_ATOM_REF_RW, .len = 10 },
                 {.type = DESC_ATOM_ARR_CNT_0, .len = sizeof(uint32_t) },
                 {.type = DESC_ATOM_ARR_CNT_1, .len = sizeof(uint32_t) },
                 {.type = DESC_ATOM_ARR_ITEM_0, .len = 2 },
                 {.type = DESC_ATOM_REF_RW, .len = 5 },
                 {.type = DESC_ATOM_VAL, .len = sizeof(uint64_t) },
                 {.type = DESC_ATOM_ARR_ITEM_1, .len = 1 },
                 {.type = DESC_ATOM_VAL, .len = sizeof(uint32_t) } },
    };

    ret = TEES_RegisterIoctlDesc(drv_info, CMD_IOCTL_COMPLEX_DATA, &tpl);
    if (ret) {
        print_info("TEES_RegisterIoctlDesc failed at %d, ret = %d (%d)\n", __LINE__, ret, errno);
        return TEE_ERROR_GENERIC;
    }

    tpl = (struct ioctl_desc){
        .cnt = 1,
        .type = IOCTL_DESC_REF,
        .tpl = { {.type = DESC_ATOM_REF_RW, .len = 10 } },
    };

    ret = TEES_RegisterIoctlDesc(drv_info, CMD_IOCTL_PROCESS_DATA, &tpl);
    if (ret) {
        print_info("TEES_RegisterIoctlDesc failed at %d, ret = %d (%d)\n", __LINE__, ret, errno);
        return TEE_ERROR_GENERIC;
    }
    print_info("Driver registered\n");

    return TEE_SUCCESS;
}

void TA_DestroyEntryPoint(void)
{
    int ret = 0;

    ret = TEES_FiniDriver(drv_info);
    if (ret) {
        print_info("TEES_FiniDriver failed, ret = %d (%d)\n'", ret, errno);
    }
    print_info("Driver released\n");
}

TEE_Result TA_OpenSessionEntryPoint(uint32_t paramTypes,
    TEE_Param params[4], void **sessionContext)
{
    (void) paramTypes;
    (void) params;
    (void) sessionContext;

    return TEE_SUCCESS;
}

void TA_CloseSessionEntryPoint(void *sessionContext)
{
    (void) sessionContext;
}

TEE_Result TA_InvokeCommandEntryPoint(void *sessionContext,
    uint32_t commandID, uint32_t paramTypes, TEE_Param params[4])
{
    (void) sessionContext;
    (void) params;
    (void) paramTypes;
    (void) commandID;

    return TEE_SUCCESS;
}
