zynq中断嵌套怎么实现，我这部分代码需要加哪些内容？

#include <stdio.h>
#include "xscugic.h"
#include "xil_exception.h"
#include "my_sopc.h"
#include "sleep.h"
#define INT_CFG0_OFFSET 0x00000C00
// Parameter definitions
#define SW1_INT_ID              61
#define SW2_INT_ID              62
#define SW3_INT_ID              63
#define INTC_DEVICE_ID          XPAR_PS7_SCUGIC_0_DEVICE_ID
#define INT_TYPE_RISING_EDGE    0x03
#define INT_TYPE_HIGHLEVEL      0x01
#define INT_TYPE_MASK           0x03
static XScuGic INTCInst;
static void SW_intr_Handler_ID61(void *param);
static void SW_intr_Handler_ID62(void *param);
static void SW_intr_Handler_ID63(void *param);
static int IntcInitFunction(u16 DeviceId);
unsigned int Inter_ID62_flag=0;
unsigned int count1=0;
unsigned int count2=0;
static void SW_intr_Handler_ID61(void *param)
{
    int sw_id = (int)param;
    int num_id61=0;
    printf("SW%d int\n\r", sw_id);
    num_id61=~((u32)1 << (4 + 16U)) & ((1 << 4) | 0xFFFF0000U);
    Xil_Out32(MY_DATA_2_LSW,num_id61);//直接寄存器操作

    while(count1 < 200)
    {
     printf("count1=%d \n\r", count1);
     num_id61=~((u32)1 << (4 + 16U)) & ((1 << 4) | 0xFFFF0000U);
     Xil_Out32(MY_DATA_2_LSW,num_id61);//直接寄存器操作
     usleep(100000);
     num_id61=~((u32)1 << (4 + 16U)) &  0xFFFF0000U;
     Xil_Out32(MY_DATA_2_LSW,num_id61);//直接寄存器操作
     usleep(100000);
     count1++;
    }
    count1=0;
}
static void SW_intr_Handler_ID62(void *param)
{
    int sw_id = (int)param;
    int num_id62=0;
    printf("SW%d int\n\r", sw_id);
    while((Inter_ID62_flag==1)&&(count2 < 300))
    {
     printf("count2=%d \n\r", count2);
     num_id62=~((u32)1 << (5 + 16U)) & ((1 << 5) | 0xFFFF0000U);
     Xil_Out32(MY_DATA_2_LSW,num_id62);//直接寄存器操作
     usleep(100000);
     num_id62=~((u32)1 << (5 + 16U)) &  0xFFFF0000U;
     Xil_Out32(MY_DATA_2_LSW,num_id62);//直接寄存器操作
     usleep(100000);
     count2++;
    }
    count2=0;
    Inter_ID62_flag=0;
    printf("Inter_ID62_flag=%d\n\r", Inter_ID62_flag);
}
static void SW_intr_Handler_ID63(void *param)
{
    int sw_id = (int)param;
    int num_id63=0;
    printf("SW%d int\n\r", sw_id);
    num_id63=~((u32)1 << (6+ 16U)) & ((1 << 6) | 0xFFFF0000U);
    Xil_Out32(MY_DATA_2_LSW,num_id63);//直接寄存器操作
    Inter_ID62_flag=1;
    printf("Inter_ID62_flag=%d\n\r", Inter_ID62_flag);
}

