Merge pull request #2503 from qz721/libcpu_cortex_a

Make 'qemu-vexpress-a9' and 'imx6ul' use the same libcpu code.

13 files changed, 785 insertions, 164 deletions

diff --git a/libcpu/arm/cortex-a/context_gcc.S b/libcpu/arm/cortex-a/context_gcc.S
index 97b3dbc447..b009630fa8 100644
--- a/libcpu/arm/cortex-a/context_gcc.S
+++ b/libcpu/arm/cortex-a/context_gcc.S
@@ -8,7 +8,14 @@
  * 2013-07-05     Bernard      the first version
  */
 
+#include "rtconfig.h"
 .section .text, "ax"
+
+#ifdef RT_USING_SMP
+#define rt_hw_interrupt_disable rt_hw_local_irq_disable
+#define rt_hw_interrupt_enable  rt_hw_local_irq_enable
+#endif
+
 /*
  * rt_base_t rt_hw_interrupt_disable();
  */
@@ -34,6 +41,16 @@ rt_hw_interrupt_enable:
 rt_hw_context_switch_to:
     ldr sp, [r0]            @ get new task stack pointer
 
+#ifdef RT_USING_SMP
+    mov     r0, r1
+    bl      rt_cpus_lock_status_restore
+#endif /*RT_USING_SMP*/
+
+#ifdef RT_USING_LWP
+    ldmfd sp, {r13, r14}^   @ pop usr_sp usr_lr
+    add sp, #8
+#endif
+
     ldmfd sp!, {r4}         @ pop new task spsr
     msr spsr_cxsf, r4
 
@@ -62,9 +79,24 @@ rt_hw_context_switch:
 
     stmfd sp!, {r4}         @ push cpsr
 
+#ifdef RT_USING_LWP
+    stmfd sp, {r13, r14}^   @ push usr_sp usr_lr
+    sub sp, #8
+#endif
+
     str sp, [r0]            @ store sp in preempted tasks TCB
     ldr sp, [r1]            @ get new task stack pointer
 
+#ifdef RT_USING_SMP
+    mov     r0, r2
+    bl      rt_cpus_lock_status_restore
+#endif /*RT_USING_SMP*/
+
+#ifdef RT_USING_LWP
+    ldmfd sp, {r13, r14}^   @ pop usr_sp usr_lr
+    add sp, #8
+#endif
+
     ldmfd sp!, {r4}         @ pop new task cpsr to spsr
     msr spsr_cxsf, r4
     ldmfd sp!, {r0-r12, lr, pc}^  @ pop new task r0-r12, lr & pc, copy spsr to cpsr
@@ -72,11 +104,70 @@ rt_hw_context_switch:
 /*
  * void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to);
  */
+.equ Mode_USR,        0x10
+.equ Mode_FIQ,        0x11
+.equ Mode_IRQ,        0x12
+.equ Mode_SVC,        0x13
+.equ Mode_ABT,        0x17
+.equ Mode_UND,        0x1B
+.equ Mode_SYS,        0x1F
+
+.equ I_Bit,           0x80            @ when I bit is set, IRQ is disabled
+.equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
+
 .globl rt_thread_switch_interrupt_flag
 .globl rt_interrupt_from_thread
 .globl rt_interrupt_to_thread
 .globl rt_hw_context_switch_interrupt
 rt_hw_context_switch_interrupt:
+#ifdef RT_USING_SMP
+    /* r0 :irq_mod context
+     * r1 :addr of from_thread's sp
+     * r2 :addr of to_thread's sp
+     * r3 :to_thread's tcb
+     */
+
+    @ r0 point to {r0-r3} in stack
+    push    {r1 - r3}
+    mov     r1, r0
+    add     r0, r0, #4*4
+    ldmfd   r0!, {r4-r12,lr}@ reload saved registers
+    mrs     r3,  spsr       @ get cpsr of interrupt thread
+    sub     r2,  lr, #4     @ save old task's pc to r2
+    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
+
+    stmfd   sp!, {r2}       @ push old task's pc
+    stmfd   sp!, {r4-r12,lr}@ push old task's lr,r12-r4
+    ldmfd   r1,  {r4-r7}    @ restore r0-r3 of the interrupt thread
+    stmfd   sp!, {r4-r7}    @ push old task's r0-r3
+    stmfd   sp!, {r3}       @ push old task's cpsr
+
+#ifdef RT_USING_LWP
+    stmfd sp, {r13,r14}^    @push usr_sp usr_lr
+    sub sp, #8
+#endif
+
+    msr     cpsr_c, #I_Bit|F_Bit|Mode_IRQ
+    pop     {r1 - r3}
+    mov     sp, r0
+    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
+    str     sp, [r1]
+
+    ldr     sp, [r2]
+    mov     r0, r3
+    bl      rt_cpus_lock_status_restore
+
+#ifdef RT_USING_LWP
+    ldmfd sp, {r13,r14}^  @pop usr_sp usr_lr
+    add sp, #8
+#endif
+
+    ldmfd   sp!, {r4}       @ pop new task's cpsr to spsr
+    msr     spsr_cxsf, r4
+
+    ldmfd   sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
+
+#else /*RT_USING_SMP*/
     ldr r2, =rt_thread_switch_interrupt_flag
     ldr r3, [r2]
     cmp r3, #1
@@ -89,3 +180,4 @@ _reswitch:
     ldr r2, =rt_interrupt_to_thread     @ set rt_interrupt_to_thread
     str r1, [r2]
     bx  lr
+#endif /*RT_USING_SMP*/
diff --git a/libcpu/arm/cortex-a/cp15.h b/libcpu/arm/cortex-a/cp15.h
index 486f38200d..97c5b93ad2 100644
--- a/libcpu/arm/cortex-a/cp15.h
+++ b/libcpu/arm/cortex-a/cp15.h
@@ -5,6 +5,7 @@
  *
  * Change Logs:
  * Date           Author       Notes
+ * 2018-03-25     quanzhao     the first version
  */
 #ifndef __CP15_H__
 #define __CP15_H__
