Zynq-PS-SDK(13) 之 VDMA+VTC+AXI4S-VideoOut 视频通路软件配置

在《Zynq-PS-SDK(12) 之 VDMA+VTC+AXI4S-VideoOut 视频通路硬件搭建》我们已经生成了 bitStream，硬件环境已经准备 OK，接下来进行软件的配置；

软件的配置主要针对的对象是下面的黄色部分，也就是 AXI4-lite 总线接入的寄存器配置，涉及到如下：

1、Dynamic Clock Generator；

2、VTC；

3、VDMA；

这里新增一个函数，叫做 Steph_VideoEngineInit，用于配置 video 相关的寄存器：

/********************************* Video Engine Initialize *********************************/
int Steph_VideoEngineInit(void)
{
    int Status = XST_SUCCESS;

    XAxiVdma_Config *vdmaConfig;

    Steph_FrameBufferInit();

    /*
     * Initialize VDMA driver
     */
    vdmaConfig = XAxiVdma_LookupConfig(VGA_VDMA_ID);
    if (!vdmaConfig)
    {
        xil_printf("No video DMA found for ID %d\r\n", VGA_VDMA_ID);
    }
    Status = XAxiVdma_CfgInitialize(&vdma, vdmaConfig, vdmaConfig->BaseAddress);
    if (Status != XST_SUCCESS)
    {
        xil_printf("VDMA Configuration Initialization failed %d\r\n", Status);
    }

    /*
     * Initialize the Display controller and start it
     */
    Status = DisplayInitialize(&dispCtrl, &vdma, DISP_VTC_ID, DYNCLK_BASEADDR, pFrames, DEMO_STRIDE);
    if (Status != XST_SUCCESS)
    {
        xil_printf("Display Ctrl initialization failed during demo initialization%d\r\n", Status);

    }
    Status = DisplayStart(&dispCtrl);
    if (Status != XST_SUCCESS)
    {
        xil_printf("Couldn't start display during demo initialization%d\r\n", Status);
    }

    DemoPrintTest(dispCtrl.framePtr[dispCtrl.curFrame], dispCtrl.vMode.width, dispCtrl.vMode.height, dispCtrl.stride, DEMO_PATTERN_0);

    return Status;
}

这里，缓存配置成为了 1 帧，用于显示图片：

#define BYTES_PIXEL 3

#define DEMO_MAX_FRAME (1920*1080*BYTES_PIXEL)
#define DEMO_STRIDE (1920 * BYTES_PIXEL)

/*
 * Framebuffers for video data
 */
u8 frameBuf[DISPLAY_NUM_FRAMES][DEMO_MAX_FRAME] __attribute__ ((aligned(64)));
u8 *pFrames[DISPLAY_NUM_FRAMES]; //array of pointers to the frame buffers

/********************************* Framebuffer Initialize *********************************/
void Steph_FrameBufferInit(void)
{
    int i = 0;
    /*
     * Initialize an array of pointers to the 3 frame buffers
     */
    for (i = 0; i < DISPLAY_NUM_FRAMES; i++)
    {
        pFrames[i] = frameBuf[i];
    }
}

在 DisplayInitialize 配置了动态时钟：

	ClkFindParams(dispPtr->vMode.freq, &clkMode);

	/*
	 * Store the obtained frequency to pxlFreq. It is possible that the PLL was not able to
	 * exactly generate the desired pixel clock, so this may differ from vMode.freq.
	 */
	dispPtr->pxlFreq = clkMode.freq;

	/*
	 * Write to the PLL dynamic configuration registers to configure it with the calculated
	 * parameters.
	 */
	if (!ClkFindReg(&clkReg, &clkMode))
	{
		xdbg_printf(XDBG_DEBUG_GENERAL, "Error calculating CLK register values\n\r");
		return XST_FAILURE;
	}
	ClkWriteReg(&clkReg, dispPtr->dynClkAddr);

	/*
	 * Enable the dynamically generated clock
    */
	ClkStart(dispPtr->dynClkAddr);

配置的函数主要是通过传入的期望时钟频率 vMode.freq，通过调用 ClkFindParams 来计算并寻找合适的输出时钟频率：clkMode.freq，并配置到寄存器中；

