// ----------------------------------------------------------
// PL390 - Generic Interrupt Controller
//
// initial code had been provided by ARM Support team
// adopted by A. Nikolaev
//
//
// GIC exercise
// ----------------------------------------------------------

#include "gic.h"
#include "mcom02.h"

handler_funct_ptr func_handlers[INTERRUPTS_NUM][CORE_CNT];
handler_funct_ptr exception_handlers[SIZE][CORE_CNT];

__attribute__(( used )) const unsigned int ulICCIAR = 0x3900010c;
__attribute__(( used )) const unsigned int ulICCEOIR = 0x39000110;
__attribute__(( used )) const unsigned int ulICCPMR = 0x39000104;

volatile struct pl390_gic_dist_if* gic_dist;
volatile struct pl390_gic_cpu_if* gic_cpu;

// ------------------------------------------------------------
unsigned int get_core_number()
{
    unsigned int core_number;
    // get core number
    core_number = (unsigned int) &core_number;
    asm("MRC p15, 0, r8, c0, c0, 5");
    asm("MOV %0, r8" : "=r"(core_number));

    return core_number & 0x3;
}

// ------------------------------------------------------------
void initGICPointers(unsigned int dist, unsigned int cpu) {
    gic_dist = (volatile struct pl390_gic_dist_if*) dist;
    gic_cpu = (volatile struct pl390_gic_cpu_if*) cpu;

    return;
}

// ------------------------------------------------------------
//  Global enable of the Interrupt Distributor
void enableGIC(void) {
    gic_dist->ICDDCR = 1;
}

// Global disable of the Interrupt Distributor
void disableGIC(void) {
    gic_dist->ICDDCR = 0;
}

// ------------------------------------------------------------
// Enables the interrupt source number ID
void enableIntID(unsigned int ID) {
    unsigned int bank;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    gic_dist->ICDISER[bank] = ID;

}

// Disables the interrupt source number ID

void disableIntID(unsigned int ID) {
    unsigned int bank;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    gic_dist->ICDICER[bank] = ID;

}

// ------------------------------------------------------------
// Sets the priority of the specified ID
void setIntPriority(unsigned int ID, unsigned int priority) {
    unsigned int bank, tmp;

    priority = priority & 0xFF; // Priority field is 8-bits, mask off unused bit
    bank = ID / 4; // There are 4 IDs per register, need to work out which register to access
    ID = ID & 0x3; // ... and which field within the register
    ID = ID * 8; // Convert from which field to a bit offset (8-bits per field)

    priority = priority << ID; // Move prioity value into correct bit position

    tmp = gic_dist->ICDIPR[bank]; // Read the current value in the register
    tmp = tmp & ~(0xFF << ID); // Blank out the field holding the value we're modifying
    tmp = tmp | priority; // OR in the new priority
    gic_dist->ICDIPR[bank] = tmp;

}

// Returns the priority of the specified ID
unsigned int getIntPriority(unsigned int ID) {
    unsigned int bank, tmp;

    bank = ID / 4; // There are 4 IDs per register, need to work out which register to access
    ID = ID & 0x3; // ... and which field within the register
    ID = ID * 8; // Convert from which field to a bit offset (8-bits per field)

    tmp = gic_dist->ICDIPR[bank];

    tmp = tmp >> ID; // Shift desired field to bit position 0
    tmp = tmp & 0xFF; // Mask off the other bits

    return tmp;
}

// ------------------------------------------------------------
// Sets the target CPUs of the specified ID
// For 'target' use one of the above defines
void setIntTarget(unsigned int ID, unsigned int target) {
    unsigned int bank, tmp;

    target = target & 0xFF; // Target field is 8-bits, mask off unused bit
    bank = ID / 4; // There are 4 IDs per register, need to work out which register to access
    ID = ID & 0x3; // ... and which field within the register
    ID = ID * 8; // Convert from which field to a bit offset (8-bits per field)

    target = target << ID; // Move prioity value into correct bit position

    tmp = gic_dist->ICDIPTR[bank]; // Read the current value in the register
    tmp = tmp & ~(0xFF << ID); // Blank out the field holding the value we're modifying
    tmp = tmp | target; // OR in the new target
    gic_dist->ICDIPTR[bank] = tmp;

}