@@ -15,6 +16,9 @@ void rt_cpu_mmu_disable(void);
 void rt_cpu_mmu_enable(void);
 void rt_cpu_tlb_set(volatile unsigned long*);
 
+void rt_cpu_dcache_clean_flush(void);
+void rt_cpu_icache_flush(void);
+
 void rt_cpu_vector_set_base(unsigned int addr);
 
 #endif
diff --git a/libcpu/arm/cortex-a/cp15_gcc.S b/libcpu/arm/cortex-a/cp15_gcc.S
index 0e3e60655c..dd2436ffee 100644
--- a/libcpu/arm/cortex-a/cp15_gcc.S
+++ b/libcpu/arm/cortex-a/cp15_gcc.S
@@ -15,6 +15,11 @@ rt_cpu_get_smp_id:
 
 .globl rt_cpu_vector_set_base
 rt_cpu_vector_set_base:
+    /* clear SCTRL.V to customize the vector address */
+    mrc     p15, #0, r1, c1, c0, #0
+    bic     r1, #(1 << 13)
+    mcr     p15, #0, r1, c1, c0, #0
+    /* set up the vector address */
     mcr     p15, #0, r0, c12, c0, #0
     dsb
     bx      lr
@@ -36,7 +41,7 @@ rt_hw_cpu_icache_enable:
 _FLD_MAX_WAY:
    .word  0x3ff
 _FLD_MAX_IDX:
-   .word  0x7ff
+   .word  0x7fff
 
 .globl rt_cpu_dcache_clean_flush
 rt_cpu_dcache_clean_flush:
@@ -84,6 +89,14 @@ finished:
     pop     {r4-r11}
     bx      lr
 
+.globl rt_cpu_icache_flush
+rt_cpu_icache_flush:
+    mov r0, #0
+    mcr p15, 0, r0, c7, c5, 0       @ I+BTB cache invalidate
+    dsb
+    isb
+    bx      lr
+
 .globl rt_hw_cpu_dcache_disable
 rt_hw_cpu_dcache_disable:
     push    {r4-r11, lr}
diff --git a/libcpu/arm/cortex-a/cpu.c b/libcpu/arm/cortex-a/cpu.c
index 2d78715011..6b129f89ee 100644
--- a/libcpu/arm/cortex-a/cpu.c
+++ b/libcpu/arm/cortex-a/cpu.c
@@ -6,28 +6,78 @@
  * Change Logs:
  * Date           Author       Notes
  * 2011-09-15     Bernard      first version
+ * 2018-11-22     Jesven       add rt_hw_cpu_id()
  */
 
 #include <rthw.h>
 #include <rtthread.h>
 #include <board.h>
 
+#ifdef RT_USING_SMP
+int rt_hw_cpu_id(void)
+{
+    int cpu_id;
+    __asm__ volatile (
+            "mrc p15, 0, %0, c0, c0, 5"
+            :"=r"(cpu_id)
+            );
+    cpu_id &= 0xf;
+    return cpu_id;
+};
+
+void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
+{
+    unsigned long tmp;
+    unsigned long newval;
+    rt_hw_spinlock_t lockval;
+
+    __asm__ __volatile__(
+            "pld [%0]"
+            ::"r"(&lock->slock)
+            );
+
+    __asm__ __volatile__(
+            "1: ldrex   %0, [%3]\n"
+            "   add %1, %0, %4\n"
+            "   strex   %2, %1, [%3]\n"
+            "   teq %2, #0\n"
+            "   bne 1b"
+            : "=&r" (lockval), "=&r" (newval), "=&r" (tmp)
+            : "r" (&lock->slock), "I" (1 << 16)
+            : "cc");
+
+    while (lockval.tickets.next != lockval.tickets.owner) {
+        __asm__ __volatile__("wfe":::"memory");
+        lockval.tickets.owner = *(volatile unsigned short *)(&lock->tickets.owner);
+    }
+
+    __asm__ volatile ("dmb":::"memory");
+}
+
+void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
+{
+    __asm__ volatile ("dmb":::"memory");
+    lock->tickets.owner++;
+    __asm__ volatile ("dsb ishst\nsev":::"memory");
+}
+#endif /*RT_USING_SMP*/
+
 /**
- * @addtogroup AM33xx
+ * @addtogroup ARM CPU
  */
 /*@{*/
 
 /** shutdown CPU */
 void rt_hw_cpu_shutdown()
 {
-	rt_uint32_t level;
-	rt_kprintf("shutdown...\n");
+    rt_uint32_t level;
+    rt_kprintf("shutdown...\n");
 
-	level = rt_hw_interrupt_disable();
-	while (level)
-	{
-		RT_ASSERT(0);
-	}
+    level = rt_hw_interrupt_disable();
+    while (level)
+    {
+        RT_ASSERT(0);
+    }
 }
 
 /*@}*/