接着配置 VTC，也就是时序，时序的话，需要根据具体的分辨率配置不同的时序。这里，由于我们是 Source 设备，所以配置成为 Generator 即可；

	vtcConfig = XVtc_LookupConfig(vtcId);
	/* Checking Config variable */
	if (NULL == vtcConfig) {
		return (XST_FAILURE);
	}
	Status = XVtc_CfgInitialize(&(dispPtr->vtc), vtcConfig, vtcConfig->BaseAddress);
	/* Checking status */
	if (Status != (XST_SUCCESS)) {
		return (XST_FAILURE);
	}

	/*
	 * Configure the vtc core with the display mode timing parameters
	 */
	vtcTiming.HActiveVideo = dispPtr->vMode.width;	/**< Horizontal Active Video Size */
	vtcTiming.HFrontPorch = dispPtr->vMode.hps - dispPtr->vMode.width;	/**< Horizontal Front Porch Size */
	vtcTiming.HSyncWidth = dispPtr->vMode.hpe - dispPtr->vMode.hps;		/**< Horizontal Sync Width */
	vtcTiming.HBackPorch = dispPtr->vMode.hmax - dispPtr->vMode.hpe + 1;		/**< Horizontal Back Porch Size */
	vtcTiming.HSyncPolarity = dispPtr->vMode.hpol;	/**< Horizontal Sync Polarity */
	vtcTiming.VActiveVideo = dispPtr->vMode.height;	/**< Vertical Active Video Size */
	vtcTiming.V0FrontPorch = dispPtr->vMode.vps - dispPtr->vMode.height;	/**< Vertical Front Porch Size */
	vtcTiming.V0SyncWidth = dispPtr->vMode.vpe - dispPtr->vMode.vps;	/**< Vertical Sync Width */
	vtcTiming.V0BackPorch = dispPtr->vMode.vmax - dispPtr->vMode.vpe + 1;;	/**< Horizontal Back Porch Size */
	vtcTiming.V1FrontPorch = dispPtr->vMode.vps - dispPtr->vMode.height;	/**< Vertical Front Porch Size */
	vtcTiming.V1SyncWidth = dispPtr->vMode.vpe - dispPtr->vMode.vps;	/**< Vertical Sync Width */
	vtcTiming.V1BackPorch = dispPtr->vMode.vmax - dispPtr->vMode.vpe + 1;;	/**< Horizontal Back Porch Size */
	vtcTiming.VSyncPolarity = dispPtr->vMode.vpol;	/**< Vertical Sync Polarity */
	vtcTiming.Interlaced = 0;		/**< Interlaced / Progressive video */

	/* Setup the VTC Source Select config structure. */
	/* 1=Generator registers are source */
	/* 0=Detector registers are source */
	memset((void *)&SourceSelect, 0, sizeof(SourceSelect));
	SourceSelect.VBlankPolSrc = 1;
	SourceSelect.VSyncPolSrc = 1;
	SourceSelect.HBlankPolSrc = 1;
	SourceSelect.HSyncPolSrc = 1;
	SourceSelect.ActiveVideoPolSrc = 1;
	SourceSelect.ActiveChromaPolSrc= 1;
	SourceSelect.VChromaSrc = 1;
	SourceSelect.VActiveSrc = 1;
	SourceSelect.VBackPorchSrc = 1;
	SourceSelect.VSyncSrc = 1;
	SourceSelect.VFrontPorchSrc = 1;
	SourceSelect.VTotalSrc = 1;
	SourceSelect.HActiveSrc = 1;
	SourceSelect.HBackPorchSrc = 1;
	SourceSelect.HSyncSrc = 1;
	SourceSelect.HFrontPorchSrc = 1;
	SourceSelect.HTotalSrc = 1;

	XVtc_SelfTest(&(dispPtr->vtc));

	XVtc_RegUpdateEnable(&(dispPtr->vtc));
	XVtc_SetGeneratorTiming(&(dispPtr->vtc), &vtcTiming);
	XVtc_SetSource(&(dispPtr->vtc), &SourceSelect);
    /*
	 * Enable VTC core, releasing backpressure on VDMA
	 */
	XVtc_EnableGenerator(&dispPtr->vtc);