//Returns the target CPUs of the specified ID
unsigned int getIntTarget(unsigned int ID) {
    unsigned int bank, tmp;

    bank = ID / 4; // There are 4 IDs per register, need to work out which register to access
    ID = ID & 0x3; // ... and which field within the register
    ID = ID * 8; // Convert from which field to a bit offset (8-bits per field)

    tmp = gic_dist->ICDIPTR[bank];

    tmp = tmp >> ID; // Shift desired field to bit position 0
    tmp = tmp & 0xFF; // Mask off the other bits

    return tmp;
}

// ----------------------------------------------------------
// Configures the specified ID as being level or edge triggered
void configureID(unsigned int ID, unsigned int conf) {
    unsigned int bank, tmp;

    conf = conf & 0x3; // Mask out unused bits

    bank = ID / 16; // There are 16 IDs per register, need to work out which register to access
    ID = ID & 0xF; // ... and which field within the register
    ID = ID * 2; // Convert from which field to a bit offset (2-bits per field)

    conf = conf << ID; // Move prioity value into correct bit position

    tmp = gic_dist->ICDICFR[bank]; // Read current vlase
    tmp = tmp & ~(0x3 << ID); // Clear the bits for the specified field
    tmp = tmp | conf; // OR in new configuration
    gic_dist->ICDICFR[bank] = tmp; // Write updated value back

}

// ----------------------------------------------------------
// Sets the pending bit of the specified ID
void setIntPending(unsigned int ID) {
    unsigned int bank;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    gic_dist->ICDISPR[bank] = ID;

}

// Clears the pending bit of the specified ID
void clearIntPending(unsigned int ID) {
    unsigned int bank;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    gic_dist->ICDICPR[bank] = ID;

}

// Returns the value of the status bit of the specified ID
unsigned int getIntPending(unsigned int ID) {
    unsigned int bank, tmp;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    tmp = gic_dist->ICDISPR[bank]; // Read the register containing the ID we are interested in
    tmp = tmp >> ID; // Shift the status bit for specified ID to position 0
    tmp = tmp & 0x1; // Mask off the rest of the register

    return tmp;
}

// ------------------------------------------------------------
// Send a software generate interrupt
void sendSGI(unsigned int ID, unsigned int cpu_list, unsigned int filter_list, unsigned int SATT) {
    // Ensure unused bits are clear, and shift into correct bit position
    ID = ID & 0xF;
    SATT = (SATT & 0x1) << 15;
    cpu_list = (cpu_list & 0xFF) << 16;
    filter_list = (filter_list & 0x3) << 24;

    // Combine fields
    ID = ID | SATT | cpu_list | filter_list;

    gic_dist->ICDSGIR = ID;

}

// ------------------------------------------------------------
// Sets the specified ID as secure
void makeIntSecure(unsigned int ID) {
    unsigned int bank, tmp;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position
    ID = ~ID; // Invert to get mask

    tmp = gic_dist->ICDISR[bank]; // Read current value
    tmp = tmp & ID;
    gic_dist->ICDISR[bank] = tmp;

}

// Set the specified ID as non-secure
void makeIntNonSecure(unsigned int ID) {
    unsigned int bank, tmp;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    tmp = gic_dist->ICDISR[bank]; // Read current value
    tmp = tmp | ID; // Or with bit mask to set the bit
    gic_dist->ICDISR[bank] = tmp; // Write-back
}

// Returns the security of the specified ID
unsigned int getIntSecurity(unsigned int ID) {
    // TBD
    return 0;
}

// ------------------------------------------------------------
// CPU Interface functions
// ------------------------------------------------------------