diff --git a/libcpu/arm/cortex-a/gic.c b/libcpu/arm/cortex-a/gic.c
new file mode 100644
index 0000000000..2a46142b54
--- /dev/null
+++ b/libcpu/arm/cortex-a/gic.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2013-07-20     Bernard      first version
+ * 2014-04-03     Grissiom     many enhancements
+ * 2018-11-22     Jesven       add rt_hw_ipi_send()
+ *                             add rt_hw_ipi_handler_install()
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+
+#include "gic.h"
+#include "cp15.h"
+
+struct arm_gic
+{
+    rt_uint32_t offset;         /* the first interrupt index in the vector table */
+
+    rt_uint32_t dist_hw_base;   /* the base address of the gic distributor */
+    rt_uint32_t cpu_hw_base;    /* the base addrees of the gic cpu interface */
+};
+
+/* 'ARM_GIC_MAX_NR' is the number of cores */
+static struct arm_gic _gic_table[ARM_GIC_MAX_NR];
+
+#define GIC_CPU_CTRL(hw_base)               __REG32((hw_base) + 0x00)
+#define GIC_CPU_PRIMASK(hw_base)            __REG32((hw_base) + 0x04)
+#define GIC_CPU_BINPOINT(hw_base)           __REG32((hw_base) + 0x08)
+#define GIC_CPU_INTACK(hw_base)             __REG32((hw_base) + 0x0c)
+#define GIC_CPU_EOI(hw_base)                __REG32((hw_base) + 0x10)
+#define GIC_CPU_RUNNINGPRI(hw_base)         __REG32((hw_base) + 0x14)
+#define GIC_CPU_HIGHPRI(hw_base)            __REG32((hw_base) + 0x18)
+
+#define GIC_DIST_CTRL(hw_base)              __REG32((hw_base) + 0x000)
+#define GIC_DIST_TYPE(hw_base)              __REG32((hw_base) + 0x004)
+#define GIC_DIST_IGROUP(hw_base, n)         __REG32((hw_base) + 0x080 + ((n)/32) * 4)
+#define GIC_DIST_ENABLE_SET(hw_base, n)     __REG32((hw_base) + 0x100 + ((n)/32) * 4)
+#define GIC_DIST_ENABLE_CLEAR(hw_base, n)   __REG32((hw_base) + 0x180 + ((n)/32) * 4)
+#define GIC_DIST_PENDING_SET(hw_base, n)    __REG32((hw_base) + 0x200 + ((n)/32) * 4)
+#define GIC_DIST_PENDING_CLEAR(hw_base, n)  __REG32((hw_base) + 0x280 + ((n)/32) * 4)
+#define GIC_DIST_ACTIVE_SET(hw_base, n)     __REG32((hw_base) + 0x300 + ((n)/32) * 4)
+#define GIC_DIST_ACTIVE_CLEAR(hw_base, n)   __REG32((hw_base) + 0x380 + ((n)/32) * 4)
+#define GIC_DIST_PRI(hw_base, n)            __REG32((hw_base) + 0x400 +  ((n)/4) * 4)
+#define GIC_DIST_TARGET(hw_base, n)         __REG32((hw_base) + 0x800 +  ((n)/4) * 4)
+#define GIC_DIST_CONFIG(hw_base, n)         __REG32((hw_base) + 0xc00 + ((n)/16) * 4)
+#define GIC_DIST_SOFTINT(hw_base)           __REG32((hw_base) + 0xf00)
+#define GIC_DIST_CPENDSGI(hw_base, n)       __REG32((hw_base) + 0xf10 + ((n)/4) * 4)
+#define GIC_DIST_ICPIDR2(hw_base)           __REG32((hw_base) + 0xfe8)
+
+static unsigned int _gic_max_irq;
+
+int arm_gic_get_active_irq(rt_uint32_t index)
+{
+    int irq;
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = GIC_CPU_INTACK(_gic_table[index].cpu_hw_base);
+    irq += _gic_table[index].offset;
+    return irq;
+}
+
+void arm_gic_ack(rt_uint32_t index, int irq)
+{
+    rt_uint32_t mask = 1 << (irq % 32);
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    GIC_DIST_ENABLE_CLEAR(_gic_table[index].dist_hw_base, irq) = mask;
+    GIC_CPU_EOI(_gic_table[index].cpu_hw_base) = irq;
+    GIC_DIST_ENABLE_SET(_gic_table[index].dist_hw_base, irq) = mask;
+}
+
+void arm_gic_mask(rt_uint32_t index, int irq)
+{
+    rt_uint32_t mask = 1 << (irq % 32);
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    GIC_DIST_ENABLE_CLEAR(_gic_table[index].dist_hw_base, irq) = mask;
+}
+
+void arm_gic_clear_pending(rt_uint32_t index, int irq)
+{
+    rt_uint32_t mask = 1 << (irq % 32);
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    GIC_DIST_PENDING_CLEAR(_gic_table[index].dist_hw_base, irq) = mask;
+}
+
+void arm_gic_clear_active(rt_uint32_t index, int irq)
+{
+    rt_uint32_t mask = 1 << (irq % 32);
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    GIC_DIST_ACTIVE_CLEAR(_gic_table[index].dist_hw_base, irq) = mask;
+}
+
+/* Set up the cpu mask for the specific interrupt */
+void arm_gic_set_cpu(rt_uint32_t index, int irq, unsigned int cpumask)
+{
+    rt_uint32_t old_tgt;
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    old_tgt = GIC_DIST_TARGET(_gic_table[index].dist_hw_base, irq);
+
+    old_tgt &= ~(0x0FFUL << ((irq % 4)*8));
+    old_tgt |=   cpumask << ((irq % 4)*8);
+
+    GIC_DIST_TARGET(_gic_table[index].dist_hw_base, irq) = old_tgt;
+}
+
+void arm_gic_umask(rt_uint32_t index, int irq)
+{
+    rt_uint32_t mask = 1 << (irq % 32);
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    irq = irq - _gic_table[index].offset;
+    RT_ASSERT(irq >= 0);
+
+    GIC_DIST_ENABLE_SET(_gic_table[index].dist_hw_base, irq) = mask;
+}
+
+void arm_gic_dump_type(rt_uint32_t index)
+{
+    unsigned int gic_type;
+
+    gic_type = GIC_DIST_TYPE(_gic_table[index].dist_hw_base);
+    rt_kprintf("GICv%d on %p, max IRQs: %d, %s security extension(%08x)\n",
+               (GIC_DIST_ICPIDR2(_gic_table[index].dist_hw_base) >> 4) & 0xf,
+               _gic_table[index].dist_hw_base,
+               _gic_max_irq,
+               gic_type & (1 << 10) ? "has" : "no",
+               gic_type);
+}
+
+void arm_gic_dump(rt_uint32_t index)
+{
+    unsigned int i, k;
+
+    k = GIC_CPU_HIGHPRI(_gic_table[index].cpu_hw_base);
+    rt_kprintf("--- high pending priority: %d(%08x)\n", k, k);
+    rt_kprintf("--- hw mask ---\n");
+    for (i = 0; i < _gic_max_irq / 32; i++)
+    {
+        rt_kprintf("0x%08x, ",
+                   GIC_DIST_ENABLE_SET(_gic_table[index].dist_hw_base,
+                                       i * 32));
+    }
+    rt_kprintf("\n--- hw pending ---\n");
+    for (i = 0; i < _gic_max_irq / 32; i++)
+    {
+        rt_kprintf("0x%08x, ",
+                   GIC_DIST_PENDING_SET(_gic_table[index].dist_hw_base,
+                                        i * 32));
+    }
+    rt_kprintf("\n--- hw active ---\n");
+    for (i = 0; i < _gic_max_irq / 32; i++)
+    {
+        rt_kprintf("0x%08x, ",
+                   GIC_DIST_ACTIVE_SET(_gic_table[index].dist_hw_base,
+                                       i * 32));
+    }
+    rt_kprintf("\n");
+}
+#ifdef RT_USING_FINSH
+#include <finsh.h>
+FINSH_FUNCTION_EXPORT_ALIAS(arm_gic_dump, gic, show gic status);
+#endif
+
+int arm_gic_dist_init(rt_uint32_t index, rt_uint32_t dist_base, int irq_start)
+{
+    unsigned int gic_type, i;
+    rt_uint32_t cpumask = 1 << 0;
+
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    _gic_table[index].dist_hw_base = dist_base;
+    _gic_table[index].offset = irq_start;
+
+    /* Find out how many interrupts are supported. */
+    gic_type = GIC_DIST_TYPE(dist_base);
+    _gic_max_irq = ((gic_type & 0x1f) + 1) * 32;
+
+    /*
+     * The GIC only supports up to 1020 interrupt sources.
+     * Limit this to either the architected maximum, or the
+     * platform maximum.
+     */
+    if (_gic_max_irq > 1020)
+        _gic_max_irq = 1020;
+    if (_gic_max_irq > ARM_GIC_NR_IRQS) /* the platform maximum interrupts */
+        _gic_max_irq = ARM_GIC_NR_IRQS;
+
+    cpumask |= cpumask << 8;
+    cpumask |= cpumask << 16;
+    cpumask |= cpumask << 24;
+
+    GIC_DIST_CTRL(dist_base) = 0x0;
+
+    /* Set all global interrupts to be level triggered, active low. */
+    for (i = 32; i < _gic_max_irq; i += 16)
+        GIC_DIST_CONFIG(dist_base, i) = 0x0;
+
+    /* Set all global interrupts to this CPU only. */
+    for (i = 32; i < _gic_max_irq; i += 4)
+        GIC_DIST_TARGET(dist_base, i) = cpumask;
+
+    /* Set priority on all interrupts. */
+    for (i = 0; i < _gic_max_irq; i += 4)
+        GIC_DIST_PRI(dist_base, i) = 0xa0a0a0a0;
+
+    /* Disable all interrupts. */
+    for (i = 0; i < _gic_max_irq; i += 32)
+        GIC_DIST_ENABLE_CLEAR(dist_base, i) = 0xffffffff;
+
+#if 0
+    /* All interrupts defaults to IGROUP1(IRQ). */
+    for (i = 0; i < _gic_max_irq; i += 32)
+        GIC_DIST_IGROUP(dist_base, i) = 0xffffffff;
+#endif
+    for (i = 0; i < _gic_max_irq; i += 32)
+        GIC_DIST_IGROUP(dist_base, i) = 0;
+
+    /* Enable group0 and group1 interrupt forwarding. */
+    GIC_DIST_CTRL(dist_base) = 0x01;
+
+    return 0;
+}
+
+int arm_gic_cpu_init(rt_uint32_t index, rt_uint32_t cpu_base)
+{
+    RT_ASSERT(index < ARM_GIC_MAX_NR);
+
+    _gic_table[index].cpu_hw_base = cpu_base;
+
+    GIC_CPU_PRIMASK(cpu_base) = 0xf0;
+    GIC_CPU_BINPOINT(cpu_base) = 0x7;
+    /* Enable CPU interrupt */
+    GIC_CPU_CTRL(cpu_base) = 0x01;
+
+    return 0;
+}
+
+void arm_gic_set_group(rt_uint32_t index, int vector, int group)
+{
+    /* As for GICv2, there are only group0 and group1. */
+    RT_ASSERT(group <= 1);
+    RT_ASSERT(vector < _gic_max_irq);
+
+    if (group == 0)
+    {
+        GIC_DIST_IGROUP(_gic_table[index].dist_hw_base,
+                        vector) &= ~(1 << (vector % 32));
+    }
+    else if (group == 1)
+    {
+        GIC_DIST_IGROUP(_gic_table[index].dist_hw_base,
+                        vector) |=  (1 << (vector % 32));
+    }
+}
+
+#ifdef RT_USING_SMP
+void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask)
+ {
+     /* note: ipi_vector maybe different with irq_vector */
+     GIC_DIST_SOFTINT(_gic_table[0].dist_hw_base) = (cpu_mask << 16) | ipi_vector;
+}
+#endif
+
+#ifdef RT_USING_SMP
+void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler)
+{
+    /* note: ipi_vector maybe different with irq_vector */
+    rt_hw_interrupt_install(ipi_vector, ipi_isr_handler, 0, "IPI_HANDLER");
+}
+#endif
diff --git a/libcpu/arm/cortex-a/gic.h b/libcpu/arm/cortex-a/gic.h
new file mode 100644
index 0000000000..6d86dd31e0
--- /dev/null
+++ b/libcpu/arm/cortex-a/gic.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2013-07-20     Bernard      first version
+ */
+
+#ifndef __GIC_H__
+#define __GIC_H__
+
+#include <rthw.h>
+#include <platform.h>
+
+int arm_gic_dist_init(rt_uint32_t index, rt_uint32_t dist_base, int irq_start);
+int arm_gic_cpu_init(rt_uint32_t index, rt_uint32_t cpu_base);
+
+void arm_gic_mask(rt_uint32_t index, int irq);
+void arm_gic_umask(rt_uint32_t index, int irq);
+void arm_gic_set_cpu(rt_uint32_t index, int irq, unsigned int cpumask);
+void arm_gic_set_group(rt_uint32_t index, int vector, int group);
+
+int arm_gic_get_active_irq(rt_uint32_t index);
+void arm_gic_ack(rt_uint32_t index, int irq);
+
+void arm_gic_clear_active(rt_uint32_t index, int irq);
+void arm_gic_clear_pending(rt_uint32_t index, int irq);
+
+void arm_gic_dump_type(rt_uint32_t index);
+void arm_gic_dump(rt_uint32_t index);
+
+#endif
+
diff --git a/libcpu/arm/cortex-a/interrupt.c b/libcpu/arm/cortex-a/interrupt.c
index fb6e3ff3e0..b7a70719a9 100644
--- a/libcpu/arm/cortex-a/interrupt.c
+++ b/libcpu/arm/cortex-a/interrupt.c
@@ -6,68 +6,32 @@
  * Change Logs:
  * Date           Author       Notes
  * 2013-07-06     Bernard      first version