最后配置 VDMA 相关寄存器，这里有一些需要描述的东西，首先，需要告诉 VDMA 的 framebuffer 的起始地址在哪，所以要配置起始地址寄存器，同时还要配置图像的长宽，让 VDMA 知道从什么地方，搬运多长的数据；最后开启 VDMA，并停留在一帧：

XAxiVdma_Config *vdmaConfig;

    /*
     * Initialize VDMA driver
     */
    vdmaConfig = XAxiVdma_LookupConfig(VGA_VDMA_ID);
    if (!vdmaConfig)
    {
        xil_printf("No video DMA found for ID %d\r\n", VGA_VDMA_ID);
    }
    Status = XAxiVdma_CfgInitialize(&vdma, vdmaConfig, vdmaConfig->BaseAddress);
    if (Status != XST_SUCCESS)
    {
        xil_printf("VDMA Configuration Initialization failed %d\r\n", Status);
    }

	/*
	 * Configure the VDMA to access a frame with the same dimensions as the
	 * current mode
	 */
	dispPtr->vdmaConfig.VertSizeInput = dispPtr->vMode.height;
	dispPtr->vdmaConfig.HoriSizeInput = (dispPtr->vMode.width) * 3;
	dispPtr->vdmaConfig.FixedFrameStoreAddr = dispPtr->curFrame;
	/*
	 *Also reset the stride and address values, in case the user manually changed them
	 */
	dispPtr->vdmaConfig.Stride = dispPtr->stride;
	for (i = 0; i < DISPLAY_NUM_FRAMES; i++)
	{
		dispPtr->vdmaConfig.FrameStoreStartAddr[i] = (u32)  dispPtr->framePtr[i];
	}

	/*
	 * Perform the VDMA driver calls required to start a transfer. Note that no data is actually
	 * transferred until the disp_ctrl core signals the VDMA core by pulsing fsync.
	 */

	Status = XAxiVdma_DmaConfig(dispPtr->vdma, XAXIVDMA_READ, &(dispPtr->vdmaConfig));
	if (Status != XST_SUCCESS)
	{
		xil_printf("Read channel config failed %d\r\n", Status);
		return XST_FAILURE;
	}
	Status = XAxiVdma_DmaSetBufferAddr(dispPtr->vdma, XAXIVDMA_READ, dispPtr->vdmaConfig.FrameStoreStartAddr);
	if (Status != XST_SUCCESS)
	{
		xil_printf("Read channel set buffer address failed %d\r\n", Status);
		return XST_FAILURE;
	}
	Status = XAxiVdma_DmaStart(dispPtr->vdma, XAXIVDMA_READ);
	if (Status != XST_SUCCESS)
	{
		xil_printf("Start read transfer failed %d\r\n", Status);
		return XST_FAILURE;
	}
	Status = XAxiVdma_StartParking(dispPtr->vdma, dispPtr->curFrame, XAXIVDMA_READ);
	if (Status != XST_SUCCESS)
	{
		xil_printf("Unable to park the channel %d\r\n", Status);
		return XST_FAILURE;
	}

配置完成，这样的话，一旦开启 VDMA，那么数据的自动搬运就开始；

代码上传至 gitee：

https://gitee.com/stephenzhou-tech/Zynq7020_PS

可运行节点：

[Display] : Finished the Display to HDMI channel project

下面是 main 函数全部代码：

/******************************************************************************
*
* Copyright (C) 2010 - 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.
*
******************************************************************************/
/*****************************************************************************/
/**
*
* @file hello_world.c
*
* This file contains a design example using the Cortex A9 Scu Private
* Timer and the driver (XScuTimer) using interrupts, and MIO LED.
*
* @note		None.
*
* <pre>
*
* MODIFICATION HISTORY:
*
* Ver   Who  Date     Changes
* ----- ---- -------- ---------------------------------------------
* 1.00a nm   03/10/10 First release
* </pre>
*
******************************************************************************/

/***************************** Include Files *********************************/

#include "xparameters.h"
#include "xscutimer.h"
#include "xscugic.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include "xgpiops.h"
#include "steph_axi_pwm.h"
#include "sleep.h"
#include "xil_mmu.h"
#include "xaxivdma.h"
#include "xvtc.h"
#include "xil_cache.h"
#include "dynclk.h"
#include "xil_types.h"
#include "pic_800_600.h"
#include "display_ctrl.h"
#include "display_demo.h"