// Enables the processor interface
// Must been done one each core seperately
void enableCPUInterface(void) {
    unsigned int tmp;

    tmp = gic_cpu->ICCICR;
    tmp = tmp | 0x1; // Set bit 0
    gic_cpu->ICCICR = tmp;
}

// Disables the processor interface
void disableCPUInterface(void) {
    unsigned int tmp;

    tmp = gic_cpu->ICCICR;
    tmp = tmp | 0xFFFFFFE; // Clear bit 0
    gic_cpu->ICCICR = tmp;
}

// Enables the sending of secure interrupts as FIQs
void enableSecureFIQs(void) {
    unsigned int tmp;

    tmp = gic_cpu->ICCICR;
    tmp = tmp | 0x8; // Set bit 3
    gic_cpu->ICCICR = tmp;
}

// Disables the sending of secure interrupts as FIQs
void disableSecureFIQs(void) {
    unsigned int tmp;

    tmp = gic_cpu->ICCICR;
    tmp = tmp | 0xFFFFFFF7; // Clear bit 3
    gic_cpu->ICCICR = tmp;
}

//  Returns the value of the Interrupt Acknowledge Register
unsigned int readIntAck(void) {
    return gic_cpu->ICCIAR;
}

// Writes ID to the End Of Interrupt register
void writeEOI(unsigned int ID) {
    gic_cpu->ICCEOIR = ID;
}

// Sets the Priority mask register for the core run on
// The reset value masks ALL interrupts!
void setPriorityMask(unsigned int priority) {
    gic_cpu->ICCPMR = (priority & 0xFF);
}

// Sets the Binary Point Register for the core run on
void setBinaryPoint(unsigned int priority) {
    gic_cpu->ICCBPR = (priority & 0xFF);
}

// ------------------------------------------------------------
// End of pl390_gic.c
// ------------------------------------------------------------

/* Global interrupt functions */
void enable_IRQ(void) {
    __asm__("MRS r1, CPSR");
    __asm__("BIC r1, r1, #0x80");
    __asm__("MSR CPSR_c, r1");
}

void disable_IRQ(void) {
    __asm__("MRS r1, CPSR");
    __asm__("ORR r1, r1, #0x80");
    __asm__("MSR CPSR_c, r1");
}

void enable_FIQ(void) {
    __asm__("MRS r1, CPSR");
    __asm__("BIC r1, r1, #0x40");
    __asm__("MSR CPSR_c, r1");
}

void disable_FIQ(void) {
    __asm__("MRS r1, CPSR");
    __asm__("ORR r1, r1, #0x40");
    __asm__("MSR CPSR_c, r1");
}

void C_int_handler(unsigned int ulICCIAR) {

    unsigned int id, mark;
    handler_funct_ptr current_function;

    // calculate which CPU cause IRQ
    id = ulICCIAR & 0x3FF;
    mark = ulICCIAR - id;

    if(id < INTERRUPTS_NUM)
    {
        if (mark == 0) {
            current_function = func_handlers[id][0];
        } else {
            current_function = func_handlers[id][1];
        }
        if(current_function) {
                  current_function((int) id);
                }
        else {
            // TODO: error message
        }
    }
}

void C_exception_handler(unsigned int exception_id) {
    unsigned int core_number = get_core_number();

    // call handler
    exception_handlers[exception_id][core_number](exception_id);
}


void attach_irq_handler(unsigned int ID, handler_funct_ptr funct_pointer, unsigned int core) {

    func_handlers[ID][core] = funct_pointer;

}

void attach_exception_handler(enum EXCEPTIONS ID, handler_funct_ptr funct_pointer, unsigned int core) {
    exception_handlers[ID][core] = funct_pointer;
}


void reset_irq_handlers(void) {
    unsigned int core_number = get_core_number();

    if(core_number >= CORE_CNT) return;

    int i;
    for (i = 0; i < INTERRUPTS_NUM; i++) {
        func_handlers[i][core_number] = 0;
    }
}

