mini2440 ucos FIQ中断问题

我想在ucos中加入FIQ处理函数。
在2440init.s做了一些修改，
异常向量表中FIQ的跳转修改为
b    FIQ_HANDLE
跳转到
    IMPORT Handler_FIQ
FIQ_HANDLE
    stmfd    sp!,{r0-r3,lr}
    mrs        r3,spsr
    stmfd    sp!,{r3}
    bl        Handler_FIQ
    ldmfd    sp!,{r3}
    msr        spsr_cxsf,r3
    ldmfd    sp!,{r0-r3,pc}^

Handler_FIQ是FIQ的处理函数，但运行后，当产生FIQ中断，程序死了。

void Handler_FIQ(void)
{
#if OS_CRITICAL_METHOD == 3                                /* Allocate storage for CPU status register */
                      OS_CPU_SR  cpu_sr;
   #endif
    OS_ENTER_CRITICAL();
    beep(1);
    delay(100);
    beep(0);

    rEINTPEND = rEINTPEND;
    rSRCPND =rINTPND;
    rINTPND = rINTPND;
    OS_EXIT_CRITICAL();
}

为什么会这样？
我应该怎么做？