berlin_tools/bootloader/source/arch/arm64/gic_diag.c

/*
*
* Public C file to initialise the MPCore Interrupt Distributor.
* This file contains one function.
*
* Description:
* This code sets up the primary interrupt controller (GIC) in the MPCore
* to generate an IRQ to CPU 0 or 1
*
* Implementation:
* After the MPCore GIC is initialised with enableMPGIC(), you can use
* setEnableMPGIC() and clearEnableMPGIC() to set and clear interrupt
* sources in the ARM11 MPCore.
*
* Inputs:
* None.
*
* Outputs:
* MPCore Interrupt Distributor registers.
*
* Return Value:
* None.
*/

#include "memmap.h"
#include "soc.h"
#include "apbRegBase.h"
#include "gic_diag.h"
#include "debug.h"
#include "io.h"
#include "lgpl_printf.h"


typedef unsigned long uintptr_t;
typedef unsigned int  uint32_t;

typedef struct {
    int irq_id;
    void (*isr)(void);
    int is_used;
}isr_entry_t;

#define NUM_MP_CORE         4
#define MASTER_MP_CORE      0

#define MAX_ISR_ENTRY 4 //define max isr entry to 4

static int is_irq_inited = 0;
static isr_entry_t isr_entry[MAX_ISR_ENTRY];

#define GIC_BASE                    MEMMAP_GIC_REG_BASE

#define GICD_BASE                   0x1000
#define GICC_BASE                   0x2000

#define GICD_CTLR                   (GICD_BASE + 0x00)          // Distributor Control Register
#define GICD_TYPER                  (GICD_BASE + 0x04)          // Interrupt Controller Type Register
#define GICD_IGROUPRn               (GICD_BASE + 0x080)         // Interrupt Group Registers
#define GICD_ISENABLERn             (GICD_BASE + 0x100)         // Interrupt Set-Enable Registers
#define GICD_ICENABLERn             (GICD_BASE + 0x180)         // Interrupt Clear-Enable Registers
#define GICD_ISPENDRn               (GICD_BASE + 0x200)         // Interrupt Set-Pending Registers
#define GICD_ICPENDRn               (GICD_BASE + 0x280)         // Interrupt Clear-Pending Registers
#define GICD_ISACTIVERn             (GICD_BASE + 0x300)         // Interrupt Set-Active Registers
#define GICD_ICACTIVERn             (GICD_BASE + 0x380)         // Interrupt Clear-Active Registers
#define GICD_IPRIORITYRn            (GICD_BASE + 0x400)         // Interrupt Priority Registers
#define GICD_ITARGETSRn             (GICD_BASE + 0x800)         // Interrupt Processor Targets Registers
#define GICD_ICFGRn                 (GICD_BASE + 0xC00)         // Interrupt Configuration Registers
#define GICD_SGIR                   (GICD_BASE + 0xF00)         // Software Generated Interrupt Register
#define GICD_ICPIDR2                (GICD_BASE + 0xFE8)         // Peripheral ID2 Register

#define GICC_CTLR                   (GICC_BASE + 0x00)          // CPU Interface Control Register
#define GICC_PMR                    (GICC_BASE + 0x04)          // Interrupt Priority Mask RegisteR
#define GICC_BPR                    (GICC_BASE + 0x08)          // Binary Point Register
#define GICC_IAR                    (GICC_BASE + 0x0C)          // Interrupt Acknowledge Register
#define GICC_EOIR                   (GICC_BASE + 0x10)          // End of Interrupt Register

#define GIC_REG_READ(reg)           (*(volatile unsigned int*)((uintptr_t)(GIC_BASE+(reg))))
#define GIC_REG_WRITE(reg, v)       (*(volatile unsigned int*)((uintptr_t)(GIC_BASE+(reg))) = (v))
#define GIC_REG_WRITE_WITH_MASK(a, v, m)                      GIC_REG_WRITE(a, (GIC_REG_READ(a)&(~(m))) | ((v)&(m)))
#define GIC_REG_SET_1BIT_WITH_MASK(base, index, value)        GIC_REG_WRITE_WITH_MASK(base+(index/32*4), (value)<<((index)&0x1f), (1<<((index)&0x1f)))
#define GIC_REG_SET_8BIT_WITH_MASK(base, index, value)        GIC_REG_WRITE_WITH_MASK(base+(index/4*4), (value)<<(8*((index)&0x3)), (0xff<<(8*((index)&0x3))))