void dummy_exception_handler(int id) {
    asm("WFI");
}

void dummy_prefetch_abort_handler(int id) {
    /*asm("LDR r5, = 0x00800000");
    asm("STR r4, [r5]");
    asm("MRC p15, 0, r0, c5, c0, 1"); // IFSR
    asm("STR r0, [r5]");
    asm("MRC p15, 0, r0, c6, c0, 2"); // IFAR
    asm("STR r0, [r5]");*/
    asm("WFI");
}

void dummy_data_abort_handler(int id) {
    /*asm("LDR r5, = 0x00800000");
    asm("STR r4, [r5]");
    asm("MRC p15, 0, r0, c5, c0, 0"); // DFSR
    asm("STR r0, [r5]");
    asm("MRC p15, 0, r0, c6, c0, 0"); // DFAR
    asm("STR r0, [r5]");*/
    asm("WFI");
}

void reset_exception_handlers(void) {

    unsigned int core_number = get_core_number();
    if(core_number >= CORE_CNT) return;

    exception_handlers[UNDEFINED_INSTRUCTION][core_number] = dummy_exception_handler;
    exception_handlers[SOFTWARE_INTERRUPT][core_number] = dummy_exception_handler;
    exception_handlers[PREFETCH_ABORT][core_number] = dummy_prefetch_abort_handler;
    exception_handlers[DATA_ABORT][core_number] = dummy_data_abort_handler;
    exception_handlers[FIQ_PROCESSING][core_number] = dummy_exception_handler;
}

/****************************************************
 Group of procedures for old tests compatibility
 Do not use it in new tests!!!!
 ******************************************************/

void InterruptEnable(void) {
    // Enable interrupt controller
    enable_IRQ();
}

void InterruptDisable(void) {
    disable_IRQ();
}

void open_mask(int ID) {
    enableIntID((unsigned int) ID);
    setIntTarget((unsigned int) ID, 1);
    setIntPriority((unsigned int) ID, 0);
}

void openIM_mask(int ID) {
    // without body
}

void close_mask(int ID) {
    disableIntID((unsigned int) ID);
}

void closeIM_mask(int ID) {
}






void prefetchAbortInterrupt(int adr)
{
    dummy_prefetch_abort_handler(0);
}
void dataAbortInterrupt(int adr)
{
    dummy_data_abort_handler(0);
}
void FIQInterrupt()
{

}
void vApplicationIRQHandler( unsigned int ulICCIAR )
{
    C_int_handler(ulICCIAR);
}


// *****************************************************
void risc_initialize_gic()
{
  initGICPointers(GIC_BASE, ICC_BASE);
  reset_irq_handlers();
  reset_exception_handlers();
  enableGIC();
  enableCPUInterface();
  setPriorityMask(0xFF);
  enable_IRQ();
}

void risc_enable_interrupt(INTERRUPT_TYPE ID, unsigned int target, unsigned int priority)
{
  enableIntID(ID);
  setIntTarget(ID, target);
  setIntPriority(ID, priority);
}

void risc_disable_interrupt(INTERRUPT_TYPE ID)
{
    unsigned int bank;

    bank = ID / 32; // There are 32 IDs per register, need to work out which register to access
    ID = ID & 0x1f; // ... and which bit within the register

    ID = 1 << ID; // Move a '1' into the correct bit position

    gic_dist->ICDICER[bank] = ID;
}

void risc_disable_interrupts()
{
    gic_cpu->ICCPMR = 0;
}

void risc_enable_interrupts()
{
    gic_cpu->ICCPMR = 0xFF;
}

enum ERL_ERROR risc_register_interrupt(handler_funct_ptr funct_pointer, INTERRUPT_TYPE ID, unsigned int core)
{
    if (funct_pointer != 0)
        func_handlers[ID][core] = funct_pointer;
    else return ERL_UNITIALIZED_ARG;
    return ERL_NO_ERROR;
}