/******************************************************************************
*
* Copyright (C) 2009 - 2014 Xilinx, Inc.  All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* Use of the Software is limited solely to applications:
* (a) running on a Xilinx device, or
* (b) that interact with a Xilinx device through a bus or interconnect.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
* XILINX  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* Except as contained in this notice, the name of the Xilinx shall not be used
* in advertising or otherwise to promote the sale, use or other dealings in
* this Software without prior written authorization from Xilinx.
*
******************************************************************************/

/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

#include <stdio.h>
#include <stdint.h>
#include <sleep.h>
#include <assert.h>

#include "platform.h"
#include "xscugic.h"
#include "xil_printf.h"
#include "csaccess.h"

#define N_MES 100

#define FTM_TRIGOUT 0
#define CTM_CHAN 	0

#define PREM_CNTR_ADDR	0xa0000000
#define PREM_PHASE_CONF 0
#define PREM_LIMIT_PREF 0x4
#define PREM_LIMIT_COMP 0x8
#define PREM_LIMIT_WB 	0xc
#define PREM_CNTR_PREF	0x10

#define PREM_IRQ0 		121U

#define CTI_SOC1_ADDR 0xFE9A0000

#define MRS32(reg) ({ uint32_t v; asm volatile ("mrs %0," # reg : "=r" (v)); v; })
#define MRS64(reg) ({ uint64_t v; asm volatile ("mrs %0," # reg : "=r" (v)); v; })

#define MSR(reg, v) ({ asm volatile ("msr " # reg ",%0" :: "r" (v)); })

static void ccntr_init(void)
{
	MSR(PMCNTENSET_EL0, 0x80000000);
	MSR(PMCR_EL0, MRS32(PMCR_EL0) | 1);
}

static uint64_t ccntr_get(void)
{
	return MRS64(PMCCNTR_EL0);
}

static uint32_t reg_read(uintptr_t addr)
{
	return *(volatile uint32_t *) addr;
}

static void reg_write(uintptr_t addr, uint32_t data)
{
	volatile uint32_t *localaddr = (volatile uint32_t *)addr;
	*localaddr = data;
}

volatile static uint64_t tic[N_MES];
volatile static uint64_t tac[N_MES];
volatile static uint32_t count[N_MES];
volatile static size_t i_mes = 0;
volatile static size_t irq_ack = 0;

volatile size_t stop = 0xffffffff;

static void irq_handler()
{
	// get stop time
	tac[i_mes] = ccntr_get();
	// clear the interrupt by resetting the counters
	reg_write(PREM_CNTR_ADDR, 0);
	// increment measurement to s
	i_mes++;
	irq_ack = 1;
}

static XScuGic Intc;
void irq_configure(int irq, void * handler)
{

	/*
	 * Initialize the interrupt controller driver so that it is ready to
	 * use.
	 */
	XScuGic_Config  * GicConfig = XScuGic_LookupConfig(XPAR_SCUGIC_0_DEVICE_ID);
	assert(!XScuGic_CfgInitialize(&Intc, GicConfig,
					GicConfig->CpuBaseAddress));
	/*
	 * Setup the Interrupt System
	 * Connect the interrupt controller interrupt handler to the hardware
	 * interrupt handling logic in the ARM processor.
	 */
	// set edge triggered
	XScuGic_SetPriorityTriggerType(&Intc, irq, 0xa0, 0b11);
	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
			(Xil_ExceptionHandler) XScuGic_InterruptHandler,
			&Intc);
	/*
	 * Enable interrupts in the ARM
	 */
	Xil_ExceptionEnable();

	/*
	 * Connect a device driver handler that will be called when an
	 * interrupt for the device occurs, the device driver handler performs
	 * the specific interrupt processing for the device
	 */
	XScuGic_Connect(&Intc, irq,
			   (Xil_ExceptionHandler) handler,
			   (void *)&Intc);


	/*
	 * Enable the interrupt for the device and then cause (simulate) an
	 * interrupt so the handlers will be called
	 */
	XScuGic_Enable(&Intc, irq);
}

int main()
{
    init_platform();
    print("# configure\n\r");

    // trig configure
	ccntr_init();
    cs_init();
    cs_device_t cti_soc1 = cs_device_register(CTI_SOC1_ADDR);
    cs_cti_reset(cti_soc1);
    cs_cti_set_global_channels(cti_soc1, 0);
    cs_cti_set_trigout_channels(cti_soc1, FTM_TRIGOUT, 1 << CTM_CHAN);
    cs_cti_enable(cti_soc1);
    print("# cs configured\n\r");

    // counter configure
	reg_write(PREM_CNTR_ADDR, 0);
	printf("counter: %u\n\r", reg_read(PREM_CNTR_ADDR + PREM_CNTR_PREF));
	reg_write(PREM_CNTR_ADDR + PREM_LIMIT_PREF, 0);

	// irq configure
	printf("configure irq\n\r");
	fflush(0);
	irq_configure(PREM_IRQ0, irq_handler);
	sleep(5);

#if 1
    print("# measure\n\r");
    fflush(0);
    // while not the end of measurements windows
    while (i_mes < N_MES) {
    	// get start time
    	tic[i_mes] = ccntr_get();
    	// write phase 1
    	reg_write(PREM_CNTR_ADDR, 1);
    	// send signal
    	cs_cti_pulse_channel(cti_soc1, CTM_CHAN);
		while (!irq_ack);
		// reset pl counter (write phase 0)
		irq_ack = 0;
		usleep(500);
    }

    // print measurements
    print("#tac\ttic\tdelta\tns \n\r");
    for (int i = 0; i < N_MES; ++i)
    {
    	printf("%lu\t%lu\t%lu\t%lf\n\r", tac[i], tic[i], tac[i] - tic[i], (tac[i] - tic[i]) / (double) 1200 );
    }
    fflush(0);
#else

    long unsigned int start = ccntr_get();
    reg_write(PREM_CNTR_ADDR, 1);
    cs_cti_pulse_channel(cti_soc1, CTM_CHAN);
    while (!irq_ack);

    printf("*****%lf*******", (double) (stop - start) / (double) 1200);
    printf("%lu", i_mes);
    fflush(0);
    cleanup_platform();
    cs_shutdown();
    print("done!");
    return 0;

#endif

}