#define GIC_REG_SET_1BIT(base, index, value)        GIC_REG_WRITE(base+(index/32*4), (value)<<((index)&0x1f))



#define GIC_PPI_NUM                 (0x20)
#define GIC_SPI_NUM                 (IRQ_TOTAL_IRQ)
#define MAX_GIC_IRQ_NUM             (GIC_PPI_NUM + GIC_SPI_NUM)


extern uint32_t getMPid(void);
extern void EnableIRQ(void);
void GIC_IRQ_Handler(void)
{
    unsigned int MPCoreInterruptID;
    int irq_id, i = 0;
    //int MPid = getMPid();
    MPCoreInterruptID = GIC_REG_READ(GICC_IAR);                            // reading ID from Acknowledge register changes the interrupt from Pending to Active
    irq_id = MPCoreInterruptID & 0x3FF;
    if (irq_id == 0x3FF) {
        printf("GIC IRQ spurious irq\n");
	GIC_REG_WRITE(GICC_EOIR, MPCoreInterruptID);
        return;
    }
    dbg_printf(PRN_DBG, " Gic IRQ %d received\n", irq_id);
    for(i = 0; i < MAX_ISR_ENTRY; i++) {
        if(isr_entry[i].irq_id == irq_id) {
            isr_entry[i].isr();
            break;
        }
    }
    GIC_REG_WRITE(GICC_EOIR, MPCoreInterruptID);
    return;
}
static void fix_for_missing_ack()
{
    int i;
    if (0 == (GIC_REG_READ(GICD_CTLR) & 1))
    {
        return;
    }
    for (i = 0; i < MAX_GIC_IRQ_NUM; i += 32)
    {
        unsigned int temp;
        temp = GIC_REG_READ(GICD_ISACTIVERn+(i/32)*4);
        if (temp)
        {
            int j;
            for (j = 0; j < 32; j++)
			{
				if (temp & (1<<j))
				{
                    GIC_REG_WRITE(GICC_EOIR, i + j);
				}
			}
		}
	}
}