- * 2014-04-03     Grissiom     port to VMM
+ * 2018-11-22     Jesven       add smp support
  */
 
 #include <rthw.h>
 #include <rtthread.h>
+#include "interrupt.h"
+#include "gic.h"
 
-#include <irq_numbers.h>
-#include <interrupt.h>
-
-#include <gic.h>
-#include "cp15.h"
-
-#define MAX_HANDLERS                IMX_INTERRUPT_COUNT
-
-extern volatile rt_uint8_t rt_interrupt_nest;
 
 /* exception and interrupt handler table */
 struct rt_irq_desc isr_table[MAX_HANDLERS];
 
-rt_uint32_t rt_interrupt_from_thread;
-rt_uint32_t rt_interrupt_to_thread;
-rt_uint32_t rt_thread_switch_interrupt_flag;
+#ifndef RT_USING_SMP
+/* Those varibles will be accessed in ISR, so we need to share them. */
+rt_uint32_t rt_interrupt_from_thread        = 0;
+rt_uint32_t rt_interrupt_to_thread          = 0;
+rt_uint32_t rt_thread_switch_interrupt_flag = 0;
+#endif
 
+const unsigned int VECTOR_BASE = 0x00;
 extern void rt_cpu_vector_set_base(unsigned int addr);
 extern int system_vectors;
 
