#define _PCIDEV_DMA_C
#define _BSD_SOURCE
#define _DEFAULT_SOURCE
#define _POSIX_C_SOURCE 199309L

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sched.h>
#include <time.h>
#include <sys/time.h>
#include <arpa/inet.h>

#include <pcilib.h>
#include <pcilib/kmem.h>
#include <pcilib/error.h>
#include <pcilib/debug.h>

#include "dma.h"
#include "dma_private.h"

    // We will use get_block_ba for real bus-mapped buffers
#define pcilib_kmem_get_block_addr pcilib_kmem_get_block_pa


pcilib_dma_context_t *pcidev_dma_init(pcilib_t *pcilib, const char *model, const void *arg) {
    pcilib_register_value_t version_value;

    pcidev_dma_t *ctx = malloc(sizeof(pcidev_dma_t));

    if (ctx) {
	memset(ctx, 0, sizeof(pcidev_dma_t));
	ctx->dmactx.pcilib = pcilib;

	if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_timeout", &version_value))
	    ctx->version = version_value;
	else
	    ctx->version = PCILIB_VERSION;
	

        pcilib_info("Sample DMA engine, version %lu", ctx->version);

    }

    return (pcilib_dma_context_t*)ctx;
}

void  pcidev_dma_free(pcilib_dma_context_t *vctx) {
    pcidev_dma_t *ctx = (pcidev_dma_t*)vctx;

    if (ctx) {
	pcidev_dma_stop(vctx, PCILIB_DMA_ENGINE_ALL, PCILIB_DMA_FLAGS_DEFAULT);
	free(ctx);
    }
}

static void pcidev_dma_disable(pcidev_dma_t *ctx) {
	// We calling this function before cleaning memory to ensure no DMA write operations into this memory might be performed after return from this function
    return;
}

static void pcidev_dma_enable(pcidev_dma_t *ctx) {
	// Sequence to enable DMA engine and start streaming data. Memory should be ready and configured
    return;
}

static size_t pcidev_dma_find_buffer_by_bus_addr(pcidev_dma_t *ctx, uintptr_t bus_addr) {
    size_t i;

    for (i = 0; i < ctx->ring_size; i++) {
	uintptr_t buf_addr = pcilib_kmem_get_block_addr(ctx->dmactx.pcilib, ctx->pages, i);

	if (bus_addr == buf_addr) 
	    return i;
    }

    return (size_t)-1;
}


