[BSP] Add fh8620 bsp from Shanghai Fullhan Microelectronics Co., Ltd.

FH8620 BSP
Copyright (c) 2016 Shanghai Fullhan Microelectronics Co., Ltd.
All rights reserved

10 files changed, 1981 insertions, 0 deletions

diff --git a/libcpu/arm/armv6/arm_entry_gcc.S b/libcpu/arm/armv6/arm_entry_gcc.S
new file mode 100644
index 0000000000..f20a384e84
--- /dev/null
+++ b/libcpu/arm/armv6/arm_entry_gcc.S
@@ -0,0 +1,145 @@
+/*
+ * File      : arm_entry_gcc.S
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2014-11-07     weety    first version
+ */
+
+#include <rtconfig.h>
+
+#include "armv6.h"
+
+//#define DEBUG
+
+.macro	PRINT, str
+#ifdef DEBUG
+	stmfd	sp!, {r0-r3, ip, lr}
+	add	r0, pc, #4
+	bl	rt_kprintf
+	b	1f
+	.asciz  "UNDEF: \str\n"
+	.balign 4
+1:	ldmfd	sp!, {r0-r3, ip, lr}
+#endif
+	.endm
+
+.macro  PRINT1, str, arg
+#ifdef DEBUG
+	stmfd	sp!, {r0-r3, ip, lr}
+	mov	r1, \arg
+	add	r0, pc, #4
+	bl	rt_kprintf
+	b	1f
+	.asciz  "UNDEF: \str\n"
+	.balign 4
+1:	ldmfd	sp!, {r0-r3, ip, lr}
+#endif
+	.endm
+
+.macro  PRINT3, str, arg1, arg2, arg3
+#ifdef DEBUG
+	stmfd	sp!, {r0-r3, ip, lr}
+	mov	r3, \arg3
+	mov	r2, \arg2
+	mov	r1, \arg1
+	add	r0, pc, #4
+	bl	rt_kprintf
+	b	1f
+	.asciz  "UNDEF: \str\n"
+	.balign 4
+1:	ldmfd	sp!, {r0-r3, ip, lr}
+#endif
+	.endm
+
+.macro	get_current_thread, rd
+	ldr	\rd, .current_thread
+	ldr	\rd, [\rd]
+	.endm
+
+.current_thread:
+	.word	rt_current_thread
+
+#ifdef RT_USING_NEON
+	.align	6
+
+/* is the neon instuction on arm mode? */
+.neon_opcode:
+	.word	0xfe000000			@ mask
+	.word	0xf2000000			@ opcode
+
+	.word	0xff100000			@ mask
+	.word	0xf4000000			@ opcode
+
+	.word	0x00000000			@ end mask
+	.word	0x00000000			@ end opcode
+#endif
+
+/* undefined instruction exception processing */
+.globl undef_entry
+undef_entry:
+	PRINT1 "r0=0x%08x", r0
+	PRINT1 "r2=0x%08x", r2
+	PRINT1 "r9=0x%08x", r9
+	PRINT1 "sp=0x%08x", sp
+
+#ifdef RT_USING_NEON
+	ldr	r6, .neon_opcode
+__check_neon_instruction:
+	ldr	r7, [r6], #4		@ load mask value
+	cmp	r7, #0				@ end mask?
+	beq	__check_vfp_instruction
+	and	r8, r0, r7
+	ldr	r7, [r6], #4		@ load opcode value
+	cmp	r8, r7				@ is NEON instruction?
+	bne	__check_neon_instruction
+	b	vfp_entry
+__check_vfp_instruction:
+#endif
+	tst	r0, #0x08000000			@ only CDP/CPRT/LDC/STC instruction has bit 27
+	tstne	r0, #0x04000000		@ bit 26 set on both ARM and Thumb-2 instruction
+	moveq	pc, lr				@ no vfp coprocessor instruction, return
+	get_current_thread r10
+	and	r8, r0, #0x00000f00		@ get coprocessor number
+	PRINT1 "CP=0x%08x", r8
+	add	pc, pc, r8, lsr #6
+	nop
+	mov pc,	lr				@ CP0
+	mov pc,	lr				@ CP1
+	mov pc,	lr				@ CP2
+	mov pc,	lr				@ CP3
+	mov pc,	lr				@ CP4
+	mov pc,	lr				@ CP5
+	mov pc,	lr				@ CP6
+	mov pc,	lr				@ CP7
+	mov pc,	lr				@ CP8
+	mov pc,	lr				@ CP9
+#ifdef RT_USING_VFP
+	b	vfp_entry			@ CP10 VFP
+	b	vfp_entry			@ CP11 VFP
+#else
+	mov pc,	lr				@ CP10 VFP
+	mov pc,	lr				@ CP11 VFP
+#endif
+	mov pc,	lr				@ CP12
+	mov pc,	lr				@ CP13
+	mov pc,	lr				@ CP14 DEBUG
+	mov pc,	lr				@ CP15 SYS CONTROL
+
+
diff --git a/libcpu/arm/armv6/armv6.h b/libcpu/arm/armv6/armv6.h
new file mode 100644
index 0000000000..860f09661f
--- /dev/null
+++ b/libcpu/arm/armv6/armv6.h
@@ -0,0 +1,107 @@
+/*
+ * File      : armv6.h
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+ 
+#ifndef __ARMV6_H__
+#define __ARMV6_H__
+
+
+/*****************************/
+/* CPU Mode                  */
+/*****************************/
+#define USERMODE        0x10
+#define FIQMODE         0x11
+#define IRQMODE         0x12
+#define SVCMODE         0x13
+#define ABORTMODE       0x17
+#define UNDEFMODE       0x1b
+#define MODEMASK        0x1f
+#define NOINT           0xc0
+
+#ifndef __ASSEMBLY__
+struct rt_hw_register
+{
+	rt_uint32_t cpsr;
+    rt_uint32_t r0;
+    rt_uint32_t r1;
+    rt_uint32_t r2;
+    rt_uint32_t r3;
+    rt_uint32_t r4;
+    rt_uint32_t r5;
+    rt_uint32_t r6;
+    rt_uint32_t r7;
+    rt_uint32_t r8;
+    rt_uint32_t r9;
+    rt_uint32_t r10;
+    rt_uint32_t fp;
+    rt_uint32_t ip;
+    rt_uint32_t sp;
+    rt_uint32_t lr;
+    rt_uint32_t pc;
+};
+#if(0)
+struct rt_hw_register{
+	rt_uint32_t r0;
+	rt_uint32_t r1;
+	rt_uint32_t r2;
+	rt_uint32_t r3;
+	rt_uint32_t r4;
+	rt_uint32_t r5;
+	rt_uint32_t r6;
+	rt_uint32_t r7;
+	rt_uint32_t r8;
+	rt_uint32_t r9;
+	rt_uint32_t r10;
+	rt_uint32_t fp;
+	rt_uint32_t ip;
+	rt_uint32_t sp;
+	rt_uint32_t lr;
+	rt_uint32_t pc;
+	rt_uint32_t cpsr;
+	rt_uint32_t ORIG_r0;
+};
+#endif
+#endif
+
+/* rt_hw_register offset */
+#define S_FRAME_SIZE        68
+
+#define S_PC                64
+#define S_LR                60
+#define S_SP                56
+#define S_IP                52
+#define S_FP                48
+#define S_R10               44
+#define S_R9                40
+#define S_R8                36
+#define S_R7                32
+#define S_R6                28
+#define S_R5                24
+#define S_R4                20
+#define S_R3                16
+#define S_R2                12
+#define S_R1                8
+#define S_R0                4
+#define S_CPSR              0
+
+
+#endif
diff --git a/libcpu/arm/armv6/context_gcc.S b/libcpu/arm/armv6/context_gcc.S
new file mode 100644
index 0000000000..87ba43ae52
--- /dev/null
+++ b/libcpu/arm/armv6/context_gcc.S
@@ -0,0 +1,122 @@
+/*
+ * File      : context.S
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2011-01-13     weety    copy from mini2440
+ */
+
+/*!
+ * \addtogroup ARMv6
+ */
+/*@{*/
+
+#include <rtconfig.h>
+
+#define NOINT          0xc0
+#define FPEXC_EN       (1 << 30) /* VFP enable bit */
+
+/*
+ * rt_base_t rt_hw_interrupt_disable();
+ */
+.globl rt_hw_interrupt_disable
+rt_hw_interrupt_disable:
+    mrs r0, cpsr
+    cpsid if
+    bx  lr
+
+/*
+ * void rt_hw_interrupt_enable(rt_base_t level);
+ */
+.globl rt_hw_interrupt_enable
+rt_hw_interrupt_enable:
+    msr cpsr_c, r0
+    bx  lr
+
+/*
+ * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
+ * r0 --> from
+ * r1 --> to
+ */
+.globl rt_hw_context_switch
+rt_hw_context_switch:
+    stmfd   sp!, {lr}       @ push pc (lr should be pushed in place of PC)
+    stmfd   sp!, {r0-r12, lr}   @ push lr & register file
+
+    mrs r4, cpsr
+    tst lr, #0x01
+    orrne r4, r4, #0x20     @ it's thumb code
+
+    stmfd sp!, {r4}         @ push cpsr
+
+    str sp, [r0]            @ store sp in preempted tasks TCB
+    ldr sp, [r1]            @ get new task stack pointer
+
+    ldmfd sp!, {r4}         @ pop new task cpsr to spsr
+    msr spsr_cxsf, r4
+#ifdef RT_USING_VFP
+    vmrs    r0, fpexc
+    bic     r0, r0, #FPEXC_EN    @ always disable VFP so we can 
+                                 @ lazily save/restore the old state.
+    vmsr    fpexc, r0
+#endif
+_do_switch:
+    ldmfd sp!, {r0-r12, lr, pc}^  @ pop new task r0-r12, lr & pc, copy spsr to cpsr
+
+/*
+ * void rt_hw_context_switch_to(rt_uint32 to);
+ * r0 --> to
+ */
+.globl rt_hw_context_switch_to
+rt_hw_context_switch_to:
+    ldr sp, [r0]            @ get new task stack pointer
+
+    ldmfd sp!, {r4}         @ pop new task spsr
+    msr spsr_cxsf, r4
+
+    bic r4, r4, #0x20       @ must be ARM mode
+    msr cpsr_cxsf, r4
+#ifdef RT_USING_VFP
+    vmrs    r0, fpexc
+    bic     r0, r0, #FPEXC_EN    @ always disable VFP so we can 
+                                 @ lazily save/restore the old state.
+    vmsr    fpexc, r0
+#endif
+    ldmfd sp!, {r0-r12, lr, pc}^   @ pop new task r0-r12, lr & pc
+
+/*
+ * void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to);
+ */
+.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:
+    ldr r2, =rt_thread_switch_interrupt_flag
+    ldr r3, [r2]
+    cmp r3, #1
+    beq _reswitch
+    mov r3, #1              @ set rt_thread_switch_interrupt_flag to 1
+    str r3, [r2]
+    ldr r2, =rt_interrupt_from_thread   @ set rt_interrupt_from_thread
+    str r0, [r2]
+_reswitch:
+    ldr r2, =rt_interrupt_to_thread     @ set rt_interrupt_to_thread
+    str r1, [r2]
+    bx  lr
diff --git a/libcpu/arm/armv6/cpuport.c b/libcpu/arm/armv6/cpuport.c
new file mode 100644
index 0000000000..a0578a6d69
--- /dev/null
+++ b/libcpu/arm/armv6/cpuport.c
@@ -0,0 +1,248 @@
+/*
+ * File      : cpu.c
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Develop Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2011-01-13     weety      modified from mini2440
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+
+#define ICACHE_MASK	(rt_uint32_t)(1 << 12)
+#define DCACHE_MASK	(rt_uint32_t)(1 << 2)
+
+extern void machine_reset(void);
+extern void machine_shutdown(void);
+
+#ifdef __GNUC__
+rt_inline rt_uint32_t cp15_rd(void)
+{
+	rt_uint32_t i;
+
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+	return i;
+}
+
+rt_inline void cache_enable(rt_uint32_t bit)
+{
+	__asm__ __volatile__(			\
+		"mrc  p15,0,r0,c1,c0,0\n\t"	\
+		"orr  r0,r0,%0\n\t"			\
+	   	"mcr  p15,0,r0,c1,c0,0"		\
+		:							\
+		:"r" (bit)					\
+		:"memory");
+}
+
+rt_inline void cache_disable(rt_uint32_t bit)
+{
+	__asm__ __volatile__(			\
+		"mrc  p15,0,r0,c1,c0,0\n\t"	\
+		"bic  r0,r0,%0\n\t"			\
+		"mcr  p15,0,r0,c1,c0,0"		\
+		:							\
+		:"r" (bit)					\
+		:"memory");
+}
+
+
+#endif
+
+#ifdef __CC_ARM
+rt_inline rt_uint32_t cp15_rd(void)
+{
+	rt_uint32_t i;
+
+	__asm
+	{
+		mrc p15, 0, i, c1, c0, 0
+	}
+
+	return i;
+}
+
+rt_inline void cache_enable(rt_uint32_t bit)
+{
+	rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+		orr value, value, bit
+		mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+rt_inline void cache_disable(rt_uint32_t bit)
+{
+	rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+		bic value, value, bit
+		mcr p15, 0, value, c1, c0, 0
+	}
+}
+#endif
+
+/**
+ * enable I-Cache
+ *
+ */
+void rt_hw_cpu_icache_enable()
+{
+	cache_enable(ICACHE_MASK);
+}
+
+/**
+ * disable I-Cache
+ *
+ */
+void rt_hw_cpu_icache_disable()
+{
+	cache_disable(ICACHE_MASK);
+}
+
+/**
+ * return the status of I-Cache
+ *
+ */
+rt_base_t rt_hw_cpu_icache_status()
+{
+	return (cp15_rd() & ICACHE_MASK);
+}
+
+/**
+ * enable D-Cache
+ *
+ */
+void rt_hw_cpu_dcache_enable()
+{
+	cache_enable(DCACHE_MASK);
+}
+
+/**
+ * disable D-Cache
+ *
+ */
+void rt_hw_cpu_dcache_disable()
+{
+	cache_disable(DCACHE_MASK);
+}
+
+/**
+ * return the status of D-Cache
+ *
+ */
+rt_base_t rt_hw_cpu_dcache_status()
+{
+	return (cp15_rd() & DCACHE_MASK);
+}
+
+/**
+ * reset cpu by dog's time-out
+ *
+ */
+void rt_hw_cpu_reset()
+{
+	
+	rt_kprintf("Restarting system...\n");
+	machine_reset();
+
+	while(1);	/* loop forever and wait for reset to happen */
+
+	/* NEVER REACHED */
+}
+
+/**
+ *  shutdown CPU
+ *
+ */
+void rt_hw_cpu_shutdown()
+{
+	rt_uint32_t level;
+	rt_kprintf("shutdown...\n");
+
+	level = rt_hw_interrupt_disable();
+	machine_shutdown();
+	while (level)
+	{
+		RT_ASSERT(0);
+	}
+}
+
+#ifdef RT_USING_CPU_FFS
+/**
+ * This function finds the first bit set (beginning with the least significant bit) 
+ * in value and return the index of that bit.
+ *
+ * Bits are numbered starting at 1 (the least significant bit).  A return value of 
+ * zero from any of these functions means that the argument was zero.
+ * 
+ * @return return the index of the first bit set. If value is 0, then this function 
+ * shall return 0.
+ */
+#if defined(__CC_ARM)
+int __rt_ffs(int value)
+{
+	register rt_uint32_t x;
+
+	if (value == 0)
+		return value;
+	
+	__asm
+	{
+		rsb x, value, #0
+		and x, x, value
+		clz x, x
+		rsb x, x, #32
+	}
+
+	return x;
+}
+#elif defined(__IAR_SYSTEMS_ICC__)
+int __rt_ffs(int value)
+{
+	if (value == 0)
+		return value;
+
+	__ASM("RSB  r4, r0, #0");
+	__ASM("AND  r4, r4, r0");
+	__ASM("CLZ  r4, r4");
+	__ASM("RSB  r0, r4, #32");
+}
+#elif defined(__GNUC__)
+int __rt_ffs(int value)
+{
+	if (value == 0)
+		return value;
+
+	value &= (-value);
+	asm ("clz %0, %1": "=r"(value) :"r"(value));
+
+	return (32 - value);
+}
+#endif
+
+#endif
+
+
+/*@}*/
diff --git a/libcpu/arm/armv6/mmu.c b/libcpu/arm/armv6/mmu.c
new file mode 100644
index 0000000000..9a6ebd4e6f
--- /dev/null
+++ b/libcpu/arm/armv6/mmu.c
@@ -0,0 +1,562 @@
+/*
+ * File      : mmu.c
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+
+#include "mmu.h"
+
+#ifdef __CC_ARM
+void mmu_setttbase(rt_uint32_t i)
+{
+	register rt_uint32_t value;
+
+	/* Invalidates all TLBs.Domain access is selected as
+	 * client by configuring domain access register,
+	 * in that case access controlled by permission value
+	 * set by page table entry
+	 */
+	value = 0;
+	__asm
+	{
+		mcr p15, 0, value, c8, c7, 0
+	}
+
+	value = 0x55555555;
+	__asm
+	{
+		mcr p15, 0, value, c3, c0, 0
+			mcr p15, 0, i, c2, c0, 0
+	}
+}
+
+void mmu_set_domain(rt_uint32_t i)
+{
+	__asm
+	{
+		mcr p15,0, i, c3, c0,  0
+	}
+}
+
+void mmu_enable()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			orr value, value, #0x01
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_disable()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			bic value, value, #0x01
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_enable_icache()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			orr value, value, #0x1000
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_enable_dcache()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			orr value, value, #0x04
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_disable_icache()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			bic value, value, #0x1000
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_disable_dcache()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			bic value, value, #0x04
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_enable_alignfault()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			orr value, value, #0x02
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_disable_alignfault()
+{
+	register rt_uint32_t value;
+
+	__asm
+	{
+		mrc p15, 0, value, c1, c0, 0
+			bic value, value, #0x02
+			mcr p15, 0, value, c1, c0, 0
+	}
+}
+
+void mmu_clean_invalidated_cache_index(int index)
+{
+	__asm
+	{
+		mcr p15, 0, index, c7, c14, 2
+	}
+}
+
+void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while(ptr < buffer + size)
+	{
+		__asm
+		{
+			MCR p15, 0, ptr, c7, c14, 1
+		}
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+void mmu_clean_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while (ptr < buffer + size)
+	{
+		__asm
+		{
+			MCR p15, 0, ptr, c7, c10, 1
+		}
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+void mmu_invalidate_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while (ptr < buffer + size)
+	{
+		__asm
+		{
+			MCR p15, 0, ptr, c7, c6, 1
+		}
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+void mmu_invalidate_tlb()
+{
+	register rt_uint32_t value;
+
+	value = 0;
+	__asm
+	{
+		mcr p15, 0, value, c8, c7, 0
+	}
+}
+
+void mmu_invalidate_icache()
+{
+	register rt_uint32_t value;
+
+	value = 0;
+
+	__asm
+	{
+		mcr p15, 0, value, c7, c5, 0
+	}
+}
+
+
+void mmu_invalidate_dcache_all()
+{
+	register rt_uint32_t value;
+
+	value = 0;
+
+	__asm
+	{
+		mcr p15, 0, value, c7, c6, 0
+	}
+}
+#elif defined(__GNUC__)
+void mmu_setttbase(register rt_uint32_t i)
+{
+	register rt_uint32_t value;
+
+	/* Invalidates all TLBs.Domain access is selected as
+	 * client by configuring domain access register,
+	 * in that case access controlled by permission value
+	 * set by page table entry
+	 */
+	value = 0;
+	asm ("mcr p15, 0, %0, c8, c7, 0"::"r"(value));
+
+	value = 0x55555555;
+	asm ("mcr p15, 0, %0, c3, c0, 0"::"r"(value));
+	asm ("mcr p15, 0, %0, c2, c0, 0"::"r"(i));
+}
+
+void mmu_set_domain(register rt_uint32_t i)
+{
+	asm ("mcr p15,0, %0, c3, c0,  0": :"r" (i));
+}
+
+void mmu_enable()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i |= 0x1;
+	/* Enables the extended page tables to be configured for
+	   the hardware page translation mechanism, Subpage AP bits disabled */
+	i |= (1 << 23); /* support for ARMv6 MMU features */
+	i |= (1 << 13); /* High exception vectors selected, address range = 0xFFFF0000-0xFFFF001C */
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_disable()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i &= ~0x1;
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_enable_icache()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i |= (1 << 12);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_enable_dcache()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i |= (1 << 2);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_disable_icache()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i &= ~(1 << 12);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_disable_dcache()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i &= ~(1 << 2);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_enable_alignfault()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i |= (1 << 1);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_disable_alignfault()
+{
+	register rt_uint32_t i;
+
+	/* read control register */
+	asm ("mrc p15, 0, %0, c1, c0, 0":"=r" (i));
+
+	i &= ~(1 << 1);
+
+	/* write back to control register */
+	asm ("mcr p15, 0, %0, c1, c0, 0": :"r" (i));
+}
+
+void mmu_clean_invalidated_cache_index(int index)
+{
+	asm ("mcr p15, 0, %0, c7, c14, 2": :"r" (index));
+}
+
+void mmu_clean_invalidated_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while(ptr < buffer + size)
+	{
+		asm ("mcr p15, 0, %0, c7, c14, 1": :"r" (ptr));
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+
+void mmu_clean_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while (ptr < buffer + size)
+	{
+		asm ("mcr p15, 0, %0, c7, c10, 1": :"r" (ptr));
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+void mmu_invalidate_dcache(rt_uint32_t buffer, rt_uint32_t size)
+{
+	unsigned int ptr;
+
+	ptr = buffer & ~(CACHE_LINE_SIZE - 1);
+
+	while (ptr < buffer + size)
+	{
+		asm ("mcr p15, 0, %0, c7, c6, 1": :"r" (ptr));
+		ptr += CACHE_LINE_SIZE;
+	}
+}
+
+void mmu_invalidate_tlb()
+{
+	asm ("mcr p15, 0, %0, c8, c7, 0": :"r" (0));
+}
+
+void mmu_invalidate_icache()
+{
+	asm ("mcr p15, 0, %0, c7, c5, 0": :"r" (0));
+}
+
+void mmu_invalidate_dcache_all()
+{
+	asm ("mcr p15, 0, %0, c7, c6, 0": :"r" (0));
+}
+#endif
+
+/* level1 page table */
+static volatile unsigned int _pgd_table[4*1024] ALIGN(16*1024);
+/*
+ * level2 page table
+ * RT_MMU_PTE_SIZE must be 1024*n
+ */
+#define RT_MMU_PTE_SIZE		4096
+static volatile unsigned int _pte_table[RT_MMU_PTE_SIZE] ALIGN(1*1024);
+
+void mmu_create_pgd(struct mem_desc *mdesc)
+{
+	volatile rt_uint32_t *pTT;
+	volatile int i, nSec;
+	pTT = (rt_uint32_t *)_pgd_table + (mdesc->vaddr_start >> 20);
+	nSec = (mdesc->vaddr_end >> 20) - (mdesc->vaddr_start >> 20);
+	for(i = 0; i <= nSec; i++)
+	{
+		*pTT = mdesc->sect_attr | (((mdesc->paddr_start >> 20) + i) << 20);
+		pTT++;
+	}
+}
+
+void mmu_create_pte(struct mem_desc *mdesc)
+{
+	volatile rt_uint32_t *pTT;
+	volatile rt_uint32_t *p_pteentry;
+	int i;
+	rt_uint32_t vaddr;
+	rt_uint32_t total_page = 0;
+	rt_uint32_t pte_offset = 0;
+	rt_uint32_t sect_attr = 0;
+
+	total_page = (mdesc->vaddr_end >> 12) - (mdesc->vaddr_start >> 12) + 1;
+	pte_offset = mdesc->sect_attr & 0xfffffc00;
+	sect_attr = mdesc->sect_attr & 0x3ff;
+	vaddr = mdesc->vaddr_start;
+
+	for(i = 0; i < total_page; i++)
+	{
+		pTT = (rt_uint32_t *)_pgd_table + (vaddr >> 20);
+		if (*pTT == 0) /* Level 1 page table item not used, now update pgd item */
+		{
+			*pTT = pte_offset | sect_attr;
+			p_pteentry = (rt_uint32_t *)pte_offset +
+				((vaddr & 0x000ff000) >> 12);
+			pte_offset += 1024;
+		}
+		else /* using old Level 1 page table item */
+		{
+			p_pteentry = (rt_uint32_t *)(*pTT & 0xfffffc00) +
+				((vaddr & 0x000ff000) >> 12);
+		}
+
+
+		*p_pteentry = mdesc->page_attr | (((mdesc->paddr_start >> 12) + i) << 12);
+		vaddr += 0x1000;
+	}
+}
+
+static void build_pte_mem_desc(struct mem_desc *mdesc, rt_uint32_t size)
+{
+	rt_uint32_t pte_offset = 0;
+	rt_uint32_t nsec = 0;
+	/* set page table */
+	for (; size > 0; size--)
+	{
+		if (mdesc->mapped_mode == PAGE_MAPPED)
+		{
+			nsec = (RT_ALIGN(mdesc->vaddr_end, 0x100000) - RT_ALIGN_DOWN(mdesc->vaddr_start, 0x100000)) >> 20;
+			mdesc->sect_attr |= (((rt_uint32_t)_pte_table)& 0xfffffc00) + pte_offset;
+			pte_offset += nsec << 10;
+		}
+		if (pte_offset >= RT_MMU_PTE_SIZE)
+		{
+			rt_kprintf("PTE table size too little\n");
+			RT_ASSERT(0);
+		}
+
+		mdesc++;
+	}
+}
+
+
+void rt_hw_mmu_init(struct mem_desc *mdesc, rt_uint32_t size)
+{
+	/* disable I/D cache */
+	mmu_disable_dcache();
+	mmu_disable_icache();
+	mmu_disable();
+	mmu_invalidate_tlb();
+
+	/* clear pgd and pte table */
+	rt_memset((void *)_pgd_table, 0, 16*1024);
+	rt_memset((void *)_pte_table, 0, RT_MMU_PTE_SIZE);
+	build_pte_mem_desc(mdesc, size);
+	/* set page table */
+	for (; size > 0; size--)
+	{
+		if (mdesc->mapped_mode == SECT_MAPPED)
+		{
+			mmu_create_pgd(mdesc);
+		}
+		else
+		{
+			mmu_create_pte(mdesc);
+		}
+
+		mdesc++;
+	}
+
+	/* set MMU table address */
+	mmu_setttbase((rt_uint32_t)_pgd_table);
+
+	/* enables MMU */
+	mmu_enable();
+
+	/* enable Instruction Cache */
+	mmu_enable_icache();
+
+	/* enable Data Cache */
+	mmu_enable_dcache();
+
+	mmu_invalidate_icache();
+	mmu_invalidate_dcache_all();
+}
+
diff --git a/libcpu/arm/armv6/mmu.h b/libcpu/arm/armv6/mmu.h
new file mode 100644
index 0000000000..f6e788d13d
--- /dev/null
+++ b/libcpu/arm/armv6/mmu.h
@@ -0,0 +1,208 @@
+/*
+ * File      : mmu.h
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+
+#ifndef __MMU_H__
+#define __MMU_H__
+
+#include <rtthread.h>
+
+#define CACHE_LINE_SIZE	32
+
+/*
+ * Hardware page table definitions.
+ *
+ * + Level 1 descriptor (PGD)
+ *   - common
+ */
+#define PGD_TYPE_MASK       (3 << 0)
+#define PGD_TYPE_FAULT      (0 << 0)
+#define PGD_TYPE_TABLE      (1 << 0)
+#define PGD_TYPE_SECT       (2 << 0)
+#define PGD_BIT4            (1 << 4)
+#define PGD_DOMAIN(x)       ((x) << 5)
+#define PGD_PROTECTION      (1 << 9)    /* ARMv5 */
+/*
+ *   - section
+ */
+#define PGD_SECT_BUFFERABLE	(1 << 2)
+#define PGD_SECT_CACHEABLE  (1 << 3)
+#define PGD_SECT_XN         (1 << 4)    /* ARMv6 */
+#define PGD_SECT_AP0        (1 << 10)
+#define PGD_SECT_AP1        (1 << 11)
+#define PGD_SECT_TEX(x)     ((x) << 12) /* ARMv5 */
+#define PGD_SECT_APX        (1 << 15)   /* ARMv6 */
+#define PGD_SECT_S          (1 << 16)   /* ARMv6 */
+#define PGD_SECT_nG         (1 << 17)   /* ARMv6 */
+#define PGD_SECT_SUPER      (1 << 18)   /* ARMv6 */
+
+#define PGD_SECT_UNCACHED   (0)
+#define PGD_SECT_BUFFERED   (PGD_SECT_BUFFERABLE)
+#define PGD_SECT_WT         (PGD_SECT_CACHEABLE)
+#define PGD_SECT_WB         (PGD_SECT_CACHEABLE | PGD_SECT_BUFFERABLE)
+#define PGD_SECT_MINICACHE  (PGD_SECT_TEX(1) | PGD_SECT_CACHEABLE)
+#define PGD_SECT_WBWA       (PGD_SECT_TEX(1) | PGD_SECT_CACHEABLE | PGD_SECT_BUFFERABLE)
+#define PGD_SECT_NONSHARED_DEV  (PGD_SECT_TEX(2))
+
+
+/*
+ * + Level 2 descriptor (PTE)
+ *   - common
+ */
+#define PTE_TYPE_MASK       (3 << 0)
+#define PTE_TYPE_FAULT      (0 << 0)
+#define PTE_TYPE_LARGE      (1 << 0)
+#define PTE_TYPE_SMALL      (2 << 0)
+#define PTE_TYPE_EXT        (3 << 0)    /* ARMv5 */
+#define PTE_BUFFERABLE      (1 << 2)
+#define PTE_CACHEABLE       (1 << 3)
+
+/*
+ *   - extended small page/tiny page
+ */
+#define PTE_EXT_XN          (1 << 0)    /* ARMv6 */
+#define PTE_EXT_AP_MASK     (3 << 4)
+#define PTE_EXT_AP0         (1 << 4)
+#define PTE_EXT_AP1         (2 << 4)
+#define PTE_EXT_AP_UNO_SRO  (0 << 4)
+#define PTE_EXT_AP_UNO_SRW  (PTE_EXT_AP0)
+#define PTE_EXT_AP_URO_SRW  (PTE_EXT_AP1)
+#define PTE_EXT_AP_URW_SRW  (PTE_EXT_AP1|PTE_EXT_AP0)
+#define PTE_EXT_TEX(x)      ((x) << 6)  /* ARMv5 */
+#define PTE_EXT_APX         (1 << 9)    /* ARMv6 */
+#define PTE_EXT_SHARED      (1 << 10)   /* ARMv6 */
+#define PTE_EXT_NG          (1 << 11)   /* ARMv6 */
+
+/*
+ *   - small page
+ */
+#define PTE_SMALL_AP_MASK       (0xff << 4)
+#define PTE_SMALL_AP_UNO_SRO    (0x00 << 4)
+#define PTE_SMALL_AP_UNO_SRW    (0x55 << 4)
+#define PTE_SMALL_AP_URO_SRW    (0xaa << 4)
+#define PTE_SMALL_AP_URW_SRW    (0xff << 4)
+
+
+/*
+ *  sector table properities
+ */
+#define SECT_CB          (PGD_SECT_CACHEABLE|PGD_SECT_BUFFERABLE) //cache_on, write_back
+#define SECT_CNB         (PGD_SECT_CACHEABLE)                     //cache_on, write_through
+#define SECT_NCB         (PGD_SECT_BUFFERABLE)                    //cache_off,WR_BUF on
+#define SECT_NCNB        (0 << 2)                                 //cache_off,WR_BUF off
+
+#define SECT_AP_RW       (PGD_SECT_AP0|PGD_SECT_AP1)              //supervisor=RW, user=RW
+#define SECT_AP_RO       (PGD_SECT_AP0|PGD_SECT_AP1|PGD_SECT_APX) //supervisor=RO, user=RO
+
+#define SECT_RWX_CB      (SECT_AP_RW|PGD_DOMAIN(0)|PGD_SECT_WB|PGD_TYPE_SECT) /* Read/Write/executable, cache, write back */
+#define SECT_RWX_CNB     (SECT_AP_RW|PGD_DOMAIN(0)|PGD_SECT_WT|PGD_TYPE_SECT) /* Read/Write/executable, cache, write through */
+#define SECT_RWX_NCNB    (SECT_AP_RW|PGD_DOMAIN(0)|PGD_TYPE_SECT) /* Read/Write/executable without cache and write buffer */
+#define SECT_RWX_FAULT   (SECT_AP_RW|PGD_DOMAIN(1)|PGD_TYPE_SECT) /* Read/Write without cache and write buffer */
+
+#define SECT_RWNX_CB     (SECT_AP_RW|PGD_DOMAIN(0)|PGD_SECT_WB|PGD_TYPE_SECT|PGD_SECT_XN) /* Read/Write, cache, write back */
+#define SECT_RWNX_CNB    (SECT_AP_RW|PGD_DOMAIN(0)|PGD_SECT_WT|PGD_TYPE_SECT|PGD_SECT_XN) /* Read/Write, cache, write through */
+#define SECT_RWNX_NCNB   (SECT_AP_RW|PGD_DOMAIN(0)|PGD_TYPE_SECT|PGD_SECT_XN) /* Read/Write without cache and write buffer */
+#define SECT_RWNX_FAULT  (SECT_AP_RW|PGD_DOMAIN(1)|PGD_TYPE_SECT|PGD_SECT_XN) /* Read/Write without cache and write buffer */
+
+
+#define SECT_ROX_CB      (SECT_AP_RO|PGD_DOMAIN(0)|PGD_SECT_WB|PGD_TYPE_SECT) /* Read Only/executable, cache, write back */
+#define SECT_ROX_CNB     (SECT_AP_RO|PGD_DOMAIN(0)|PGD_SECT_WT|PGD_TYPE_SECT) /* Read Only/executable, cache, write through */
+#define SECT_ROX_NCNB    (SECT_AP_RO|PGD_DOMAIN(0)|PGD_TYPE_SECT) /* Read Only/executable without cache and write buffer */
+#define SECT_ROX_FAULT   (SECT_AP_RO|PGD_DOMAIN(1)|PGD_TYPE_SECT) /* Read Only without cache and write buffer */
+
+#define SECT_RONX_CB     (SECT_AP_RO|PGD_DOMAIN(0)|PGD_SECT_WB|PGD_TYPE_SECT|PGD_SECT_XN) /* Read Only, cache, write back */
+#define SECT_RONX_CNB    (SECT_AP_RO|PGD_DOMAIN(0)|PGD_SECT_WT|PGD_TYPE_SECT|PGD_SECT_XN) /* Read Only, cache, write through */
+#define SECT_RONX_NCNB   (SECT_AP_RO|PGD_DOMAIN(0)|PGD_TYPE_SECT|PGD_SECT_XN) /* Read Only without cache and write buffer */
+#define SECT_RONX_FAULT  (SECT_AP_RO|PGD_DOMAIN(1)|PGD_TYPE_SECT|PGD_SECT_XN) /* Read Only without cache and write buffer */
+
+#define SECT_TO_PAGE     (PGD_DOMAIN(0)|PGD_TYPE_TABLE) /* Level 2 descriptor (PTE) entry properity */
+
+/*
+ * page table properities
+ */
+#define PAGE_CB          (PTE_BUFFERABLE|PTE_CACHEABLE)  //cache_on, write_back
+#define PAGE_CNB         (PTE_CACHEABLE)                 //cache_on, write_through
+#define PAGE_NCB         (PTE_BUFFERABLE)                //cache_off,WR_BUF on
+#define PAGE_NCNB        (0 << 2)                        //cache_off,WR_BUF off
+
+#define PAGE_AP_RW       (PTE_EXT_AP0|PTE_EXT_AP1)             //supervisor=RW, user=RW
+#define PAGE_AP_RO       (PTE_EXT_AP0|PTE_EXT_AP1|PTE_EXT_APX) //supervisor=RO, user=RO
+
+#define PAGE_RWX_CB      (PAGE_AP_RW|PAGE_CB|PTE_TYPE_SMALL) /* Read/Write/executable, cache, write back */
+#define PAGE_RWX_CNB     (PAGE_AP_RW|PAGE_CNB|PTE_TYPE_SMALL) /* Read/Write/executable, cache, write through */
+#define PAGE_RWX_NCNB    (PAGE_AP_RW|PTE_TYPE_SMALL) /* Read/Write/executable without cache and write buffer */
+#define PAGE_RWX_FAULT   (PAGE_AP_RW|PTE_TYPE_SMALL) /* Read/Write without cache and write buffer */
+
+#define PAGE_RWNX_CB     (PAGE_AP_RW|PAGE_CB|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read/Write, cache, write back */
+#define PAGE_RWNX_CNB    (PAGE_AP_RW|PAGE_CNB|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read/Write, cache, write through */
+#define PAGE_RWNX_NCNB   (PAGE_AP_RW|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read/Write without cache and write buffer */
+#define PAGE_RWNX_FAULT  (PAGE_AP_RW|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read/Write without cache and write buffer */
+
+
+#define PAGE_ROX_CB      (PAGE_AP_RO|PAGE_CB|PTE_TYPE_SMALL) /* Read Only/executable, cache, write back */
+#define PAGE_ROX_CNB     (PAGE_AP_RO|PAGE_CNB|PTE_TYPE_SMALL) /* Read Only/executable, cache, write through */
+#define PAGE_ROX_NCNB    (PAGE_AP_RO|PTE_TYPE_SMALL) /* Read Only/executable without cache and write buffer */
+#define PAGE_ROX_FAULT   (PAGE_AP_RO|PTE_TYPE_SMALL) /* Read Only without cache and write buffer */
+
+#define PAGE_RONX_CB     (PAGE_AP_RO|PAGE_CB|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read Only, cache, write back */
+#define PAGE_RONX_CNB    (PAGE_AP_RO|PAGE_CNB|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read Only, cache, write through */
+#define PAGE_RONX_NCNB   (PAGE_AP_RO|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read Only without cache and write buffer */
+#define PAGE_RONX_FAULT  (PAGE_AP_RO|PTE_TYPE_SMALL|PTE_EXT_XN) /* Read Only without cache and write buffer */
+
+
+#define DESC_SEC		(0x2|(1<<4))
+#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 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)
+
+#define RW_CB		(AP_RW|DOMAIN0|CB|DESC_SEC)		/* Read/Write, cache, write back */
+#define RW_CNB		(AP_RW|DOMAIN0|CNB|DESC_SEC)	/* Read/Write, cache, write through */
+#define RW_NCNB		(AP_RW|DOMAIN0|NCNB|DESC_SEC)	/* Read/Write without cache and write buffer */
+#define RW_FAULT	(AP_RW|DOMAIN1|NCNB|DESC_SEC)	/* Read/Write without cache and write buffer */
+
+struct mem_desc {
+	rt_uint32_t vaddr_start;
+	rt_uint32_t vaddr_end;
+	rt_uint32_t paddr_start;
+	rt_uint32_t sect_attr;   /* when page mapped */
+	rt_uint32_t page_attr;   /* only sector mapped valid */
+	rt_uint32_t mapped_mode;
+#define     SECT_MAPPED  0
+#define     PAGE_MAPPED  1
+};
+
+void rt_hw_mmu_init(struct mem_desc *mdesc, rt_uint32_t size);
+
+#endif
+
diff --git a/libcpu/arm/armv6/stack.c b/libcpu/arm/armv6/stack.c
new file mode 100644
index 0000000000..ceb04f0985
--- /dev/null
+++ b/libcpu/arm/armv6/stack.c
@@ -0,0 +1,79 @@
+/*
+ * File      : stack.c
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2011-01-13     weety      copy from mini2440
+ */
+#include <rtthread.h>
+
+/*****************************/
+/* CPU Mode                  */
+/*****************************/
+#define USERMODE		0x10
+#define FIQMODE			0x11
+#define IRQMODE			0x12
+#define SVCMODE			0x13
+#define ABORTMODE		0x17
+#define UNDEFMODE		0x1b
+#define MODEMASK		0x1f
+#define NOINT			0xc0
+
+/**
+ * This function will initialize thread stack
+ *
+ * @param tentry the entry of thread
+ * @param parameter the parameter of entry 
+ * @param stack_addr the beginning stack address
+ * @param texit the function will be called when thread exit
+ *
+ * @return stack address
+ */
+rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
+	rt_uint8_t *stack_addr, void *texit)
+{
+	rt_uint32_t *stk;
+
+	stk 	 = (rt_uint32_t*)stack_addr;
+	*(stk) 	 = (rt_uint32_t)tentry;			/* entry point */
+	*(--stk) = (rt_uint32_t)texit;			/* lr */
+	*(--stk) = 0;							/* r12 */
+	*(--stk) = 0;							/* r11 */
+	*(--stk) = 0;							/* r10 */
+	*(--stk) = 0;							/* r9 */
+	*(--stk) = 0;							/* r8 */
+	*(--stk) = 0;							/* r7 */
+	*(--stk) = 0;							/* r6 */
+	*(--stk) = 0;							/* r5 */
+	*(--stk) = 0;							/* r4 */
+	*(--stk) = 0;							/* r3 */
+	*(--stk) = 0;							/* r2 */
+	*(--stk) = 0;							/* 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   */
+
+	/* return task's current stack address */
+	return (rt_uint8_t *)stk;
+}
+
diff --git a/libcpu/arm/armv6/vfp.c b/libcpu/arm/armv6/vfp.c
new file mode 100644
index 0000000000..478c0603d7
--- /dev/null
+++ b/libcpu/arm/armv6/vfp.c
@@ -0,0 +1,161 @@
+/*
+ * File      : vfp.c
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Develop Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2014-11-07     weety      first version
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+#include "vfp.h"
+
+#ifdef RT_USING_VFP
+
+rt_uint32_t vfpregs_offset = offsetof(struct rt_thread, vfpregs);
+
+struct vfp_context *last_vfp_context = RT_NULL;
+
+int vfp_switch(rt_uint32_t cmd, struct rt_thread *thread)
+{
+	rt_uint32_t fpexc;
+	//rt_kprintf("%s:%d, %x\n", __func__, cmd, thread);
+
+	switch (cmd) {
+		case THREAD_INIT:
+		{
+			struct vfp_context *vfp = &thread->vfpregs;
+
+			rt_memset(vfp, 0, sizeof(struct vfp_context));
+			vfp->fpexc = FPEXC_EN;
+			vfp->fpscr = FPSCR_RN;
+
+			if (last_vfp_context == vfp)
+				last_vfp_context = RT_NULL;
+			vmsr(FPEXC, vmrs(FPEXC) & ~FPEXC_EN);
+			break;
+		}
+		case THREAD_EXIT:
+		{
+				/* release case: Per-thread VFP cleanup. */
+			struct vfp_context *vfp = &thread->vfpregs;
+
+			if (last_vfp_context == vfp)
+				last_vfp_context = RT_NULL;
+			break;
+		}
+		default:
+			break;
+	}
+
+	return 0;
+}
+
+int vfp_thread_init(struct rt_thread *thread)
+{
+	return vfp_switch(THREAD_INIT, thread);
+}
+
+
+int vfp_thread_exit(struct rt_thread *thread)
+{
+	return vfp_switch(THREAD_EXIT, thread);
+}
+
+rt_uint32_t read_vfp_regs(rt_uint32_t *buf)
+{
+	rt_uint32_t value;
+	rt_uint32_t length;
+	asm  volatile ("vldmia  %0!, {d0-d15}"
+			:
+			:"r"(buf)
+			:"cc");
+	length = 32;
+#ifdef RT_USING_VFPv3
+	asm  volatile ("vmrs  %0, mvfr0"
+			:"=r"(value)
+			:);
+	if ((value & MVFR0_A_SIMD_MASK) == 2)
+	{
+		asm  volatile ("vldmia  %0!, {d16-d31}"
+				:
+				:"r"(buf)
+				:"cc");
+		length = 64;
+	}
+	else
+	{
+		length = 32;
+	}
+#endif
+
+	return length;
+}
+
+void vfp_exception(rt_uint32_t instruction, rt_uint32_t fpexc)
+{
+	int i = 0;
+	int length = 0;
+	rt_uint32_t fpscr, fpsid;
+#ifdef RT_USING_VFPv3
+	unsigned long long vfp_regs[32];
+#else
+	unsigned long long vfp_regs[16];
+#endif
+
+	rt_kprintf("VFP: exception: instruction %08x fpexc %08x\n", instruction, fpexc);
+	fpsid = vmrs(FPSID);
+	fpscr = vmrs(FPSCR);
+	rt_kprintf("VFP: exception: fpsid %08x fpscr %08x\n", fpsid, fpscr);
+
+	length = read_vfp_regs((rt_uint32_t *)vfp_regs);
+	rt_kprintf("VFP: exception registers: {s0~s%d}\n", length - 1);
+	for(i = 0; i < length; i++)
+	{
+		if (i && !(i & 0x3))
+			rt_kprintf("\n");
+		rt_kprintf("0x%08x ", ((rt_uint32_t *)vfp_regs)[i]);
+	}
+	rt_kprintf("\n");
+	
+}
+
+void vfp_init(void)
+{
+	int ret = 0;
+	unsigned int value;
+	asm  volatile ("mrc p15, 0, %0, c1, c0, 2"
+			:"=r"(value)
+			:);
+	value |= 0xf00000;/*enable CP10, CP11 user access*/
+	asm volatile("mcr p15, 0, %0, c1, c0, 2"
+			:
+			:"r"(value));
+#if 0
+	asm volatile("fmrx %0, fpexc"
+			:"=r"(value));
+	value |=(1<<30);
+	asm volatile("fmxr fpexc, %0"
+			:
+			:"r"(value));
+#endif
+}
+
+
+#endif
diff --git a/libcpu/arm/armv6/vfp.h b/libcpu/arm/armv6/vfp.h
new file mode 100644
index 0000000000..0ec7b26b72
--- /dev/null
+++ b/libcpu/arm/armv6/vfp.h
@@ -0,0 +1,110 @@
+/*
+ * File      : vfp.h
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Develop Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2014-11-07     weety      first version
+ */
+
+#ifndef __VFP_H__
+#define __VFP_H__
+
+/* FPSID register bits */
+#define FPSID_IMPLEMENTER_BIT   (24)
+#define FPSID_IMPLEMENTER_MASK  (0xff << FPSID_IMPLEMENTER_BIT)
+#define FPSID_SW                (1 << 23)
+#define FPSID_FORMAT_BIT        (21)
+#define FPSID_FORMAT_MASK       (0x3 << FPSID_FORMAT_BIT)
+#define FPSID_NODOUBLE          (1 << 20)
+#define FPSID_ARCH_BIT          (16)
+#define FPSID_ARCH_MASK         (0xF << FPSID_ARCH_BIT)
+#define FPSID_PART_BIT          (8)
+#define FPSID_PART_MASK         (0xFF << FPSID_PART_BIT)
+#define FPSID_VARIANT_BIT       (4)
+#define FPSID_VARIANT_MASK      (0xF << FPSID_VARIANT_BIT)
+#define FPSID_REVISION_BIT      (0)
+#define FPSID_REVISION_MASK     (0xF << FPSID_REVISION_BIT)
+
+/* FPSCR register bits */
+#define FPSCR_DN           (1<<25) /* Default NaN mode enable bit */
+#define FPSCR_FZ           (1<<24) /* Flush-to-zero mode enable bit */
+#define FPSCR_RN           (0<<22) /* Round to nearest (RN) mode */
+#define FPSCR_RP           (1<<22) /* Round towards plus infinity (RP) mode */
+#define FPSCR_RM           (2<<22) /* Round towards minus infinity (RM) mode */
+#define FPSCR_RZ           (3<<22) /* Round towards zero (RZ) mode */
+#define FPSCR_RMODE_BIT    (22)
+#define FPSCR_RMODE_MASK   (3 << FPSCR_RMODE_BIT)
+#define FPSCR_STRIDE_BIT   (20)
+#define FPSCR_STRIDE_MASK  (3 << FPSCR_STRIDE_BIT)
+#define FPSCR_LENGTH_BIT   (16)
+#define FPSCR_LENGTH_MASK  (7 << FPSCR_LENGTH_BIT)
+#define FPSCR_IDE          (1<<15) /* Input Subnormal exception trap enable bit */
+#define FPSCR_IXE          (1<<12) /* Inexact exception trap enable bit */
+#define FPSCR_UFE          (1<<11) /* Underflow exception trap enable bit */
+#define FPSCR_OFE          (1<<10) /* Overflow exception trap enable bit */
+#define FPSCR_DZE          (1<<9)  /* Division by Zero exception trap enable bit */
+#define FPSCR_IOE          (1<<8)  /* Invalid Operation exception trap enable bit */
+#define FPSCR_IDC          (1<<7)  /* Input Subnormal cumulative exception flag */
+#define FPSCR_IXC          (1<<4)  /* Inexact cumulative exception flag */
+#define FPSCR_UFC          (1<<3)  /* Underflow cumulative exception flag */
+#define FPSCR_OFC          (1<<2)  /* Overflow cumulative exception flag */
+#define FPSCR_DZC          (1<<1)  /* Division by Zero cumulative exception flag */
+#define FPSCR_IOC          (1<<0)  /* Invalid Operation cumulative exception flag */
+
+/* FPEXC register bits */
+#define FPEXC_EX          (1 << 31) /* When EX is set, the VFP coprocessor is in the exceptional state */
+#define FPEXC_EN          (1 << 30) /* VFP enable bit */
+#define FPEXC_DEX         (1 << 29) /* Defined synchronous instruction exceptional flag */
+#define FPEXC_FP2V        (1 << 28) /*  FPINST2 instruction valid flag */
+#define FPEXC_LENGTH_BIT  (8)
+#define FPEXC_LENGTH_MASK (7 << FPEXC_LENGTH_BIT)
+#define FPEXC_INV         (1 << 7)  /* Input exception flag */
+#define FPEXC_UFC         (1 << 3)  /* Potential underflow flag */
+#define FPEXC_OFC         (1 << 2)  /* Potential overflow flag */
+#define FPEXC_IOC         (1 << 0)  /* Potential invalid operation flag */
+#define FPEXC_TRAP_MASK   (FPEXC_INV|FPEXC_UFC|FPEXC_OFC|FPEXC_IOC)
+
+
+/* MVFR0 register bits */
+#define MVFR0_A_SIMD_BIT    (0)
+#define MVFR0_A_SIMD_MASK   (0xf << MVFR0_A_SIMD_BIT)
+
+
+/* thread switch micro */
+#define THREAD_INIT   0
+#define THREAD_EXIT   1
+
+/*
+ * get VFP register
+ */
+
+#define vmrs(vfp) ({ \
+    rt_uint32_t var; \
+    asm("vmrs  %0, "#vfp"" : "=r" (var) : : "cc"); \
+    var; \
+ })
+
+#define vmsr(vfp, var) \
+    asm("vmsr  "#vfp",  %0"	\
+       : : "r" (var) : "cc")
+
+
+#endif
+
+
diff --git a/libcpu/arm/armv6/vfp_entry_gcc.S b/libcpu/arm/armv6/vfp_entry_gcc.S
new file mode 100644
index 0000000000..2c60d0f26a
--- /dev/null
+++ b/libcpu/arm/armv6/vfp_entry_gcc.S
@@ -0,0 +1,239 @@
+/*
+ * File      : vfp_entry_gcc.S
+ * This file is part of RT-Thread RTOS
+ * COPYRIGHT (C) 2006, RT-Thread Development Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2014-11-07     weety    first version
+ */
+
+#include <rtconfig.h>
+
+#ifdef RT_USING_VFP
+
+#include "armv6.h"
+#include "vfp.h"
+
+//#define DEBUG
+
+.macro	PRINT, str
+#ifdef DEBUG
+    stmfd sp!, {r0-r3, ip, lr}
+    add   r0, pc, #4
+    bl    rt_kprintf
+    b     1f
+    .asciz  "VFP: \str\n"
+    .balign 4
+1: ldmfd    sp!, {r0-r3, ip, lr}
+#endif
+    .endm
+
+.macro  PRINT1, str, arg
+#ifdef DEBUG
+    stmfd sp!, {r0-r3, ip, lr}
+    mov   r1, \arg
+    add   r0, pc, #4
+    bl    rt_kprintf
+    b     1f
+    .asciz  "VFP: \str\n"
+    .balign 4
+1:  ldmfd    sp!, {r0-r3, ip, lr}
+#endif
+    .endm
+
+.macro  PRINT3, str, arg1, arg2, arg3
+#ifdef DEBUG
+    stmfd sp!, {r0-r3, ip, lr}
+    mov   r3, \arg3
+    mov   r2, \arg2
+    mov   r1, \arg1
+    add   r0, pc, #4
+    bl    rt_kprintf
+    b     1f
+    .asciz  "VFP: \str\n"
+    .balign 4
+1:  ldmfd  sp!, {r0-r3, ip, lr}
+#endif
+    .endm
+
+.macro get_vfpregs_offset, rd
+    ldr \rd, .vfp_offset
+    ldr \rd, [\rd]
+    .endm
+
+.vfp_offset:
+    .word  vfpregs_offset
+
+.macro vfp_restore_working_reg, base, rd0
+    vldmia \base!, {d0-d15}
+#ifdef RT_USING_VFPv3
+    vmrs \rd0, mvfr0
+    and  \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
+    cmp  \rd0, #2                       @ 0b0000  Not supported.
+                                        @ 0b0001  Supported, 16 ��64-bit registers.
+                                        @ 0b0010  Supported, 32 ��64-bit registers.
+    vldmiaeq \base!, {d16-d31}
+    addne    \base, \base, #32*4        @ skip unused registers
+#endif
+    .endm
+
+.macro vfp_save_working_reg, base, rd0
+    vstmia \base!, {d0-d15}             @ save the working registers
+#ifdef RT_USING_VFPv3
+    vmrs \rd0, mvfr0
+    and  \rd0, \rd0, #MVFR0_A_SIMD_MASK @ A_SIMD registers
+    cmp  \rd0, #2                       @ 0b0000  Not supported.
+                                        @ 0b0001  Supported, 16 ��64-bit registers.
+                                        @ 0b0010  Supported, 32 ��64-bit registers.
+    vstmiaeq \base!, {d16-d31}
+    addne    \base, \base, #32*4        @ skip unused registers
+#endif
+    .endm
+
+.macro vfp_restore_state, base, fpexc_rd, rd0, rd1, rd2
+    ldmia    \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ load FPEXC, FPSCR, FPINST, FPINST2
+    tst      \fpexc_rd, #FPEXC_EX                 @ vfp is in the exceptional state?
+    beq      1f
+    vmsr     fpinst, \rd1                         @ restore fpinst
+    tst      \fpexc_rd, #FPEXC_FP2V               @ FPINST2 instruction valid
+    beq      1f
+    vmsr     fpinst2, \rd2                        @ restore fpinst2
+1:
+    vmsr     fpscr, \rd0                          @ restore fpscr
+    .endm
+
+.macro vfp_save_state, base, fpexc_rd, rd0, rd1, rd2
+    vmrs  \rd0, fpscr                          @ current status
+    tst   \fpexc_rd, #FPEXC_EX                 @ vfp is in the exceptional state?
+    beq   1f
+    vmrs  \rd1, fpinst                         @ get fpinst
+    tst   \fpexc_rd, #FPEXC_FP2V               @ FPINST2 instruction valid
+    beq   1f
+    vmrs  \rd2, fpinst2                        @ get fpinst2
+1:
+    stmia \base, {\fpexc_rd, \rd0, \rd1, \rd2} @ save FPEXC, FPSCR, FPINST, FPINST2
+    .endm
+
+
+/*
+ *  VFP hardware support entry point.
+ *  r0  = faulted instruction
+ *  r2  = faulted PC+4
+ *  r9  = successful return
+ *  r10 = rt_thread structure
+ *  lr  = failure return
+ */
+
+.globl vfp_entry
+vfp_entry:
+    ldr  r1, =rt_interrupt_nest
+    ldr  r1, [r1]                     @ get rt_interrupt_nest
+    cmp  r1, #0                       @ rt_interrupt_nest == 0?
+    bne  irq_vfp_entry                @ irq handler used VFP
+
+    get_vfpregs_offset r11
+    add  r10, r10, r11  @ r10 = vfpregs
+
+    vmrs r1, fpexc
+    tst  r1, #FPEXC_EN
+    bne  __lookup_vfp_exceptions      @ if the VFP already enabled, now checking vfp exceptions
+
+    ldr  r3, last_vfp_context_address
+    orr  r1, r1, #FPEXC_EN            @ set VFP enable bit
+    ldr  r4, [r3]                     @ get last_vfp_context pointer
+    bic  r5, r1, #FPEXC_EX            @ clear exceptions status
+    cmp  r4, r10
+    beq  __switch_to_the_same_thread  @ switch to the same thread, checking pending exception.
+
+    vmsr fpexc, r5                    @ enable VFP, clear any pending exceptions
+
+    /* Save the current VFP registers to the old thread context */
+    cmp  r4, #0
+    beq  __no_last_vfp_context
+    vfp_save_working_reg r4, r5       @ save the working registers
+    vfp_save_state r4, r1, r5, r6, r8 @ save vfp state registers
+
+__no_last_vfp_context:
+    str    r10, [r3]                    @ update the last_vfp_context pointer
+
+    vfp_restore_working_reg r10, r5     @ restore the working registers
+    vfp_restore_state r10, r1, r5, r6, r8 @ restore vfp state registers
+
+__switch_to_the_same_thread:
+    tst      r1, #FPEXC_EX
+    bne      __do_exception
+    vmsr     fpexc, r1                  @ restore fpexc last
+    sub      r2, r2, #4
+    str      r2, [sp, #S_PC]            @ retry the faulted instruction
+    PRINT1 "return instr=0x%08x", r2
+    mov      pc, r9
+
+
+__lookup_vfp_exceptions:
+    tst   r1, #FPEXC_EX | FPEXC_DEX     @ Check for synchronous or asynchronous exception
+    bne   __do_exception
+    vmrs  r5, fpscr
+    tst   r5, #FPSCR_IXE
+    bne   __do_exception
+
+    PRINT "__lookup_vfp_exceptions"
+    mov   pc, lr
+
+__do_exception:
+    PRINT "__do_exception"
+    push  {lr}
+    mov   r5, r1
+    bic   r5, #FPEXC_EX                 @ clear exception
+    vmsr  fpexc, r5
+    bl    vfp_exception                 @ r0 = faulted instruction, r1 = fpexc
+    pop   {pc}
+    @mov   pc, lr
+
+
+irq_vfp_entry:
+    vmrs r1, fpexc
+    tst  r1, #FPEXC_EN
+    bne  __lookup_vfp_exceptions        @ if the VFP already enabled, now checking vfp exceptions
+
+    ldr  r3, last_vfp_context_address
+    orr  r1, r1, #FPEXC_EN              @ set VFP enable bit
+    ldr  r4, [r3]                       @ get last_vfp_context pointer
+    bic  r5, r1, #FPEXC_EX              @ clear exceptions status
+
+    vmsr fpexc, r5                      @ enable VFP, clear any pending exceptions
+
+    /* Save the current VFP registers to the old thread context */
+    cmp  r4, #0                         @ last_vfp_context != NULL ?
+    beq  __no_save_vfp_context
+    vfp_save_working_reg r4, r5         @ save the working registers
+    vfp_save_state r4, r1, r5, r6, r8   @ save vfp state registers
+
+    mov   r4, #0
+    str   r4, [r3]                      @ update the last_vfp_context pointer
+                                        @ last_vfp_context = NULL
+__no_save_vfp_context:
+    sub   r2, r2, #4
+    str   r2, [sp, #S_PC]               @ retry the faulted instruction
+    PRINT1 "return instr=0x%08x", r2
+    mov   pc, r9
+
+    .align
+last_vfp_context_address:
+    .word  last_vfp_context
+
+#endif