void IntcTypeSetup(XScuGic *InstancePtr, int intId, int intType)
{
    int mask;
    intType &= INT_TYPE_MASK;
    mask = XScuGic_DistReadReg(InstancePtr, INT_CFG0_OFFSET + (intId/16)*4);
    mask &= ~(INT_TYPE_MASK << (intId%16)*2);
    mask |= intType << ((intId%16)*2);
    XScuGic_DistWriteReg(InstancePtr, INT_CFG0_OFFSET + (intId/16)*4, mask);
}
int IntcInitFunction(u16 DeviceId)
{
    XScuGic_Config *IntcConfig;
    int status;
    // Interrupt controller initialisation
    IntcConfig = XScuGic_LookupConfig(DeviceId);
    status = XScuGic_CfgInitialize(&INTCInst, IntcConfig, IntcConfig->CpuBaseAddress);
    if(status != XST_SUCCESS) return XST_FAILURE;
    // Call to interrupt setup
    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
                                 (Xil_ExceptionHandler)XScuGic_InterruptHandler,
                                 &INTCInst);
    Xil_ExceptionEnable();
    // Connect SW1~SW3 interrupt to handler
    status = XScuGic_Connect(&INTCInst,
                             SW1_INT_ID,
                             (Xil_ExceptionHandler)SW_intr_Handler_ID61,
                             (void *)61);
    if(status != XST_SUCCESS) return XST_FAILURE;
    status = XScuGic_Connect(&INTCInst,
                             SW2_INT_ID,
                             (Xil_ExceptionHandler)SW_intr_Handler_ID62,
                             (void *)62);
    if(status != XST_SUCCESS) return XST_FAILURE;
    status = XScuGic_Connect(&INTCInst,
                             SW3_INT_ID,
                             (Xil_ExceptionHandler)SW_intr_Handler_ID63,
                             (void *)63);
    if(status != XST_SUCCESS) return XST_FAILURE;
    // Set interrupt type of SW1~SW3 to rising edge
    //IntcTypeSetup(&INTCInst, SW1_INT_ID, INT_TYPE_RISING_EDGE);
    XScuGic_SetPriorityTriggerType(&INTCInst, SW1_INT_ID,160, INT_TYPE_RISING_EDGE);
    //IntcTypeSetup(&INTCInst, SW2_INT_ID, INT_TYPE_RISING_EDGE);
    XScuGic_SetPriorityTriggerType(&INTCInst, SW2_INT_ID,168, INT_TYPE_RISING_EDGE);
    IntcTypeSetup(&INTCInst, SW3_INT_ID, INT_TYPE_RISING_EDGE);
    // Enable SW1~SW3 interrupts in the controller
    XScuGic_Enable(&INTCInst, SW1_INT_ID);
    XScuGic_Enable(&INTCInst, SW2_INT_ID);
    XScuGic_Enable(&INTCInst, SW3_INT_ID);
    return XST_SUCCESS;
}
int main(void)
{
u16 Value=0;
u8 i=0;
    print("PL int test\n\r");
  //EMIO的初始化 主要是禁用外部中断
Xil_Out32(MY_XINT_DIS0_ADDR,0XFFFFFFFFU);
Xil_Out32(MY_XINT_DIS1_ADDR,0XFFFFFFFFU);
Xil_Out32(MY_XINT_DIS2_ADDR,0XFFFFFFFFU);
Xil_Out32(MY_XINT_DIS3_ADDR,0XFFFFFFFFU);
  Xil_Out32(MY_DIRM_2,(Xil_In32(MY_DIRM_2)|0xff));
  Xil_Out32(MY_OEN_2,(Xil_In32(MY_OEN_2)|0xff));
    IntcInitFunction(INTC_DEVICE_ID);
    while(1)
    {
   Value = ~((u32)1 << (i + 16U)) & ((1 << i) | 0xFFFF0000U);
  // Value = (1 << i) ;
   Xil_Out32(MY_DATA_2_LSW,Value);//直接寄存器操作
   sleep(1); //延时
   // Value = ~((u32)1 << (7 + 16U)) & ((0 << 7) | 0xFFFF0000U);
  // Value &= ~(1 << i) ;
  // Xil_Out32(MY_DATA_2_LSW,Value);//
   // Xil_Out32(0xE000A040, (~(1 << 7)));
  // sleep(1); //延时
   i++;
   if(i>4)
    i=0;
    }
    return 0;
}

ZYNQ 好像不支持嵌套