-/* keep compatible with platform SDK */
-void register_interrupt_routine(uint32_t irq_id, irq_hdlr_t isr)
-{
-    rt_hw_interrupt_install(irq_id, (rt_isr_handler_t)isr, NULL, "unknown");
-}
-
-void enable_interrupt(uint32_t irq_id, uint32_t cpu_id, uint32_t priority)
+void rt_hw_vector_init(void)
 {
-    gic_set_irq_priority(irq_id, priority);
-    gic_set_irq_security(irq_id, false);    // set IRQ as non-secure
-    gic_set_cpu_target(irq_id, cpu_id, true);
-    gic_enable_irq(irq_id, true);
-}
-
-void disable_interrupt(uint32_t irq_id, uint32_t cpu_id)
-{
-    gic_enable_irq(irq_id, false);
-    gic_set_cpu_target(irq_id, cpu_id, false);
-}
-
-static void rt_hw_vector_init(void)
-{
-    int sctrl;
-    unsigned int *src = (unsigned int *)&system_vectors;
-
-    /* C12-C0 is only active when SCTLR.V = 0 */
-    asm volatile ("mrc p15, #0, %0, c1, c0, #0"
-                  :"=r" (sctrl));
-    sctrl &= ~(1 << 13);
-    asm volatile ("mcr p15, #0, %0, c1, c0, #0"
-                  :
-                  :"r" (sctrl));
-
-    asm volatile ("mcr p15, #0, %0, c12, c0, #0"
-                  :
-                  :"r" (src));
+    rt_cpu_vector_set_base((unsigned int)&system_vectors);
 }
 
 /**
@@ -75,14 +39,24 @@ static void rt_hw_vector_init(void)
  */
 void rt_hw_interrupt_init(void)
 {
+    rt_uint32_t gic_cpu_base;
+    rt_uint32_t gic_dist_base;
+    rt_uint32_t gic_irq_start;
+
+    /* initialize vector table */
     rt_hw_vector_init();
-    gic_init();
 
-    /* init interrupt nest, and context in thread sp */
-    rt_interrupt_nest = 0;
-    rt_interrupt_from_thread = 0;
-    rt_interrupt_to_thread = 0;
-    rt_thread_switch_interrupt_flag = 0;
+    /* initialize exceptions table */
+    rt_memset(isr_table, 0x00, sizeof(isr_table));
+
+    /* initialize ARM GIC */
+    gic_dist_base = platform_get_gic_dist_base();
+    gic_cpu_base = platform_get_gic_cpu_base();
+
+    gic_irq_start = GIC_IRQ_START;
+
+    arm_gic_dist_init(0, gic_dist_base, gic_irq_start);
+    arm_gic_cpu_init(0, gic_cpu_base);
 }
 
 /**
@@ -91,7 +65,7 @@ void rt_hw_interrupt_init(void)
  */
 void rt_hw_interrupt_mask(int vector)
 {
-    disable_interrupt(vector, 0);
+    arm_gic_mask(0, vector);
 }
 
 /**
@@ -100,10 +74,27 @@ void rt_hw_interrupt_mask(int vector)
  */
 void rt_hw_interrupt_umask(int vector)
 {
-    enable_interrupt(vector, 0, 0);
+    arm_gic_umask(0, vector);
 }
 
 /**
+ * This function returns the active interrupt number.
+ * @param none
+ */
+int rt_hw_interrupt_get_irq(void)
+{
+    return arm_gic_get_active_irq(0) & GIC_ACK_INTID_MASK;
+}
+
+/**
+ * This function acknowledges the interrupt.
+ * @param vector the interrupt number
+ */
+void rt_hw_interrupt_ack(int vector)
+{
+    arm_gic_ack(0, vector);
+}
+/**
  * This function will install a interrupt service routine to a interrupt.
  * @param vector the interrupt number
  * @param new_handler the interrupt service routine to be installed
@@ -126,23 +117,7 @@ rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler,
             isr_table[vector].handler = handler;
             isr_table[vector].param = param;
         }
-        // arm_gic_set_cpu(0, vector, 1 << rt_cpu_get_smp_id());
     }
 
     return old_handler;
 }
-
-/**
- * Trigger a software IRQ
- *
- * Since we are running in single core, the target CPU are always CPU0.
- */
-void rt_hw_interrupt_trigger(int vector)
-{
-    // arm_gic_trigger(0, 1, vector);
-}
-
-void rt_hw_interrupt_clear(int vector)
-{
-    gic_write_end_of_irq(vector);
-}
diff --git a/libcpu/arm/cortex-a/interrupt.h b/libcpu/arm/cortex-a/interrupt.h
new file mode 100644
index 0000000000..bff29d402d
--- /dev/null
+++ b/libcpu/arm/cortex-a/interrupt.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2013-07-06     Bernard      first version
+ */
+
+#ifndef __INTERRUPT_H__
+#define __INTERRUPT_H__
+
+#include <rthw.h>
+#include <platform.h>
+
+#define INT_IRQ     0x00
+#define INT_FIQ     0x01
+
+void rt_hw_interrupt_control(int vector, int priority, int route);
+
+void rt_hw_interrupt_init(void);
+void rt_hw_interrupt_mask(int vector);
+void rt_hw_interrupt_umask(int vector);
+
+int rt_hw_interrupt_get_irq(void);
+void rt_hw_interrupt_ack(int vector);
+
+rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler,
+        void *param, const char *name);
+
+#endif
diff --git a/libcpu/arm/cortex-a/mmu.c b/libcpu/arm/cortex-a/mmu.c
index 58acdd8145..1260f41fe5 100644
--- a/libcpu/arm/cortex-a/mmu.c
+++ b/libcpu/arm/cortex-a/mmu.c
@@ -13,35 +13,7 @@
 #include <board.h>
 
 #include "cp15.h"
