#include <stdio.h>
#include <stdio.h>
#include <sel4/sel4.h>
#include <sel4platsupport/bootinfo.h>
#include <utils/util.h>
#include <sel4runtime.h>

#include <utils/arith.h>
#include <utils/attribute.h>

#include <platsupport/io.h>
#include <platsupport/irq.h>
#include <vka/vka.h>
#include <vspace/vspace.h>

#include <sel4runtime.h>
#include <allocman/bootstrap.h>
#include <allocman/vka.h>
#include <cpio/cpio.h>

#include <sel4debug/register_dump.h>
#include <sel4platsupport/device.h>
#include <sel4platsupport/platsupport.h>
#include <sel4utils/vspace.h>
#include <sel4utils/stack.h>
#include <sel4utils/process.h>

#include <simple/simple.h>
#include <simple-default/simple-default.h>
#include <utils/util.h>
#include <vka/object.h>
#include <vka/capops.h>
#include <vspace/vspace.h>
#include <sel4platsupport/io.h>

//#define DEV_REG_PADDR 0x4011c000
//#define DEV_REG_PADDR 0x40000000
#define DEV_REG_PADDR  0x40520000 //0x40000000
#define DEV_REG_SIZE 0x3000
//#define DEV_REG_SIZE 0x200000

/* ammount of untyped memory to reserve for the driver (32mb) */
#define DRIVER_UNTYPED_MEMORY (1 << 25)
/* Number of untypeds to try and use to allocate the driver memory.
 * if we cannot get 32mb with 16 untypeds then something is probably wrong */
#define DRIVER_NUM_UNTYPEDS 16

/* dimensions of virtual memory for the allocator to use */
#define ALLOCATOR_VIRTUAL_POOL_SIZE ((1 << seL4_PageBits) * 100)

/* static memory for the allocator to bootstrap with */
#define ALLOCATOR_STATIC_POOL_SIZE ((1 << seL4_PageBits) * 20)
static char allocator_mem_pool[ALLOCATOR_STATIC_POOL_SIZE];

/* static memory for virtual memory bootstrapping */
static sel4utils_alloc_data_t data;

/* When the root task exists, it should simply suspend itself */
static void sel4test_exit(int code)
{
    seL4_TCB_Suspend(seL4_CapInitThreadTCB);
}