/************************** Constant Definitions *****************************/

/*
 * The following constants map to the XPAR parameters created in the
 * xparameters.h file. They are only defined here such that a user can easily
 * change all the needed parameters in one place.
 */
#define TIMER_DEVICE_ID		XPAR_XSCUTIMER_0_DEVICE_ID
#define INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
#define TIMER_IRPT_INTR		XPAR_SCUTIMER_INTR

#define TIMER_LOAD_VALUE	(XPAR_PS7_CORTEXA9_0_CPU_CLK_FREQ_HZ/2 - 1)
#define TIMEOUT_CNT			(30)
#define GPIO_DEVICE_ID  	XPAR_XGPIOPS_0_DEVICE_ID

/**************************** Type Definitions *******************************/

/***************** Macros (Inline Functions) Definitions *********************/
#define RD_32BIT_REG(addr)           (*(volatile uint32_t *)(addr))
#define WR_32BIT_REG(addr, value)    (*(volatile uint32_t *)(addr)) = (value)

#define STEPH_MIO_0         0
#define STEPH_MIO_1         13

#define STEPH_EMIO_0        54
#define STEPH_EMIO_1        55
#define STEPH_EMIO_2        56
#define STEPH_EMIO_3        57

#define MIO_OUTPUT          1
#define MIO_INPUT           0

#define MIO_OUTPUT_EN       1
#define MIO_OUTPUT_DIS      0

/* Accroding to the system.hdf file axi_gpio_0 */
#define AXI_GPIO_0_BASE_ADDR        0x41200000
#define GPIO_DATA_OFFSET            0x00
#define GPIO_TRI_OFFSET             0x04
#define GPIO_2_DATA_OFFSET          0x08
#define GPIO_2_TRI_OFFSET           0x0C
#define GIER_OFFSET                 0x11C
#define IP_ISR_OFFSET               0x120
#define IP_IER_OFFSET               0x128

/* PWM Configurations */
#define PWM_CYCLE                   2000
#define PWM_DUTY_STEP               3

/* CPU1 Address */
#define WAKEUP_CPU1_ADDR            0xFFFFFFF0
#define CPU1_RUN_ADDR               0x20000000
#define SOFTWARE_IRQ_ID_TO_CPU0     1
#define SOFTWARE_IRQ_ID_TO_CPU1     2

#define CPU_0_ID                    0x01
#define CPU_1_ID                    0x02

#define sev() __asm__("sev")

#define DEMO_VIDEO_H                800
#define DEMO_VIDEO_V                600
#define DEMO_VIDEO_STRIDE           (MAX_SUPPORT_H * BYTES_PIXEL)

/*
 * XPAR redefines
 */
#define DYNCLK_BASEADDR             XPAR_AXI_DYNCLK_0_BASEADDR
#define VGA_VDMA_ID                 XPAR_AXIVDMA_0_DEVICE_ID
#define DISP_VTC_ID                 XPAR_VTC_0_DEVICE_ID

#define FNC_CHECK_RETURN(Status)    \
{                                   \
    if (Status != XST_SUCCESS)      \
        return XST_FAILURE;         \
}

#define FNC_CHECK_NULL_POINTER(PTR) \
{                                   \
    if (NULL == IntcConfig)         \
        return XST_FAILURE;         \
}
/************************** Function Prototypes ******************************/

static void TimerIRQHandler(void *CallBackRef);
static void SoftwareIRQHandler(void *CallBackRef);

/************************** Variable Definitions *****************************/

XScuTimer TimerInstance;	/* Cortex A9 Scu Private Timer Instance */
XScuGic IntcInstance;		/* Interrupt Controller Instance */
XGpioPs Gpio;

DisplayCtrl dispCtrl;
XAxiVdma vdma;

volatile int TimerExpired;
volatile uint8_t trigger_cpu1 = 1;
/*
 * Framebuffers for video data
 */
u8 frameBuf[DISPLAY_NUM_FRAMES][DEMO_MAX_FRAME] __attribute__ ((aligned(64)));
u8 *pFrames[DISPLAY_NUM_FRAMES]; //array of pointers to the frame buffers

static void Steph_AXI_LED_On(void)
{
    WR_32BIT_REG(AXI_GPIO_0_BASE_ADDR + GPIO_DATA_OFFSET, 0x00);
}