int pcidev_dma_start(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t flags) {
    pcidev_dma_t *ctx = (pcidev_dma_t*)vctx;

    pcidev_dma_desc_t *hw;

    int preserve = 0;
    pcilib_kmem_flags_t kflags;
    pcilib_kmem_reuse_state_t reuse_desc, reuse_pages;
    pcilib_kmem_handle_t *desc = NULL;
    pcilib_kmem_handle_t *pages = NULL;

    pcilib_register_value_t value;

    uintptr_t dma_region = 0;

	// Support single bank for now. We can support multiple banks with little modifications, e.g. bank number can be mapped to network port...
    if (dma == PCILIB_DMA_ENGINE_INVALID) return 0;
    else if (dma > 1) return PCILIB_ERROR_INVALID_BANK;

    if (!ctx->started) ctx->started = 1;

    if (flags&PCILIB_DMA_FLAG_PERSISTENT) ctx->preserve = 1;

    if (ctx->pages) return 0;

	// Get DMA configuration
    if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_timeout", &value))
	ctx->dma_timeout = value;
    else 
	ctx->dma_timeout = PCIDEV_DMA_TIMEOUT;

    if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_page_size", &value)) {
	if (value % PCIDEV_PAGE_SIZE) {
	    pcilib_error("Invalid DMA page size (%lu) is configured", value);
	    return PCILIB_ERROR_INVALID_ARGUMENT;
	}

	ctx->page_size = value;
    } else
	ctx->page_size = PCIDEV_PAGE_SIZE;

    if ((!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_pages", &value))&&(value > 0))
	ctx->ring_size = value;
    else
	ctx->ring_size = PCIDEV_DMA_PAGES;

    if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_region_low", &value)) {
	dma_region = value;
	if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_region_low", &value)) 
	    dma_region |= ((uintptr_t)value)<<32;
    }

    if (!pcilib_read_register(ctx->dmactx.pcilib, "dmaconf", "dma_flags", &value))
	ctx->dma_flags = value;
    else
	ctx->dma_flags = 0;


	// Allocate/map shared memory. There is two structures: 'desc' is descriptor which normally would be controlled by DMA engine to inform about its operations and 'pages' with actual data.
    kflags = PCILIB_KMEM_FLAG_REUSE|PCILIB_KMEM_FLAG_EXCLUSIVE|PCILIB_KMEM_FLAG_HARDWARE|(ctx->preserve?PCILIB_KMEM_FLAG_PERSISTENT:0);

    desc = pcilib_alloc_kernel_memory(ctx->dmactx.pcilib, PCILIB_KMEM_TYPE_CONSISTENT, 1, PCIDEV_DMA_DESCRIPTOR_SIZE, PCIDEV_DMA_DESCRIPTOR_ALIGNMENT, PCILIB_KMEM_USE(PCILIB_KMEM_USE_DMA_RING, 0x00), kflags);
    if (dma_region)
	pages = pcilib_alloc_kernel_memory(ctx->dmactx.pcilib, PCILIB_KMEM_TYPE_REGION_C2S, ctx->ring_size, ctx->page_size, dma_region, PCILIB_KMEM_USE(PCILIB_KMEM_USE_DMA_PAGES, 0x00), kflags);
    else
	pages = pcilib_alloc_kernel_memory(ctx->dmactx.pcilib, PCILIB_KMEM_TYPE_DMA_C2S_PAGE, ctx->ring_size, ctx->page_size, 0, PCILIB_KMEM_USE(PCILIB_KMEM_USE_DMA_PAGES, 0x00), kflags);

    if (!desc||!pages) {
	if (pages) pcilib_free_kernel_memory(ctx->dmactx.pcilib, pages, PCILIB_KMEM_FLAG_REUSE);
	if (desc) pcilib_free_kernel_memory(ctx->dmactx.pcilib, desc, PCILIB_KMEM_FLAG_REUSE);
	pcilib_error("Can't allocate required kernel memory for PCIDEV DMA engine (%lu pages of %lu bytes + %lu byte descriptor)", ctx->ring_size, ctx->page_size, (unsigned long)PCIDEV_DMA_DESCRIPTOR_SIZE);
	return PCILIB_ERROR_MEMORY;
    }
    reuse_desc = pcilib_kmem_is_reused(ctx->dmactx.pcilib, desc);
    reuse_pages = pcilib_kmem_is_reused(ctx->dmactx.pcilib, pages);

    hw = (pcidev_dma_desc_t*)pcilib_kmem_get_ua(ctx->dmactx.pcilib, desc);

	// Try to get status of DMA engine if shared memory already initialized and check for consistency. Re-initialize or return the error if the memory is inconsistent.
    if ((reuse_pages & PCILIB_KMEM_REUSE_PARTIAL)||(reuse_desc & PCILIB_KMEM_REUSE_PARTIAL)) {
	pcidev_dma_disable(ctx);

	pcilib_free_kernel_memory(ctx->dmactx.pcilib, pages, PCILIB_KMEM_FLAG_REUSE);
	pcilib_free_kernel_memory(ctx->dmactx.pcilib, desc, PCILIB_KMEM_FLAG_REUSE);

	if (((flags&PCILIB_DMA_FLAG_STOP) == 0)||(dma_region)) {
	    pcilib_error("Inconsistent DMA buffers are found (buffers are only partially re-used). This is very wrong, please stop DMA engine and correct configuration...");
	    return PCILIB_ERROR_INVALID_STATE;
	}
	
	pcilib_warning("Inconsistent DMA buffers are found (buffers are only partially re-used), reinitializing...");
	desc = pcilib_alloc_kernel_memory(ctx->dmactx.pcilib, PCILIB_KMEM_TYPE_CONSISTENT, 1, PCIDEV_DMA_DESCRIPTOR_SIZE, PCIDEV_DMA_DESCRIPTOR_ALIGNMENT, PCILIB_KMEM_USE(PCILIB_KMEM_USE_DMA_RING, 0x00), kflags|PCILIB_KMEM_FLAG_MASS);
	pages = pcilib_alloc_kernel_memory(ctx->dmactx.pcilib, PCILIB_KMEM_TYPE_DMA_C2S_PAGE, ctx->ring_size, ctx->page_size, 0, PCILIB_KMEM_USE(PCILIB_KMEM_USE_DMA_PAGES, 0x00), kflags|PCILIB_KMEM_FLAG_MASS);
    
	if (!desc||!pages) {
	    if (pages) pcilib_free_kernel_memory(ctx->dmactx.pcilib, pages, PCILIB_KMEM_FLAG_REUSE);
	    if (desc) pcilib_free_kernel_memory(ctx->dmactx.pcilib, desc, PCILIB_KMEM_FLAG_REUSE);
	    return PCILIB_ERROR_MEMORY;
	}

	hw = (pcidev_dma_desc_t*)pcilib_kmem_get_ua(ctx->dmactx.pcilib, desc);
    } else if (reuse_desc != reuse_pages) {
	 pcilib_warning("Inconsistent DMA buffers (modes of ring and page buffers does not match), reinitializing....");
    } else if (reuse_desc & PCILIB_KMEM_REUSE_REUSED) {
	if ((reuse_desc & PCILIB_KMEM_REUSE_PERSISTENT) == 0) pcilib_warning("Lost DMA buffers are found (non-persistent mode), reinitializing...");
	else if ((reuse_desc & PCILIB_KMEM_REUSE_HARDWARE) == 0) pcilib_warning("Lost DMA buffers are found (missing HW reference), reinitializing...");
	else {
		if (hw->page_count != ctx->ring_size) 
		    pcilib_warning("Inconsistent DMA buffers are found (Number of allocated buffers (%lu) does not match current request (%lu)), reinitializing...", hw->page_count, PCIDEV_DMA_PAGES);
		else 
		    preserve = 1;
	}
    }

	// get page size if default size was used
    if (!ctx->page_size)
	ctx->page_size = pcilib_kmem_get_block_size(ctx->dmactx.pcilib, pages, 0);

    if (preserve) {
	ctx->reused = 1;
	ctx->preserve = 1;
    } else {
	ctx->reused = 0;
	
	pcidev_dma_disable(ctx);
	pcidev_dma_enable(ctx);

	ctx->last_read = ctx->ring_size - 1;

	    // Normally this should be maintained by hardware
	hw->page_count = ctx->ring_size;
	hw->last_read_addr = pcilib_kmem_get_block_addr(ctx->dmactx.pcilib, pages, ctx->ring_size - 1);
	hw->last_write_addr = pcilib_kmem_get_block_addr(ctx->dmactx.pcilib, pages, ctx->ring_size - 2);
    }

    ctx->desc = desc;
    ctx->pages = pages;

    ctx->last_read = pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_read_addr);

    return 0;
}

