Index: sys/arm/arm/cpu_asm-v6.S
===================================================================
--- sys/arm/arm/cpu_asm-v6.S
+++ sys/arm/arm/cpu_asm-v6.S
@@ -26,6 +26,7 @@
  *
  * $FreeBSD$
  */
+#include "assym.s"
 
 #include <machine/acle-compat.h>
 #include <machine/asm.h>
@@ -33,6 +34,17 @@
 #include <machine/armreg.h>
 #include <machine/sysreg.h>
 
+#if __ARM_ARCH >= 6
+#define GET_PCB(tmp) \
+	mrc CP15_TPIDRPRW(tmp); \
+	add   tmp, tmp, #(TD_PCB)
+#else
+.Lcurpcb:
+	.word _C_LABEL(__pcpu) + PC_CURPCB
+#define GET_PCB(tmp) \
+	ldr   tmp, .Lcurpcb
+#endif
+
 /* 
  * Define cache functions used by startup code, which counts on the fact that
  * only r0-r3,r12 (ip) are modified and no stack space is used.  These functions
@@ -208,3 +220,59 @@
 	bx	lr
 #endif /* __ARM_ARCH == 6 */
 END(dcache_wbinv_poc_all)
+
+ASENTRY_NP(dcache_wb_pou_checked)
+	ldr	ip, .Lcpuinfo
+	ldr	ip, [ip, #DCACHE_LINE_SIZE]
+
+	GET_PCB(r2)
+	ldr	r2, [r2]
+
+	adr	r3, _C_LABEL(cachebailout)
+	str	r3, [r2, #PCB_ONFAULT]
+1:
+	mcr	CP15_DCCMVAC(r0)
+	add	r0, r0, ip
+	subs	r1, r1, ip
+	bhi	1b
+	DSB
+	mov	r0, #0
+	str	r0, [r2, #PCB_ONFAULT]
+	mov	r0, #1			/* cannot be faulting address */
+	RET
+
+.Lcpuinfo:
+	.word	cpuinfo
+END(dcache_wb_pou_checked)
+
+ASENTRY_NP(icache_inv_pou_checked)
+	ldr	ip, .Lcpuinfo
+	ldr	ip, [ip, #ICACHE_LINE_SIZE]
+
+	GET_PCB(r2)
+	ldr	r2, [r2]
+
+	adr	r3, _C_LABEL(cachebailout)
+	str	r3, [r2, #PCB_ONFAULT]
+
+1:
+	mcr	CP15_ICIMVAU(r0)
+	add	r0, r0, ip
+	subs	r1, r1, ip
+	bhi	1b
+	DSB
+	ISB
+	mov	r0, #0
+	str	r0, [r2, #PCB_ONFAULT]
+	mov	r0, #1			/* cannot be faulting address */
+	RET
+END(icache_inv_pou_checked)
+
+/* label must be global as trap-v6.c references it */
+	.global	_C_LABEL(cachebailout)
+_C_LABEL(cachebailout):
+	DSB
+	ISB
+	mov	r1, #0
+	str	r1, [r2, #PCB_ONFAULT]
+	RET
Index: sys/arm/arm/sys_machdep.c
===================================================================
--- sys/arm/arm/sys_machdep.c
+++ sys/arm/arm/sys_machdep.c
@@ -41,8 +41,12 @@
 #include <sys/sysproto.h>
 #include <sys/syscall.h>
 #include <sys/sysent.h>
+#include <vm/vm.h>
+#include <vm/vm_extern.h>
 
+#include <machine/cpu-v6.h>
 #include <machine/sysarch.h>
+#include <machine/vmparam.h>
 
 #ifndef _SYS_SYSPROTO_H_
 struct sysarch_args {
@@ -55,16 +59,84 @@
 static int arm32_sync_icache (struct thread *, void *);
 static int arm32_drain_writebuf(struct thread *, void *);
 
+#if __ARM_ARCH >= 6
+static int
+sync_icache(uintptr_t addr, size_t len)
+{
+	size_t size;
+	vm_offset_t rv;
+
+	/* allign starting address to cacheline */
+	len += addr & cpuinfo.icache_line_mask;
+	addr &= ~cpuinfo.icache_line_mask;
+	len += addr & cpuinfo.dcache_line_mask;
+	addr &= ~cpuinfo.dcache_line_mask;
+
+	/* break whole range to pages  */
+	do {
+		size = PAGE_SIZE - (addr & (PAGE_SIZE - 1));
+		size = min(size, len);
+		rv = dcache_wb_pou_checked(addr, size);
+		if (rv == 1) /* see dcache_wb_pou_checked() */
+			rv = icache_inv_pou_checked(addr, size);
+		if (rv != 1) {
+			if (!useracc((void *)addr, size, VM_PROT_READ)) {
+				/* Invalid access */
+				return (rv);
+			}
+			/* Valid but unmapped page - skip it */
+		}
+		len -= size;
+		addr += size;
+	} while (len > 0);
+
+	/* Invalidte branch predictor buffer */
+	bpb_inv_all();
+	return (1);
+}
+#endif
+
 static int
 arm32_sync_icache(struct thread *td, void *args)
 {
 	struct arm_sync_icache_args ua;
 	int error;
-
+	ksiginfo_t ksi;
+#if __ARM_ARCH >= 6
+	vm_offset_t rv;
+#endif
 	if ((error = copyin(args, &ua, sizeof(ua))) != 0)
 		return (error);
 
+	if  (ua.len == 0) {
+		td->td_retval[0] = 0;
+		return (0);
+	}
+
+	/* Validate arguments */
+	if ((((ua.addr >> 4) + (ua.len >> 4)) >= (VM_MAXUSER_ADDRESS >> 4)) ||
+	    ((ua.addr + ua.len) >= VM_MAXUSER_ADDRESS)) {
+		ksiginfo_init_trap(&ksi);
+		ksi.ksi_signo = SIGSEGV;
+		ksi.ksi_code = SEGV_ACCERR;
+		ksi.ksi_addr = (void *)VM_MAXUSER_ADDRESS;
+		trapsignal(td, &ksi);
+		return (EINVAL);
+	}
+
+#if __ARM_ARCH >= 6
+	rv = sync_icache(ua.addr, ua.len);
+	if (rv != 1) {
+		ksiginfo_init_trap(&ksi);
+		ksi.ksi_signo = SIGSEGV;
+		ksi.ksi_code = SEGV_MAPERR;
+		ksi.ksi_addr = (void *)rv;
+		trapsignal(td, &ksi);
+		return (EINVAL);
+	}
+#else
 	cpu_icache_sync_range(ua.addr, ua.len);
+#endif
 
 	td->td_retval[0] = 0;
 	return (0);
Index: sys/arm/arm/trap-v6.c
===================================================================
--- sys/arm/arm/trap-v6.c
+++ sys/arm/arm/trap-v6.c
@@ -66,6 +66,7 @@
 #endif
 
 extern char fusubailout[];
+extern char cachebailout[];
 
 #ifdef DEBUG
 int last_fault_code;	/* For the benefit of pmap_fault_fixup() */
@@ -132,7 +133,7 @@
 	{abort_align,	"Alignment Fault"},
 	{abort_fatal,	"Debug Event"},
 	{NULL,		"Access Bit (L1)"},
-	{abort_icache,	"Instruction cache maintenance"},
+	{NULL,		"Instruction cache maintenance"},
 	{NULL,		"Translation Fault (L1)"},
 	{NULL,		"Access Bit (L2)"},
 	{NULL,		"Translation Fault (L2)"},
@@ -401,6 +402,24 @@
 	}
 
 	/*
+	 * Don't pass faulting cache operation to vm_fault(). We don't want
+	 * to handle all vm stuff at this moment.
+	 */
+	pcb = td->td_pcb;
+	if (__predict_false(pcb->pcb_onfault == cachebailout)) {
+		tf->tf_r0 = far;		/* return failing address */
+		tf->tf_pc = (register_t)pcb->pcb_onfault;
+		return;
+	}
+
+	/* Handle remaining I cache aborts. */
+	if (idx == FAULT_ICACHE) {
+		if (abort_icache(tf, idx, fsr, far, prefetch, td, &ksig))
+			goto do_trapsignal;
+		goto out;
+	}
+
+	/*
 	 * At this point, we're dealing with one of the following aborts:
 	 *
 	 *  FAULT_TRAN_xx  - Translation