static void Steph_AXI_LED_Off(void)
{
    WR_32BIT_REG(AXI_GPIO_0_BASE_ADDR + GPIO_DATA_OFFSET, 0x0F);
}

static void Steph_AXI_GPIO_Init(void)
{
    // Set as output
    Steph_AXI_LED_Off();
    WR_32BIT_REG(AXI_GPIO_0_BASE_ADDR + GPIO_TRI_OFFSET, 0x00);
}

static void Steph_PWM_ConfigCycle(uint32_t cycle)
{
    STEPH_AXI_PWM_mWriteReg(XPAR_STEPH_AXI_PWM_0_S00_AXI_BASEADDR, STEPH_AXI_PWM_S00_AXI_SLV_REG0_OFFSET, cycle);
}

static void Steph_PWM_ConfigDuty(uint32_t duty)
{
    STEPH_AXI_PWM_mWriteReg(XPAR_STEPH_AXI_PWM_0_S00_AXI_BASEADDR, STEPH_AXI_PWM_S00_AXI_SLV_REG1_OFFSET, duty);
}

static void Steph_AXI_PWM_Init(void)
{
    // The frequency of the PWM controller is 50MHz
    Steph_PWM_ConfigCycle(PWM_CYCLE);
    Steph_PWM_ConfigDuty(0x00);
}

uint32_t Steph_LEDInit(void)
{
    XGpioPs_Config *ConfigPtr = NULL;
    int Status = XST_SUCCESS;

    ConfigPtr = XGpioPs_LookupConfig(GPIO_DEVICE_ID);
    if(!ConfigPtr)
    {
        xil_printf("XGpioPs_LookupConfig Error.\r\n");
        return XST_FAILURE;
    }

    Status = XGpioPs_CfgInitialize(&Gpio, ConfigPtr,
                        ConfigPtr->BaseAddr);

    if (Status != XST_SUCCESS) {
        xil_printf("XGpioPs_CfgInitialize Error.\r\n");
        return XST_FAILURE;
    }

    // For MIO
    XGpioPs_SetDirectionPin(&Gpio, STEPH_MIO_0, MIO_OUTPUT);
    XGpioPs_SetDirectionPin(&Gpio, STEPH_MIO_1, MIO_OUTPUT);

    XGpioPs_SetOutputEnablePin(&Gpio, STEPH_MIO_0, MIO_OUTPUT_EN);
    XGpioPs_SetOutputEnablePin(&Gpio, STEPH_MIO_1, MIO_OUTPUT_EN);
    xil_printf("Steph_LED MIO Initialize Finished...\r\n");

    // For EMIO
    XGpioPs_SetDirectionPin(&Gpio, STEPH_EMIO_0, MIO_OUTPUT);

    XGpioPs_SetOutputEnablePin(&Gpio, STEPH_EMIO_0, MIO_OUTPUT_EN);
    xil_printf("Steph_LED EMIO Initialize Finished...\r\n");

    Steph_AXI_GPIO_Init();
    xil_printf("Steph_LED AXI GPIO Initialize Finished...\r\n");

    Steph_AXI_PWM_Init();
    xil_printf("Steph_LED AXI PWM Initialize Finished...\r\n");

    return Status;
}

void LED_OFF(uint32_t pin)
{
    XGpioPs_WritePin(&Gpio, pin, 0x1);
}

void LED_ON(uint32_t pin)
{
    XGpioPs_WritePin(&Gpio, pin, 0x0);
}


static void Steph_SetupCpu1(void)
{
    xil_printf("Close Cache...\r\n");
    Xil_SetTlbAttributes(0xFFFF0000, 0x14de2);
    xil_printf("Waking up CPU1...\r\n");
    Xil_Out32(WAKEUP_CPU1_ADDR, CPU1_RUN_ADDR);
    dmb();
    sev();
}

unsigned long Steph_GetSiliconVersion (void) {
  // Read PS version from MCTRL register [31:28]
  unsigned long mask = 0xF0000000;
  unsigned long *addr = (unsigned long*) 0XF8007080;
  unsigned long ps_version = (*addr & mask) >> 28;
  return ps_version;
}