int pcidev_dma_stop(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t flags) {
    pcilib_kmem_flags_t kflags;

    pcidev_dma_t *ctx = (pcidev_dma_t*)vctx;

    if (!ctx->started) return 0;

    if ((dma != PCILIB_DMA_ENGINE_INVALID)&&(dma > 1)) return PCILIB_ERROR_INVALID_BANK;

	    // ignoring previous setting if flag specified
    if (flags&PCILIB_DMA_FLAG_PERSISTENT) {
	ctx->preserve = 0;
    }

    if (ctx->preserve) {
	kflags = PCILIB_KMEM_FLAG_REUSE;
    } else {
        kflags = PCILIB_KMEM_FLAG_HARDWARE|PCILIB_KMEM_FLAG_PERSISTENT;

	ctx->started  = 0;

	pcidev_dma_disable(ctx);
    }

	// Clean buffers
    if (ctx->desc) {
	pcilib_free_kernel_memory(ctx->dmactx.pcilib, ctx->desc, kflags);
	ctx->desc = NULL;
    }

    if (ctx->pages) {
	pcilib_free_kernel_memory(ctx->dmactx.pcilib, ctx->pages, kflags);
	ctx->pages = NULL;
    }

    return 0;
}


int pcidev_dma_get_status(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcilib_dma_engine_status_t *status, size_t n_buffers, pcilib_dma_buffer_status_t *buffers) {
    size_t i;
    pcidev_dma_t *ctx = (pcidev_dma_t*)vctx;

    pcidev_dma_desc_t *hw = (pcidev_dma_desc_t*)pcilib_kmem_get_ua(ctx->dmactx.pcilib, ctx->desc);

    if (!status) return -1;

    pcilib_debug(DMA, "Current DMA status      - last read: %4u, last_read_addr: %4u (0x%x), last_write_addr: %4lu (0x%lx)", ctx->last_read, 
	pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_read_addr), hw->last_read_addr, 
	pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_write_addr), hw->last_write_addr
    );


    status->started = ctx->started;
    status->ring_size = ctx->ring_size;
    status->buffer_size = ctx->page_size;
    status->written_buffers = 0;
    status->written_bytes = 0;

	// For simplicity, we keep last_read here, and fix in the end
    status->ring_tail = ctx->last_read;

    status->ring_head = pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_write_addr);

    if (status->ring_head == (size_t)-1) {
	if (hw->last_write_addr) {
	    pcilib_warning("DMA is in unknown state, last_written_addr does not correspond any of available buffers");
	    return PCILIB_ERROR_FAILED;
	}
	status->ring_head = 0;
	status->ring_tail = 0;
    }

    if (n_buffers > ctx->ring_size) n_buffers = ctx->ring_size;

    if (buffers)
	memset(buffers, 0, n_buffers * sizeof(pcilib_dma_buffer_status_t));

    if (status->ring_head >= status->ring_tail) {
	for (i = status->ring_tail + 1; i <= status->ring_head; i++) {
	    status->written_buffers++;
	    status->written_bytes += ctx->page_size;

	    if ((buffers)&&(i < n_buffers)) {
		buffers[i].used = 1;
		buffers[i].size = ctx->page_size;
		buffers[i].first = 1;
		buffers[i].last = 1;
	    }
	}
    } else {
	for (i = 0; i <= status->ring_head; i++) {
	    status->written_buffers++;
	    status->written_bytes += ctx->page_size;

	    if ((buffers)&&(i < n_buffers)) {
	        buffers[i].used = 1;
		buffers[i].size = ctx->page_size;
	        buffers[i].first = 1;
	        buffers[i].last = 1;
	    }
	} 
	
	for (i = status->ring_tail + 1; i < status->ring_size; i++) {
	    status->written_buffers++;
	    status->written_bytes += ctx->page_size;

	    if ((buffers)&&(i < n_buffers)) {
		buffers[i].used = 1;
	        buffers[i].size = ctx->page_size;
		buffers[i].first = 1;
	        buffers[i].last = 1;
	    }
	} 
    }

	// We actually keep last_read in the ring_tail, so need to increase
    if (status->ring_tail != status->ring_head) {
	status->ring_tail++;
	if (status->ring_tail == status->ring_size) status->ring_tail = 0;
    }

    return 0;
}