//this function initialize interrupt 0 to 63
//1. Distributor Interface
//   a) Enable All interrupts.
//   b) Do not route any interrupt to CPU0 and CPU1, use this as the control to turn on interrupt at CPU0 or CPU1
//   c) All level-sensitive and 1-N (only one CPU will handle the interrupt)
//   d) All interrupts have highest priority
//2. CPU Interface
//   a) Priority Mask is lowest
//   b) Pre-empty All interrupts
void initMPGIC(void)
{
    unsigned int MPid = getMPid();
    int i;
    unsigned int temp;
    dbg_printf(PRN_INFO, "Mpid = %d\n", MPid);

    if (MPid == MASTER_MP_CORE)
    {
        fix_for_missing_ack();
        GIC_REG_WRITE(GICC_CTLR, 0);        // Disable interrupts in GIC CPU Interface
        GIC_REG_WRITE(GICD_CTLR, 0);        // Disable interrupts in GIC Distributor

        for (i = 0; i < MAX_GIC_IRQ_NUM; i += 32)
        {
            GIC_REG_WRITE(GICD_IGROUPRn     +(i/32)*4, 0xFFFFFFFF);    // all generate IRQ
            GIC_REG_WRITE(GICD_ICENABLERn+(i/32)*4, 0xFFFFFFFF);    // Disable all interrupt sources
            GIC_REG_WRITE(GICD_ICPENDRn+(i/32)*4,   0xFFFFFFFF);    // Clear all pending interrupts
            // In case interrupt is not acked, clear active interrupt
        }

    //Note that SetEnable0_31 is banked for each core
    GIC_REG_WRITE(GICD_ISENABLERn,   0xFFFFFFFF);   // Enable all SGI PPI
        GIC_REG_WRITE(GICD_IGROUPRn,    0xFFFFFFFF);    //all NS interrupts, will generate IRQ

    // check configurated cpu numbers for GIC
    temp = GIC_REG_READ(GICD_TYPER);
    if ((((temp>>5)&0x7) + 1) > 1) // more than one core
    {
        // We set distributor's GICD_ITARGETSR to enable/disable interrupt for one core while keeping interrupt enabled.
        // For single core, this method doesn't work because interrupt always is send to core. We should use disable/enable on interrupt.
            for (i = 32; i < MAX_GIC_IRQ_NUM; i += 32)
            {
                GIC_REG_WRITE(GICD_ISENABLERn+(i/32)*4, 0xFFFFFFFF);
            }
        }


        // vpp uses FIQ
//        GIC_REG_SET_1BIT_WITH_MASK(GICD_IGROUPRn,    (IRQ_dHubIntrAvio0+32), 0);
//        GIC_REG_SET_8BIT_WITH_MASK(GICD_IPRIORITYRn, (IRQ_dHubIntrAvio0+32), 0); // set to highest priority

        // Set all interrupt priorities to high.
        for (i = 0; i < MAX_GIC_IRQ_NUM; i += 4)
        {
            GIC_REG_WRITE(GICD_IPRIORITYRn+(i/4)*4, 0x80808080); // all generate IRQ
        }

        //set interrupt goes to none of the CPU0, will be turned on later
        for (i = 0; i < MAX_GIC_IRQ_NUM; i += 4)
        {
            GIC_REG_WRITE(GICD_ITARGETSRn+(i/4)*4, 0);
         }

        // Set all interrupt sources to be level-sensitive and 1-N software model
        // 1-N/N-N bit may be obsolete
        for (i = 0; i < MAX_GIC_IRQ_NUM; i += 16)
        {
            GIC_REG_WRITE(GICD_ICFGRn+(i/16)*4, 0x55555555);
        }


    // These CPU interface registers are banked for each core
    // Enable all interrupt priorities (apart from the lowest priority, 0xF)
    // Note that bits [3:0] of this register are not implemented but could be in future
    GIC_REG_WRITE(GICC_PMR, 0xFF);
    GIC_REG_WRITE(GICC_BPR, 0x3);           // Enable pre-emption on all interrupts

    //have both Secure and NS interrupt to CPU
    //CPU will ack both S and NS interruts
    //S interrupts will generate FIQ
    GIC_REG_WRITE(GICC_CTLR, 0x7);          // [3]: FIQEn, [2]:AckCtl, [1]: EnableGrp1, [0]: EnableGrp0

        //disable_irqs();

        GIC_REG_WRITE(GICD_CTLR, 0x3);      // [1]: EnableGrp1, [0]: EnableGrp0
    }
    else
    {
        GIC_REG_WRITE(GICC_CTLR, 0);            // Disable interrupts in GIC CPU Interface
        GIC_REG_WRITE(GICD_ISENABLERn,  0xFFFFFFFF);    // Enable all SGI PPI
        GIC_REG_WRITE(GICD_IGROUPRn,    0xFFFFFFFF);    //all NS interrupts, will generate IRQ
        GIC_REG_WRITE(GICC_PMR, 0xFF);          // Enable all interrupt priorities (apart from the lowest priority, 0xF)
        GIC_REG_WRITE(GICC_BPR, 0x3);           // Enable pre-emption on all interrupts
        GIC_REG_WRITE(GICC_CTLR, 0xF);          // [3]: FIQEn, [2]:AckCtl, [1]: EnableGrp1, [0]: EnableGrp0
    }
}

//enable an interrupt
void setEnableMPGIC(unsigned int id)
{
    printf("%s, EnableIRQ %d\n", __FUNCTION__, id);
    GIC_REG_SET_1BIT(GICD_ISENABLERn, id, 1);
}


//disable an interrupt
void clearEnableMPGIC(unsigned int id)
{
    GIC_REG_SET_1BIT(GICD_ICENABLERn, id, 1);
}