-
-#define DESC_SEC       (0x2)
-#define CB             (3<<2)  //cache_on, write_back
-#define CNB            (2<<2)  //cache_on, write_through
-#define NCB            (1<<2)  //cache_off,WR_BUF on
-#define NCNB           (0<<2)  //cache_off,WR_BUF off
-#define AP_RW          (3<<10) //supervisor=RW, user=RW
-#define AP_RO          (2<<10) //supervisor=RW, user=RO
-#define XN             (1<<4)  // eXecute Never
-
-#define DOMAIN_FAULT   (0x0)
-#define DOMAIN_CHK     (0x1)
-#define DOMAIN_NOTCHK  (0x3)
-#define DOMAIN0        (0x0<<5)
-#define DOMAIN1        (0x1<<5)
-
-#define DOMAIN0_ATTR   (DOMAIN_CHK<<0)
-#define DOMAIN1_ATTR   (DOMAIN_FAULT<<2)
-
-/* Read/Write, cache, write back */
-#define RW_CB          (AP_RW|DOMAIN0|CB|DESC_SEC)
-/* Read/Write, cache, write through */
-#define RW_CNB         (AP_RW|DOMAIN0|CNB|DESC_SEC)
-/* Read/Write without cache and write buffer */
-#define RW_NCNB        (AP_RW|DOMAIN0|NCNB|DESC_SEC)
-/* Read/Write without cache and write buffer, no execute */
-#define RW_NCNBXN      (AP_RW|DOMAIN0|NCNB|DESC_SEC|XN)
-/* Read/Write without cache and write buffer */
-#define RW_FAULT       (AP_RW|DOMAIN1|NCNB|DESC_SEC)
+#include "mmu.h"
 
 /* dump 2nd level page table */
 void rt_hw_cpu_dump_page_table_2nd(rt_uint32_t *ptb)
@@ -178,18 +150,25 @@ unsigned long rt_hw_set_domain_register(unsigned long domain_val)
     return old_domain;
 }
 
+void rt_hw_init_mmu_table(struct mem_desc *mdesc, rt_uint32_t size)
+{
+    /* set page table */
+    for(; size > 0; size--)
+    {
+        rt_hw_mmu_setmtt(mdesc->vaddr_start, mdesc->vaddr_end,
+                mdesc->paddr_start, mdesc->attr);
+        mdesc++;
+    }
+}
+
 void rt_hw_mmu_init(void)
 {
+    rt_cpu_dcache_clean_flush();
+    rt_cpu_icache_flush();
     rt_hw_cpu_dcache_disable();
     rt_hw_cpu_icache_disable();
     rt_cpu_mmu_disable();
 
-    /* set page table */
-    /* 4G 1:1 memory */
-    rt_hw_mmu_setmtt(0, 0xffffffff-1, 0, RW_CB);
-    /* IO memory region */
-    rt_hw_mmu_setmtt(0x44000000, 0x80000000-1, 0x44000000, RW_NCNBXN);
-
     /*rt_hw_cpu_dump_page_table(MMUTable);*/
     rt_hw_set_domain_register(0x55555555);
 
diff --git a/libcpu/arm/cortex-a/mmu.h b/libcpu/arm/cortex-a/mmu.h
new file mode 100644
index 0000000000..deec690c69
--- /dev/null
+++ b/libcpu/arm/cortex-a/mmu.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-03-25     quanzhao     the first version
+ */
+#ifndef __MMU_H_
+#define __MMU_H_
+
+#include <rtthread.h>
+
+#define DESC_SEC       (0x2)
+#define MEMWB          (3<<2)  /* write back, no write allocate */
+#define MEMWT          (2<<2)  /* write through, no write allocate */
+#define SHAREDEVICE    (1<<2)  /* shared device */
+#define STRONGORDER    (0<<2)  /* strong ordered */
+#define XN             (1<<4)  /* eXecute Never */
+#define AP_RW          (3<<10) /* supervisor=RW, user=RW */
+#define AP_RO          (2<<10) /* supervisor=RW, user=RO */
+#define SHARED         (1<<16) /* shareable */
+
+#define DOMAIN_FAULT   (0x0)
+#define DOMAIN_CHK     (0x1)
+#define DOMAIN_NOTCHK  (0x3)
+#define DOMAIN0        (0x0<<5)
+#define DOMAIN1        (0x1<<5)
+
+#define DOMAIN0_ATTR   (DOMAIN_CHK<<0)
+#define DOMAIN1_ATTR   (DOMAIN_FAULT<<2)
+
+/* device mapping type */
+#define DEVICE_MEM     (SHARED|AP_RW|DOMAIN0|SHAREDEVICE|DESC_SEC|XN)
+/* normal memory mapping type */
+#define NORMAL_MEM     (SHARED|AP_RW|DOMAIN0|MEMWB|DESC_SEC)
+
+struct mem_desc
+{
+    rt_uint32_t vaddr_start;
+    rt_uint32_t vaddr_end;
+    rt_uint32_t paddr_start;
+    rt_uint32_t attr;
+};
+
+
+#endif
diff --git a/libcpu/arm/cortex-a/stack.c b/libcpu/arm/cortex-a/stack.c
index 1d3ebcb9e6..d76b8a428a 100644
--- a/libcpu/arm/cortex-a/stack.c
+++ b/libcpu/arm/cortex-a/stack.c
@@ -50,13 +50,17 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
     *(--stk) = 0xdeadbeef;                  /* r2 */
     *(--stk) = 0xdeadbeef;                  /* r1 */
     *(--stk) = (rt_uint32_t)parameter;      /* r0 : argument */
-
     /* cpsr */
     if ((rt_uint32_t)tentry & 0x01)
         *(--stk) = SVCMODE | 0x20;          /* thumb mode */
     else
         *(--stk) = SVCMODE;                 /* arm mode   */
 
+#ifdef RT_USING_LWP
+    *(--stk) = 0;       /* user lr */
+    *(--stk) = 0;       /* user sp*/
+#endif
+
     /* return task's current stack address */
     return (rt_uint8_t *)stk;
 }
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 2f16d16807..0f4ff58364 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -6,8 +6,12 @@
  * Change Logs:
  * Date           Author       Notes
  * 2013-07-05     Bernard      the first version