int _Steph_RegisterIRQHandler(XScuGic *InstancePtr, uint16_t IntId, void *Handler, void *CallBackRef)
{
    int Status = XST_SUCCESS;

    Status = XScuGic_Connect(InstancePtr, IntId,
                (Xil_ExceptionHandler)Handler, (void *)CallBackRef);
    FNC_CHECK_RETURN(Status);

    XScuGic_Enable(InstancePtr, IntId);

    return Status;
}

int Steph_RegisterScuTimerHandler(void *Handler)
{
    return _Steph_RegisterIRQHandler(&IntcInstance, TIMER_IRPT_INTR, (void *)Handler, (void *)&TimerInstance);
}

int Steph_RegisterSoftwareHandler(void *Handler)
{
    return _Steph_RegisterIRQHandler(&IntcInstance, SOFTWARE_IRQ_ID_TO_CPU0, (void *)Handler, (void *)&IntcInstance);
}

int Steph_GicInit(void)
{
    int Status = XST_SUCCESS;
    XScuGic_Config *IntcConfig;
    XScuGic *IntcInstancePtr = &IntcInstance;

    IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
    FNC_CHECK_NULL_POINTER(IntcConfig);

    Status = XScuGic_CfgInitialize(IntcInstancePtr, IntcConfig, IntcConfig->CpuBaseAddress);
    FNC_CHECK_RETURN(Status);

    Xil_ExceptionInit();

    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT, (Xil_ExceptionHandler)XScuGic_InterruptHandler, IntcInstancePtr);

    Xil_ExceptionEnable();

    return Status;
}

int Steph_ScuTimerInit(void)
{
    int Status = XST_SUCCESS;
    XScuTimer_Config *ConfigPtr;
    XScuTimer *TimerInstancePtr = &TimerInstance;

    ConfigPtr = XScuTimer_LookupConfig(TIMER_DEVICE_ID);

    Status = XScuTimer_CfgInitialize(TimerInstancePtr, ConfigPtr, ConfigPtr->BaseAddr);
    FNC_CHECK_RETURN(Status);

    Status = XScuTimer_SelfTest(TimerInstancePtr);
    FNC_CHECK_RETURN(Status);

    XScuTimer_EnableAutoReload(TimerInstancePtr);

    XScuTimer_LoadTimer(TimerInstancePtr, TIMER_LOAD_VALUE);

    XScuTimer_EnableInterrupt(TimerInstancePtr);

    Steph_RegisterScuTimerHandler((void *)TimerIRQHandler);

    XScuTimer_Start(TimerInstancePtr);

    return Status;
}

int Steph_SoftwareInterruptInit(void)
{
    return Steph_RegisterSoftwareHandler((void *)SoftwareIRQHandler);
}