/* initialise our runtime environment */
static int init_env(void)
{
    /* An initialised vka that may be used by the test. */
    vka_t vka;
    /* virtual memory management interface */
    vspace_t vspace;
    /* abtracts over kernel version and boot environment */
    simple_t simple;
    /* IO ops for devices */
    ps_io_ops_t ops;

    allocman_t *allocman;
    reservation_t virtual_reservation;
    int error;
    void *vaddr=NULL;
    void *va=NULL;

    ZF_LOGE("Enter::init_env()");

    sel4runtime_set_exit(sel4test_exit);
    seL4_BootInfo *info = platsupport_get_bootinfo(); /* get boot info */
    simple_default_init_bootinfo(&simple, info); /* initialize simple object */
    simple_print(&simple); /* print boot info and other info about image */

    seL4_Word untyped_size_bits = seL4_TCBBits + 1;
    seL4_CPtr parent_untyped = 0;
    seL4_CPtr child_untyped = info->empty.start;
    seL4_CPtr child_tcb = child_untyped + 1;
    seL4_CPtr child_ep = child_tcb + 1;

    /*-- filter TaskContent("untyped-start", TaskContentType.ALL, subtask='init') -*/
    printf("    CSlot   \tPaddr           \tSize\tType\n");
    for (seL4_CPtr slot = info->untyped.start; slot != info->untyped.end; slot++) {
        seL4_UntypedDesc *desc = &info->untypedList[slot - info->untyped.start];
#if 0
	if(desc->paddr == 0x40000000)
	{
		printf("Got address for the physical device ...\n");
		parent_untyped = info->untyped.start;
		child_untyped = info->empty.start;
		untyped_size_bits = desc->sizeBits;

		 error = seL4_Untyped_Retype(parent_untyped, // the untyped capability to retype
                                seL4_UntypedObject, // type
                                untyped_size_bits,  //size
                                seL4_CapInitThreadCNode, // root
                                0, // node_index
                                0, // node_depth
                                child_untyped, // node_offset
                                1 // num_caps
                                );
    		ZF_LOGF_IF(error != seL4_NoError, "Failed to create endpoints.");
	}
#endif
        printf("%8p\t%16p\t2^%d\t%s\n", (void *) slot, (void *) desc->paddr, desc->sizeBits, desc->isDevice ? "device untyped" : "untyped");

    }
    /*-- endfilter -*/

    //seL4_Word num_eps = BIT(untyped_size_bits - seL4_EndpointBits);
    //error = seL4_Untyped_Retype(child_untyped, seL4_EndpointObject, 0, seL4_CapInitThreadCNode, 0, 0, child_tcb, num_eps);

    printf("Success\n");

    ZF_LOGE("Calling bootstrap_use_current_simple() ...");
        /* create an allocator */
    allocman = bootstrap_use_current_simple(&simple, ALLOCATOR_STATIC_POOL_SIZE, allocator_mem_pool);
    if (allocman == NULL) {
        ZF_LOGF("Failed to create allocman");
	   return -1;
    }

    ZF_LOGE("bootstrap_use_current_simple() done");
    /* create a vka (interface for interacting with the underlying allocator) */
    allocman_make_vka(&vka, allocman);

    /* create a vspace (virtual memory management interface). We pass
     * boot info not because it will use capabilities from it, but so
     * it knows the address and will add it as a reserved region */
     error = sel4utils_bootstrap_vspace_with_bootinfo_leaky(&vspace,
                                                       &data, simple_get_pd(&simple),
                                                       &vka, platsupport_get_bootinfo());
    if (error) {
        ZF_LOGF("Failed to bootstrap vspace");
	   return -1;
    }
    ZF_LOGE("sel4utils_bootstrap_vspace_with_bootinfo_leaky() done");

    /* fill the allocator with virtual memory */
    virtual_reservation = vspace_reserve_range(&vspace,
                                           ALLOCATOR_VIRTUAL_POOL_SIZE, seL4_AllRights, 1, &vaddr);
    if (virtual_reservation.res == 0) {
        ZF_LOGF("Failed to provide virtual memory for allocator");
	   return -1;
    }
    ZF_LOGE("vspace_reserve_range() done");

    bootstrap_configure_virtual_pool(allocman, vaddr,
                                 ALLOCATOR_VIRTUAL_POOL_SIZE, simple_get_pd(&simple));
 
    ZF_LOGE("bootstrap_configure_virtual_pool() done");
    error = sel4platsupport_new_io_ops(&vspace, &vka, &simple, &ops);
    ZF_LOGF_IF(error, "Failed to initialise IO ops");

    ZF_LOGE("Page size = 0x%x ",(1 << seL4_PageBits));
    va = ps_io_map(&ops.io_mapper, (uintptr_t) DEV_REG_PADDR, DEV_REG_SIZE, 0, PS_MEM_NORMAL);
    if (va == NULL) {
        ZF_LOGE("ps_io_map() failed to map frame");
        return -1;
    }
    ZF_LOGE("Physical address 0x%x mapped to virtual address %p \n",DEV_REG_PADDR,va);

    for(int j=0;j<DEV_REG_SIZE;j+=sizeof(unsigned int))
    {
	ZF_LOGE("Loc %d --> 0x%x ",j,*(unsigned int*)(va+j));
    }
    //ZF_LOGE("1 Data read before write = 0x%x \n",*(unsigned int*)va);
    //*(unsigned int*)(va+i) = 1;
    //ZF_LOGE("2. Data read after write = 0x%x \n",*(unsigned int *)va);

    ZF_LOGE("Exit::init_env()");
    return 0;
}

int main(void)
{
    printf("\n $$$ Hello World! $$$ \n\n");
    
    init_env();

#ifdef CONFIG_DEBUG_BUILD
    seL4_DebugNameThread(seL4_CapInitThreadTCB, "hello-world");
#endif
    
    return 0;
}