// *****************************************************************************
//
// CSCI:    
//
// Functional Group:    
//
// AUTHOR:              C. Pompey
//
// FILE:                BenchmarkUtils.c
//
//
// *****************************************************************************
#define _XOPEN_SOURCE 500
#include "BenchmarkUtils.h"
#include <stdint.h>
#include <time.h>
#include <stdio.h>

//------------------------------------------------------------------------------
// Local Defines
//------------------------------------------------------------------------------
#define FREQ_TEST_DUR_SECONDS 3
//------------------------------------------------------------------------------
// Local Typedefs
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Local Global variables (Globals with File scope)
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Global variables
//------------------------------------------------------------------------------

uint64_t timerStart;
uint64_t monitorStart;
//------------------------------------------------------------------------------
// Local Function Prototypes
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// Global Functions
//------------------------------------------------------------------------------



void BenchmarkClear(void)
{
    // Write initial value in MSR ([SPE]=1, [ME]=1).
    // This will disable interrupts associated with EE (same as
    // what IntDisableInterrupts() achieves)   
    return;
}

void BenchmarkStart(void)
{
	monitorStart = get_cycle_count();
    return;
}

void BenchmarkStop(void)
{   
    printf("Performance monitor duration %ul\n", get_cycle_count() - monitorStart);
    return;
}
//------------------------------------------------------------------------------
// Local Functions
//------------------------------------------------------------------------------

void test_freq(int seconds){
	//Wait 1 sec (1.5G clock cycles)
	uint64_t startTime = time(NULL);
	unsigned long start_count = get_cycle_count();
	unsigned long cycles_per_second = 1500000000/1.5;
	while ((get_cycle_count() - start_count)  < (cycles_per_second * seconds) ){
		;
	}
	uint64_t stopTime = time(NULL);
	printf("Test freq took %d secs when it should take %d sec\n", stopTime - startTime,seconds);
	
}




long get_cycle_count(){
    unsigned long val;
    __asm__ volatile("isb\n\t"                             /* sync before read */
                     "mrc p15, 0, %0, c9, c13, 0\n\t"      /* Read PMCCNTR */
                     "isb\n\t"                             /* sync after read */
                     : "=r"(val) :: "memory");
    return (long)val;  /* PMCCNTR is 32-bit in AArch32 */
}

void enable_counter(){
    unsigned long v;

    /* Enable + reset all counters and cycle counter (E|P|C bits = 0x7) */
    v = 0x7UL;
    __asm__ volatile("mcr p15, 0, %0, c9, c12, 0" :: "r"(v) : "memory");

    /* Enable cycle counter specifically (bit 31 in PMCNTENSET) */
    v = (1UL << 31);
    __asm__ volatile("mcr p15, 0, %0, c9, c12, 1" :: "r"(v) : "memory");

    /* Clear overflow flags (write all 1s to PMOVSR) */
    v = 0xFFFFFFFFUL;
    __asm__ volatile("mcr p15, 0, %0, c9, c12, 3" :: "r"(v) : "memory");

    /* Allow user-mode (PL0) access if needed – PMUSERENR bit0=1 */
    v = 1UL;
    __asm__ volatile("mcr p15, 0, %0, c9, c14, 0" :: "r"(v) : "memory");

    __asm__ volatile("isb" ::: "memory");  /* ensure effect before continuing */
}

void configure_cache(uint8_t enData, uint8_t enInstruction){
    uint32_t sctlr;
    uint32_t zero = 0;

    /* Read SCTLR (System Control Register) */
    __asm__ volatile("mrc p15, 0, %0, c1, c0, 0" : "=r"(sctlr) :: "memory");

    /* Instruction cache control */
    if (enInstruction) {
        if ((sctlr & (1U << 12)) == 0) {
            /* Invalidate entire I-cache to PoU */
            __asm__ volatile("mcr p15, 0, %0, c7, c5, 0" :: "r"(zero) : "memory");
            /* Invalidate branch predictor array */
            __asm__ volatile("mcr p15, 0, %0, c7, c5, 6" :: "r"(zero) : "memory");
            __asm__ volatile("dsb" ::: "memory");
            __asm__ volatile("isb" ::: "memory");
        }
        sctlr |= (1U << 12);  /* Set I bit */
    } else {
        sctlr &= ~(1U << 12); /* Clear I bit */
    }

    /* Data cache control */
    if (enData) {
        __asm__ volatile("dsb" ::: "memory");
        sctlr |= (1U << 2);   /* Set C bit */
    } else {
        /* Ideally clean+invalidate D-cache before disabling */
        __asm__ volatile("dsb" ::: "memory");
        sctlr &= ~(1U << 2);  /* Clear C bit */
    }

    /* Write updated SCTLR back */
    __asm__ volatile("mcr p15, 0, %0, c1, c0, 0" :: "r"(sctlr) : "memory");
    __asm__ volatile("isb" ::: "memory");  /* sync change */
}