+ * 2018-11-22     Jesven       in the interrupt context, use rt_scheduler_do_irq_switch checks
+ *                             and switches to a new thread
  */
 
+#include "rtconfig.h"
+
 .equ Mode_USR,        0x10
 .equ Mode_FIQ,        0x11
 .equ Mode_IRQ,        0x12
@@ -20,11 +24,11 @@
 .equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
 
 .equ UND_Stack_Size,     0x00000000
-.equ SVC_Stack_Size,     0x00000100
+.equ SVC_Stack_Size,     0x00000400
 .equ ABT_Stack_Size,     0x00000000
 .equ RT_FIQ_STACK_PGSZ,  0x00000000
-.equ RT_IRQ_STACK_PGSZ,  0x00000100
-.equ USR_Stack_Size,     0x00000100
+.equ RT_IRQ_STACK_PGSZ,  0x00000800
+.equ USR_Stack_Size,     0x00000400
 
 #define ISR_Stack_Size  (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + \
                  RT_FIQ_STACK_PGSZ + RT_IRQ_STACK_PGSZ)
@@ -44,12 +48,8 @@ stack_top:
 /* reset entry */
 .globl _reset
 _reset:
-    bl      rt_cpu_mmu_disable
     /* set the cpu to SVC32 mode and disable interrupt */
-    mrs     r0, cpsr
-    bic     r0, r0, #0x1f
-    orr     r0, r0, #0x13
-    msr     cpsr_c, r0
+    cps #Mode_SVC
 
     /* setup stack */
     bl      stack_setup
@@ -64,6 +64,20 @@ bss_loop:
     strlo   r0,[r1],#4              /* clear 4 bytes                    */
     blo     bss_loop                /* loop until done                  */
 
+#ifdef RT_USING_SMP
+    mrc p15, 0, r1, c1, c0, 1
+    mov r0, #(1<<6)
+    orr r1, r0
+    mcr p15, 0, r1, c1, c0, 1 //enable smp
+#endif
+
+    /* initialize the mmu table and enable mmu */
+    ldr r0, =platform_mem_desc
+    ldr r1, =platform_mem_desc_size
+    ldr r1, [r1]
+    bl rt_hw_init_mmu_table
+    bl rt_hw_mmu_init
+
     /* call C++ constructors of global objects                          */
     ldr     r0, =__ctors_start__
     ldr     r1, =__ctors_end__
@@ -137,12 +151,22 @@ vector_fiq:
     .align  5
 .globl vector_irq
 vector_irq:
+#ifdef RT_USING_SMP
+    clrex
+#endif
     stmfd   sp!, {r0-r12,lr}
 
     bl      rt_interrupt_enter
     bl      rt_hw_trap_irq
     bl      rt_interrupt_leave
 
+#ifdef RT_USING_SMP
+    mov     r0, sp
+    bl      rt_scheduler_do_irq_switch
+
+    ldmfd   sp!, {r0-r12,lr}
+    subs    pc,  lr, #4
+#else
     @ if rt_thread_switch_interrupt_flag set, jump to
     @ rt_hw_context_switch_interrupt_do and don't return
     ldr     r0, =rt_thread_switch_interrupt_flag
@@ -174,6 +198,11 @@ rt_hw_context_switch_interrupt_do:
     stmfd   sp!, {r1-r4}    @ push old task's r0-r3
     stmfd   sp!, {r0}       @ push old task's cpsr
 
+#ifdef RT_USING_LWP
+    stmfd sp, {r13, r14}^  @push usr_sp, usr_lr
+    sub sp, #8
+#endif
+
     ldr     r4,  =rt_interrupt_from_thread
     ldr     r5,  [r4]
     str     sp,  [r5]       @ store sp in preempted tasks's TCB
@@ -182,11 +211,18 @@ rt_hw_context_switch_interrupt_do:
     ldr     r6,  [r6]
     ldr     sp,  [r6]       @ get new task's stack pointer
 
+#ifdef RT_USING_LWP
+    ldmfd sp, {r13, r14}^  @pop usr_sp, usr_lr
+    add sp, #8
+#endif
+
     ldmfd   sp!, {r4}       @ pop new task's cpsr to spsr
     msr     spsr_cxsf, r4
 
     ldmfd   sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
 
