#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <tee_client_api.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <stdint.h>


TEEC_UUID uuid = {
    .timeLow = 0x00000000,
    .timeMid = 0,
    .timeHiAndVersion = 0,
    .clockSeqAndNode = {0, 0, 0x70, 0x68, 0x79, 0x73, 0x64, 0x65},
};

 /* Mask to extract physical page number from pagemap */
#define PHYS_DEV_PFN_ITEM_MASK (0xFFFFFFFFFFFFFFFFULL >> (64 - 54))
/* Page is present in memory */
#define PHYS_DEV_PFN_ITEM_PRESENT (1ULL << 63)
/* log_2(PAGE_SIZE) */
#define PHYS_DEV_PAGE_SHIFT (12)

static const char pagemapfile[] = "/proc/self/pagemap";
uint64_t ca_virt_to_phys(uint64_t vaddr)
{
    uint64_t paddr = 0;
    uint64_t offset;
    uint64_t ret;
    int f;
    off64_t file_offset, got_offset;
    unsigned long long item;
    
    if (!vaddr) {
        return 0;
    }
    
    f = open(pagemapfile, O_RDONLY);
    if (f < 0) {
        printf("Unable to open %s for reading. errno = %d\n", pagemapfile, errno);
        return 0;
    }
    file_offset = (off64_t)((vaddr >> PHYS_DEV_PAGE_SHIFT) * sizeof(item));
    got_offset = lseek64(f, file_offset, SEEK_SET);
    if (got_offset != file_offset) {
        printf("Error seeking to %ld in file %s. errno = %d\n",
                (unsigned long)file_offset,
                pagemapfile,
                errno);
        goto out;
    }
    ret = read(f, &item, sizeof(item));
    if (ret != sizeof(unsigned long long)) {
        printf("Error reading file %s. errno = %d\n", pagemapfile, errno);
        goto out;
    }
    
    if (item & PHYS_DEV_PFN_ITEM_PRESENT) {
        paddr = (uint64_t)((item & PHYS_DEV_PFN_ITEM_MASK) << PHYS_DEV_PAGE_SHIFT);
        offset = (vaddr & ~PAGE_MASK);
        paddr |= offset;
    }

out:
    close(f);
    return paddr;
}

int main()
{
    TEEC_Context context;
    TEEC_Session session;
    TEEC_Operation operation;
    uint32_t returnOrigin;
    
    TEEC_Result res = TEEC_InitializeContext(NULL, &context);
    if (res != TEEC_SUCCESS) {
        fprintf(stderr,"InitializeContext failed: %x\n", res);
        goto out;
    }
    
    res = TEEC_OpenSession(&context, &session, &uuid, 0, NULL, NULL, \
                           &returnOrigin);
    if (res != TEEC_SUCCESS) {
        fprintf(stderr,"TEEC_OpenSession returned %x from %x\n", res,
                returnOrigin);
        goto finalize;
    }
    
    char data[17] = "phys_dev test TA";
    int size = strlen(data);
    char* addr = malloc( size );
    if(!addr){
        printf("Failed to allocate memory (addr)...\n");
        goto finalize;
    }
    off64_t paddr = (off64_t) ca_virt_to_phys((unsigned long) addr);
    if( paddr == 0 ){
        fprintf(stderr, "user_virt_to_phys() function doesn't work...\n");
        goto free_addr;
    }
    char* ccheck = malloc( size );
    if(!ccheck){
        printf("Failed to allocate memory (ccheck)...\n");
        goto free_addr;
    }
    int i;
    for(i = 0; i < size; i++){
        ccheck[i] = rand()%10 + '0';
    }
    memcpy(addr, ccheck, size);
#if defined(__arm__)
    cacheflush((long)addr, (long)addr+size, 0);
#else
    __builtin___clear_cache ((void*)addr, (void*)addr+size);
#endif

    res = TEEC_OpenSession(&context, &session, &uuid, 0, NULL, NULL, \
                           &returnOrigin);
    if (res != TEEC_SUCCESS) {
        fprintf(stderr,"TEEC_OpenSession returned %x from %x\n", res,
                returnOrigin);
        goto free_ccheck;
    }
    /*
        params[0] : physical address to be mapped
        params[1] : data
        params[2] : check data for allocated physical memory to be continuous
    */
    operation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_INPUT,
                                            TEEC_MEMREF_TEMP_INPUT,
                                            TEEC_MEMREF_TEMP_INPUT,
                                            TEEC_NONE);
    operation.params[0].tmpref.buffer = &paddr;
    operation.params[0].tmpref.size = sizeof(paddr);
    operation.params[1].tmpref.buffer = &data;
    operation.params[1].tmpref.size = size;
    operation.params[2].tmpref.buffer = ccheck;
    operation.params[2].tmpref.size = size;

    res = TEEC_InvokeCommand(&session, 0x0, &operation, &returnOrigin);
    if (res != TEEC_SUCCESS) {
        fprintf(stderr,"TEEC_InvokeCommand returned %x from %x\n", res,
                returnOrigin);
        goto close_session;
    }
    
#if defined(__arm__)
    cacheflush((long)addr, (long)addr+size, 0);
#else
    __builtin___clear_cache ((void*)addr, (void*)addr+size);
#endif

    if( memcmp(data, addr, size) == 0 ){
        printf("PASS\n");
    }
    else{
        printf("Fail\n");
        printf("addr : %s\n", addr);
        res = TEEC_ERROR_GENERIC;
    }

close_session:
    TEEC_CloseSession(&session);
free_ccheck:
    free(ccheck);
free_addr:
    free(addr);
finalize:
    TEEC_FinalizeContext(&context);
out:
    return (res != TEEC_SUCCESS);
}