int pcidev_dma_stream_read(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, uintptr_t addr, size_t size, pcilib_dma_flags_t flags, pcilib_timeout_t timeout, pcilib_dma_callback_t cb, void *cbattr) {
    int err, ret = PCILIB_STREAMING_REQ_PACKET;

    pcilib_timeout_t wait = 0;
    struct timeval start, cur;

    pcilib_dma_flags_t packet_flags = PCILIB_DMA_FLAG_EOP;

    size_t nodata_sleep;
    struct timespec sleep_ts = {0};

    size_t cur_read;

    pcidev_dma_t *ctx = (pcidev_dma_t*)vctx;

	// We auto-start if not started yet (then, we will also stop DMA on finishing the app)
    err = pcidev_dma_start(vctx, dma, PCILIB_DMA_FLAGS_DEFAULT);
    if (err) return err;

    pcidev_dma_desc_t *hw = (pcidev_dma_desc_t*)pcilib_kmem_get_ua(ctx->dmactx.pcilib, ctx->desc);

    
    switch (sched_getscheduler(0)) {
     case SCHED_FIFO:
     case SCHED_RR:
        if (ctx->dma_flags&PCIDEV_DMA_FLAG_NOSLEEP)
    	    nodata_sleep = 0;
    	else
	    nodata_sleep = PCIDEV_DMA_NODATA_SLEEP;
	break;
     default:
	pcilib_info_once("Streaming DMA data using non real-time thread (may cause extra CPU load)", errno);
	nodata_sleep = 0;
    }

    do {
	switch (ret&PCILIB_STREAMING_TIMEOUT_MASK) {
	    case PCILIB_STREAMING_CONTINUE:
		wait = ctx->dma_timeout; 
	    break;
	    case PCILIB_STREAMING_WAIT: 
		wait = (timeout > ctx->dma_timeout)?timeout:ctx->dma_timeout;
	    break;
	}

	pcilib_debug(DMA, "Waiting for data in %4u - last_read: %4u, last_read_addr: %4u (0x%08x), last_written: %4u (0x%08x)", ctx->last_read + 1, ctx->last_read, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_read_addr), hw->last_read_addr, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_write_addr), hw->last_write_addr
	);

	    // Wait for data. In this example we always have data, but normally DMA engine will maintain last_write_addr...
	gettimeofday(&start, NULL);
	memcpy(&cur, &start, sizeof(struct timeval));
	while (((hw->last_write_addr == 0)||(hw->last_write_addr == hw->last_read_addr))&&((wait == PCILIB_TIMEOUT_INFINITE)||(((cur.tv_sec - start.tv_sec)*1000000 + (cur.tv_usec - start.tv_usec)) < wait))) {
	    if (nodata_sleep) {
	        sleep_ts.tv_nsec = nodata_sleep;
		nanosleep(&sleep_ts, NULL);
	    }
		
	    gettimeofday(&cur, NULL);
	}
	
	    // Failing out if we exited on timeout
	if ((hw->last_write_addr == 0)||(hw->last_write_addr == hw->last_read_addr)) {
	    return (ret&PCILIB_STREAMING_FAIL)?PCILIB_ERROR_TIMEOUT:0;
	}

	    // Getting next page to read
	cur_read = ctx->last_read + 1;
	if (cur_read == ctx->ring_size) cur_read = 0;

	pcilib_debug(DMA, "Got buffer          %4u - last read: %4u, last_read_addr: %4u (0x%x), last_written: %4u (0x%x)", cur_read, ctx->last_read, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_read_addr), hw->last_read_addr, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_write_addr), hw->last_write_addr
	);

	    // In real case, we would need to sync to ensure that DMA transfer is finished and cashes are coherent
	//if ((ctx->dma_flags&PCIDEV_DMA_FLAG_NOSYNC) == 0)
	//    pcilib_kmem_sync_block(ctx->dmactx.pcilib, ctx->pages, PCILIB_KMEM_SYNC_FROMDEVICE, cur_read);

        void *buf = (void*)pcilib_kmem_get_block_ua(ctx->dmactx.pcilib, ctx->pages, cur_read);
	ret = cb(cbattr, packet_flags, ctx->page_size, buf);
	if (ret < 0) return -ret;
	
	    // Implementing a real DMA engine, here we might need to put buffer back into the DMA queue
	hw->last_read_addr = pcilib_kmem_get_block_addr(ctx->dmactx.pcilib, ctx->pages, cur_read);
	hw->last_write_addr = pcilib_kmem_get_block_addr(ctx->dmactx.pcilib, ctx->pages, ctx->last_read);

	ctx->last_read = cur_read;

	pcilib_debug(DMA, "Buffer returned     %4u - last read: %4u, last_read_addr: %4u (0x%x), last_written: %4u (0x%x)", cur_read, ctx->last_read, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_read_addr), hw->last_read_addr, 
		pcidev_dma_find_buffer_by_bus_addr(ctx, hw->last_write_addr), hw->last_write_addr
	);


    } while (ret);

    return 0;
}