+#endif
+
 .macro push_svc_reg
     sub     sp, sp, #17 * 4         @/* Sizeof(struct rt_hw_exp_stack)  */
     stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
@@ -200,36 +236,86 @@ rt_hw_context_switch_interrupt_do:
 .endm
 
     .align  5
-    .globl	vector_swi
+    .globl  vector_swi
+.weak SVC_Handler
+SVC_Handler:
 vector_swi:
     push_svc_reg
     bl      rt_hw_trap_swi
     b       .
 
     .align  5
-    .globl	vector_undef
+    .globl  vector_undef
 vector_undef:
     push_svc_reg
     bl      rt_hw_trap_undef
     b       .
 
     .align  5
-    .globl	vector_pabt
+    .globl  vector_pabt
 vector_pabt:
     push_svc_reg
     bl      rt_hw_trap_pabt
     b       .
 
     .align  5
-    .globl	vector_dabt
+    .globl  vector_dabt
 vector_dabt:
     push_svc_reg
     bl      rt_hw_trap_dabt
     b       .
 
     .align  5
-    .globl	vector_resv
+    .globl  vector_resv
 vector_resv:
     push_svc_reg
     bl      rt_hw_trap_resv
     b       .
+
+#ifdef RT_USING_SMP
+.global set_secondary_cpu_boot_address
+set_secondary_cpu_boot_address:
+    ldr r0, =secondary_cpu_start
+
+    mvn r1, #0 //0xffffffff
+    ldr r2, =0x10000034
+    str r1, [r2]
+    str r0, [r2, #-4]
+    mov pc, lr
+
+.global secondary_cpu_start
+secondary_cpu_start:
+    mrc p15, 0, r1, c1, c0, 1
+    mov r0, #(1<<6)
+    orr r1, r0
+    mcr p15, 0, r1, c1, c0, 1 //enable smp
+
+    mrc p15, 0, r0, c1, c0, 0
+    bic r0, #(1<<13)
+    mcr p15, 0, r0, c1, c0, 0
+
+    cps #Mode_IRQ
+    ldr sp, =irq_stack_2_limit
+
+    cps #Mode_FIQ
+    ldr sp, =irq_stack_2_limit
+
+    cps #Mode_SVC
+    ldr sp, =svc_stack_2_limit
+
+    /* initialize the mmu table and enable mmu */
+    bl rt_hw_mmu_init
+
+    b secondary_cpu_c_start
+#endif
+
+.bss
+.align 2   //align to  2~2=4
+svc_stack_2:
+    .space (1 << 10)
+svc_stack_2_limit:
+
+irq_stack_2:
+    .space (1 << 10)
+irq_stack_2_limit:
+
diff --git a/libcpu/arm/cortex-a/trap.c b/libcpu/arm/cortex-a/trap.c
index f3b2c26e6d..54628933ff 100644
--- a/libcpu/arm/cortex-a/trap.c
+++ b/libcpu/arm/cortex-a/trap.c
@@ -10,13 +10,11 @@
 
 #include <rtthread.h>
 #include <rthw.h>
-#include <board.h>
+#include <platform.h>
 
 #include "armv7.h"
+#include "interrupt.h"
 
-#include "gic.h"
-
-extern struct rt_thread *rt_current_thread;
 #ifdef RT_USING_FINSH
 extern long list_thread(void);
 #endif
@@ -130,48 +128,52 @@ void rt_hw_trap_resv(struct rt_hw_exp_stack *regs)
 void rt_hw_trap_irq(void)
 {
     void *param;
+    int ir;
     rt_isr_handler_t isr_func;
     extern struct rt_irq_desc isr_table[];
 
-    // vectNum = RESERVED[31:13] | CPUID[12:10] | INTERRUPT_ID[9:0]
-    // send ack and get ID source
-    uint32_t vectNum = gic_read_irq_ack();
+    ir = rt_hw_interrupt_get_irq();
 
-    // Check that INT_ID isn't 1023 or 1022 (spurious interrupt)
-    if (vectNum & 0x0200)
+    if (ir == 1023)
     {
-        gic_write_end_of_irq(vectNum);  // send end of irq
+        /* Spurious interrupt */
+        return;
     }
-    else
-    {
-        // copy the local value to the global image of CPUID
-        unsigned cpu = (vectNum >> 10) & 0x7;
-        unsigned irq = vectNum & 0x1FF;
 
-        /* skip warning */
-        cpu = cpu;
-
-        // Call the service routine stored in the handlers array. If there isn't
-        // one for this IRQ, then call the default handler.
-        /* get interrupt service routine */
-        isr_func = isr_table[irq].handler;
+    /* get interrupt service routine */
+    isr_func = isr_table[ir].handler;
 #ifdef RT_USING_INTERRUPT_INFO
-        isr_table[irq].counter++;
+    isr_table[ir].counter++;
 #endif
-        if (isr_func)
-        {
-            /* Interrupt for myself. */
-            param = isr_table[irq].param;
-            /* turn to interrupt service routine */
-            isr_func(irq, param);
-        }
-
-        // Signal the end of the irq.
-        gic_write_end_of_irq(vectNum);
+    if (isr_func)
+    {
+        /* Interrupt for myself. */
+        param = isr_table[ir].param;
+        /* turn to interrupt service routine */
+        isr_func(ir, param);
     }
+
+    /* end of interrupt */
+    rt_hw_interrupt_ack(ir);
 }
 
 void rt_hw_trap_fiq(void)
 {
-    /* TODO */
+    void *param;
+    int ir;
+    rt_isr_handler_t isr_func;
+    extern struct rt_irq_desc isr_table[];
+
+    ir = rt_hw_interrupt_get_irq();
+
+    /* get interrupt service routine */
+    isr_func = isr_table[ir].handler;
+    param = isr_table[ir].param;
+
+    /* turn to interrupt service routine */
+    isr_func(ir, param);
+
+    /* end of interrupt */
+    rt_hw_interrupt_ack(ir);
 }
+