#include "test_TA_phys_dev.h"
#include <tee_internal_api.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <core/error.h>
#include <core/access.h>
#include <core/page.h>
#include <driver/mem/phys.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <inttypes.h>
#include <fcntl.h>
#include <cache.h>

#define _ROUND_MASK(a)      ((a) - 1)
#define BLK_OFFSET(x, a)    ((x) & _ROUND_MASK(a))
#define PAGE_OFFSET(x)      BLK_OFFSET((unsigned long)(x), PAGE_SIZE)
#define ROUND_PGUP_DIV(x)   (((x) >> PAGE_SHIFT) + !!PAGE_OFFSET(x))
#define NUM_PAGES(size)     ROUND_PGUP_DIV(size)

#if !defined(PHYS_DEV_NAME)
#define PHYS_DEV_NAME       "/dev/phys"
#endif

TEE_Result TA_CreateEntryPoint(void)
{
    return TEE_SUCCESS;
}

void TA_DestroyEntryPoint(void)
{
}

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) commandID;
    (void) paramTypes;

    if(paramTypes != TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT,
                                            TEE_PARAM_TYPE_MEMREF_INPUT,
                                            TEE_PARAM_TYPE_MEMREF_INPUT,
                                            TEE_PARAM_TYPE_NONE) ){
        printf("%s: Bad parameters\n", __func__);
        return TEE_ERROR_BAD_PARAMETERS;
    }

    int phys;
    uint64_t paddr;

    phys = open(PHYS_DEV_NAME, O_RDWR);
    if(phys == -1){
        printf("PHYS_DEV : failed to open phys driver. errno: %d\n", errno);
        return TEE_ERROR_GENERIC;
    }
    paddr = *((uint64_t *)params[0].memref.buffer);
    uint64_t offset = paddr & ~PAGE_MASK;
    char* data = (char *) params[1].memref.buffer;
    int size = (int) params[1].memref.size;
    char* ccheck = (char *) params[2].memref.buffer;
    int csize = (int) params[2].memref.size;
    int numpages = NUM_PAGES(offset+size);

    printf("PHYS_DEV : paddr : %llx, data : %s, size : %d, numpages : %d\n", (long long)paddr, data, size, numpages);

    char* addr = mmap(NULL, PAGE_SIZE * numpages, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_PHYS_NON_SECURE, phys, paddr & PAGE_MASK);
    if(addr == MAP_FAILED){
        printf("PHYS_DEV : failed to mmap phys memory. errno: %d\n", errno);
        close(phys);
        return TEE_ERROR_GENERIC;
    }
    char* vaddr = addr + offset;

    /* check the memory allcated from CA whether it is continuous or not
     */
    if( size != csize || memcmp(vaddr, ccheck, csize) != 0){
        printf("PHYS_DEV : size isn't equal to csize or the memory allocated by malloc in CA is not continuous\n");
        printf("PHYS_DEV : vaddr : %s   ccheck : %s\n", vaddr, ccheck);
        printf("PHYS_DEV : size : %d   csize : %d\n", size, csize);
        close(phys);
        return TEE_ERROR_GENERIC;
    }

    printf("PHYS_DEV : before memcpy(), vaddr : %s\n", vaddr);
    memcpy(vaddr, data, size);
    printf("PHYS_DEV : after memcpy(), vaddr : %s\n", vaddr);
    if(TEES_DcacheClean(vaddr, size) != 0){
        printf("TEES_DcacheClean is failed\n");
        close(phys);
        return TEE_ERROR_GENERIC;
    }
    printf("PHYS_DEV : after DcacheClean vaddr : %s\n", vaddr);

    if( munmap(addr, PAGE_SIZE * numpages) ){
        printf("DRAM mmap() error, offset = 0x%llx\n", (long long)paddr);
        close(phys);
        return TEE_ERROR_GENERIC;
    }
    close(phys);

    return TEE_SUCCESS;
}