//this function turns an interrupt on or off for a particular CPU
//by routing the interrupt to that CPU at distributor interface
//this is only useful for intId >=32,
//for intId <= 32, Int target regs are read-only
int diag_GICSetInt(int MPid, int intId, int enable)
{
    unsigned int temp;

    // check configurated cpu numbers for GIC
    temp = GIC_REG_READ(GICD_TYPER);
    if ((((temp>>5)&0x7) + 1) > 1) // more than one core
    {
        GIC_REG_SET_8BIT_WITH_MASK(GICD_ITARGETSRn, intId, ((enable ? 1:0) << MPid));
    }
    else // one core only
    {
        if(enable)
            setEnableMPGIC(intId);
        else
            clearEnableMPGIC(intId);

        return 0;
    }

    return 0;
}


int diag_GicSGI(int cpuList, int sgiId)



    //GIC_Acknowledge is effectively banked.
    //that is for CPU0 and CPU1, they are really different registers even with same address



    // writing ID to End of Interrupt register changes the interrupt from Active to Inactive
{
    //int MPid = getMPid();

    //GIC_Acknowledge is effectively banked.
    //that is for CPU0 and CPU1, they are really different registers even with same address

    GIC_REG_WRITE(GICD_SGIR, (cpuList<<16) | sgiId); // Secure SGI
    GIC_REG_WRITE(GICD_SGIR, (cpuList<<16) | (1<<15) | sgiId); // // [15]: SATT=1 for Non-Secure SGI
    return 0;
}

void do_irq ()
{
	GIC_IRQ_Handler();
}
#if defined(BERLIN_SOC_BG4DTV)
extern void  IRQ_Handler(void);
extern void arm_set_DFSR(unsigned int value);
extern unsigned int arm_get_DFSR();
extern void arm_set_DFAR(unsigned int value);
extern int arm_get_DFAR();
void clear_FSR()
{
    arm_set_DFSR(0);
    arm_set_DFAR(0);
}
static void InstallHighHandler(unsigned int handlerAddr, unsigned int vector)
{
        REG_WRITE32(vector, 0xE59FF018);
        REG_WRITE32(vector+0x20, handlerAddr);
}
#endif
static void init_irq(void)
{
    int i = 0;

    for(i = 0; i < MAX_ISR_ENTRY; i++) {
        isr_entry[i].is_used = 0;
    }
#if defined(BERLIN_SOC_BG4DTV)
    printf("%s, InstallHighHandler\n", __FUNCTION__) ;
	clear_FSR();
    InstallHighHandler((unsigned int)IRQ_Handler, MEMMAP_HIGH_EXCEPT_IRQ_REG);
#endif

    //GIC distribution interface is only needed to be initialized once
    //but the CPU interface needs to be initalized by both CPU cores
    //CPU interface registers are banked (same address)
    //So we will have both first and second CPU to call the same funtion
    dbg_printf(PRN_DBG,"%s, initMPGIC\n", __FUNCTION__) ;
    initMPGIC();

    // enable IRQ
    EnableIRQ();
}

int register_isr(void (*isr)(void), int irq_id)
{
    int i = 0;
    dbg_printf(PRN_DBG, "register irq_id = %d\n", irq_id);

    if(is_irq_inited != 1) {
        init_irq();
        is_irq_inited = 1;
    }

    for(i = 0; i < MAX_ISR_ENTRY; i++) {
        if(isr_entry[i].is_used == 0) {
            isr_entry[i].isr = isr;
            isr_entry[i].irq_id = MP_BERLIN_INTR_ID(irq_id);
            isr_entry[i].is_used = 1;
            break;
        }
    }

    if(i >= MAX_ISR_ENTRY) {
        dbg_printf(PRN_ERR, "######exceed the max number of irq!\n");
        return 1;
    }
    return 0;
}

void set_irq_enable(int irq_id)
{
    diag_GICSetInt(getMPid(), MP_BERLIN_INTR_ID(irq_id), INT_ENABLE);
}
/**/