/*
 *
 * Video Engine Initialize Functions
 *
*/
void DemoPrintTest(u8 *frame, u32 width, u32 height, u32 stride, int pattern)
{
    u32 xcoi, ycoi;
    u32 iPixelAddr = 0;
    u8 wRed, wBlue, wGreen;
    u32 xInt;
    u32 pic_number=0;


    switch (pattern)
    {
    case DEMO_PATTERN_0:

        for(ycoi = 0; ycoi < 600; ycoi++)
        {
            for(xcoi = 0; xcoi < (800 * BYTES_PIXEL); xcoi+=BYTES_PIXEL)
            {
                frame[xcoi + iPixelAddr + 0] = gImage_pic_800_600[pic_number++];
                frame[xcoi + iPixelAddr + 1] = gImage_pic_800_600[pic_number++];
                frame[xcoi + iPixelAddr + 2] = gImage_pic_800_600[pic_number++];
            }
            iPixelAddr += stride;
        }
        /*
         * Flush the framebuffer memory range to ensure changes are written to the
         * actual memory, and therefore accessible by the VDMA.
         */
        Xil_DCacheFlushRange((unsigned int) frame, DEMO_MAX_FRAME);
        break;
    case DEMO_PATTERN_1:         //Grid

        for(ycoi = 0; ycoi < height; ycoi++)
        {
            for(xcoi = 0; xcoi < (width * BYTES_PIXEL); xcoi+=BYTES_PIXEL)
            {
                if (((xcoi/BYTES_PIXEL)&0x20)^(ycoi&0x20)) {
                    wRed = 255;
                    wGreen = 255;
                    wBlue = 255;
                }
                else{
                    wRed = 0;
                    wGreen = 0;
                    wBlue = 0;
                }


                frame[xcoi + iPixelAddr + 0] = wBlue;
                frame[xcoi + iPixelAddr + 1] = wGreen;
                frame[xcoi + iPixelAddr + 2] = wRed;
            }
            iPixelAddr += stride;
        }
        /*
         * Flush the framebuffer memory range to ensure changes are written to the
         * actual memory, and therefore accessible by the VDMA.
         */
        Xil_DCacheFlushRange((unsigned int) frame, DEMO_MAX_FRAME);
        break;
    case DEMO_PATTERN_2://8 intervals color bar

        for(ycoi = 0; ycoi < height; ycoi++)
        {
            for(xcoi = 0; xcoi < (width * BYTES_PIXEL); xcoi+=BYTES_PIXEL)
            {

                frame[xcoi + iPixelAddr + 0] = xcoi/BYTES_PIXEL;
                frame[xcoi + iPixelAddr + 1] = xcoi/BYTES_PIXEL;
                frame[xcoi + iPixelAddr + 2] = xcoi/BYTES_PIXEL;
            }
            iPixelAddr += stride;
        }
        /*
         * Flush the framebuffer memory range to ensure changes are written to the
         * actual memory, and therefore accessible by the VDMA.
         */
        Xil_DCacheFlushRange((unsigned int) frame, DEMO_MAX_FRAME);
        break;
    case DEMO_PATTERN_3: //8 intervals color bar

        xInt = width*BYTES_PIXEL / 8; //each with width/8 pixels

        for(ycoi = 0; ycoi < height; ycoi++)
        {

            /*
             * Just draw white in the last partial interval (when width is not divisible by 7)
             */

            for(xcoi = 0; xcoi < (width*BYTES_PIXEL); xcoi+=BYTES_PIXEL)
            {

                if (xcoi < xInt) {                                   //White color
                    wRed = 255;
                    wGreen = 255;
                    wBlue = 255;
                }

                else if ((xcoi >= xInt) && (xcoi < xInt*2)){         //YELLOW color
                    wRed = 255;
                    wGreen = 255;
                    wBlue = 0;
                }
                else if ((xcoi >= xInt*2) && (xcoi < xInt*3)){        //CYAN color
                    wRed = 0;
                    wGreen = 255;
                    wBlue = 255;
                }
                else if ((xcoi >= xInt*3) && (xcoi < xInt*4)){        //GREEN color
                    wRed = 0;
                    wGreen = 255;
                    wBlue = 0;
                }
                else if ((xcoi >= xInt*4) && (xcoi < xInt*5)){        //MAGENTA color
                    wRed = 255;
                    wGreen = 0;
                    wBlue = 255;
                }
                else if ((xcoi >= xInt*5) && (xcoi < xInt*6)){        //RED color
                    wRed = 255;
                    wGreen = 0;
                    wBlue = 0;
                }
                else if ((xcoi >= xInt*6) && (xcoi < xInt*7)){        //BLUE color
                    wRed = 0;
                    wGreen = 0;
                    wBlue = 255;
                }
                else {                                                //BLACK color
                    wRed = 0;
                    wGreen = 0;
                    wBlue = 0;
                }

                frame[xcoi+iPixelAddr + 0] = wBlue;
                frame[xcoi+iPixelAddr + 1] = wGreen;
                frame[xcoi+iPixelAddr + 2] = wRed;
                /*
                 * This pattern is printed one vertical line at a time, so the address must be incremented
                 * by the stride instead of just 1.
                 */
            }
            iPixelAddr += stride;

        }
        /*
         * Flush the framebuffer memory range to ensure changes are written to the
         * actual memory, and therefore accessible by the VDMA.
         */
        Xil_DCacheFlushRange((unsigned int) frame, DEMO_MAX_FRAME);
        break;
    default :
        xil_printf("Error: invalid pattern passed to DemoPrintTest");
    }
}
/********************************* Framebuffer Initialize *********************************/
void Steph_FrameBufferInit(void)
{
    int i = 0;
    /*
     * Initialize an array of pointers to the 3 frame buffers
     */
    for (i = 0; i < DISPLAY_NUM_FRAMES; i++)
    {
        pFrames[i] = frameBuf[i];
    }
}

/********************************* Video Engine Initialize *********************************/
int Steph_VideoEngineInit(void)
{
    int Status = XST_SUCCESS;

    XAxiVdma_Config *vdmaConfig;

    Steph_FrameBufferInit();

    /*
     * Initialize VDMA driver
     */
    vdmaConfig = XAxiVdma_LookupConfig(VGA_VDMA_ID);
    if (!vdmaConfig)
    {
        xil_printf("No video DMA found for ID %d\r\n", VGA_VDMA_ID);
    }
    Status = XAxiVdma_CfgInitialize(&vdma, vdmaConfig, vdmaConfig->BaseAddress);
    if (Status != XST_SUCCESS)
    {
        xil_printf("VDMA Configuration Initialization failed %d\r\n", Status);
    }

    /*
     * Initialize the Display controller and start it
     */
    Status = DisplayInitialize(&dispCtrl, &vdma, DISP_VTC_ID, DYNCLK_BASEADDR, pFrames, DEMO_STRIDE);
    if (Status != XST_SUCCESS)
    {
        xil_printf("Display Ctrl initialization failed during demo initialization%d\r\n", Status);

    }
    Status = DisplayStart(&dispCtrl);
    if (Status != XST_SUCCESS)
    {
        xil_printf("Couldn't start display during demo initialization%d\r\n", Status);
    }

    DemoPrintTest(dispCtrl.framePtr[dispCtrl.curFrame], dispCtrl.vMode.width, dispCtrl.vMode.height, dispCtrl.stride, DEMO_PATTERN_0);

    return Status;
}

int main(void)
{
    uint32_t pwm_duty = 0;

    xil_printf("####### StephenZhou CPU 0 #######\r\n");
    xil_printf("SiliconVersion=0x%08X\r\n", Steph_GetSiliconVersion());

    Steph_SetupCpu1();
    xil_printf("SetupCpu1 Finished...\r\n");

    Steph_LEDInit();
    xil_printf("LED Init Finished...\r\n");

    Steph_GicInit();
    xil_printf("GIC Init Finished...\r\n");

    Steph_ScuTimerInit();
    xil_printf("Scu Timer Init Finished...\r\n");

    Steph_SoftwareInterruptInit();
    xil_printf("Software Interrupt Init Finished...\r\n");

    Steph_VideoEngineInit();
    xil_printf("Video Engine Init Finished...\r\n");

    while (1)
    {
       if (pwm_duty < PWM_CYCLE)
       {
           pwm_duty += PWM_DUTY_STEP;
       }
       else
       {
           pwm_duty = 0;
       }
       Steph_PWM_ConfigDuty(pwm_duty);

       usleep(1000);

       if (trigger_cpu1)
       {
           // Software interrupt to CPU1
           XScuGic_SoftwareIntr(&IntcInstance, SOFTWARE_IRQ_ID_TO_CPU1, CPU_1_ID);

           trigger_cpu1 = 0;
       }
    }

    return XST_SUCCESS;
}

static void TimerIRQHandler(void *CallBackRef)
{
    XScuTimer *TimerInstancePtr = (XScuTimer *) CallBackRef;

    /*
     * Check if the timer counter has expired, checking is not necessary
     * since that's the reason this function is executed, this just shows
     * how the callback reference can be used as a pointer to the instance
     * of the timer counter that expired, increment a shared variable so
     * the main thread of execution can see the timer expired.
     */
    if (XScuTimer_IsExpired(TimerInstancePtr)) {
        XScuTimer_ClearInterruptStatus(TimerInstancePtr);
        TimerExpired++;
        if(TimerExpired%2)
        {
            LED_OFF(STEPH_MIO_0);
            LED_OFF(STEPH_MIO_1);

            LED_OFF(STEPH_EMIO_0);

            Steph_AXI_LED_Off();
        }
        else
        {
            LED_ON(STEPH_MIO_0);
            LED_ON(STEPH_MIO_1);

            LED_ON(STEPH_EMIO_0);

            Steph_AXI_LED_On();
        }
        xil_printf("Timer Counter=%d\r\n", TimerExpired);

    }
}

static void SoftwareIRQHandler(void *CallBackRef)
{
    xil_printf("Soft Interrupt from CPU1\r\n") ;
    trigger_cpu1 = 1;
}