diff --git a/arch/arm/common/Kconfig b/arch/arm/common/Kconfig
index c8e198631d41..1de83ee7f057 100644
--- a/arch/arm/common/Kconfig
+++ b/arch/arm/common/Kconfig
@@ -18,3 +18,7 @@ config SHARP_PARAM
 
 config SHARP_SCOOP
 	bool
+
+config FIQ_GLUE
+	bool
+	select FIQ
diff --git a/arch/arm/common/Makefile b/arch/arm/common/Makefile
index 8cd574be94cf..44c4cfd11d0c 100644
--- a/arch/arm/common/Makefile
+++ b/arch/arm/common/Makefile
@@ -5,6 +5,7 @@
 
 obj-y				+= firmware.o
 
+obj-$(CONFIG_FIQ_GLUE)		+= fiq_glue.o fiq_glue_setup.o
 obj-$(CONFIG_SA1111)		+= sa1111.o
 obj-$(CONFIG_DMABOUNCE)		+= dmabounce.o
 obj-$(CONFIG_KRAIT_L2_ACCESSORS) += krait-l2-accessors.o
diff --git a/arch/arm/common/fiq_glue_setup.c b/arch/arm/common/fiq_glue_setup.c
index 8cb1b611c6d5..a2d0d82ae8c5 100644
--- a/arch/arm/common/fiq_glue_setup.c
+++ b/arch/arm/common/fiq_glue_setup.c
@@ -33,7 +33,7 @@ static void fiq_glue_setup_helper(void *info)
 {
 	struct fiq_glue_handler *handler = info;
 	fiq_glue_setup(handler->fiq, handler,
-		__get_cpu_var(fiq_stack) + THREAD_START_SP,
+		(void *)((*this_cpu_ptr(&fiq_stack)) + THREAD_START_SP),
 		fiq_return_handler);
 }
 
@@ -139,7 +139,7 @@ void fiq_glue_resume(void)
 	if (!current_handler)
 		return;
 	fiq_glue_setup(current_handler->fiq, current_handler,
-		__get_cpu_var(fiq_stack) + THREAD_START_SP,
+		(void *)((*this_cpu_ptr(&fiq_stack)) + THREAD_START_SP),
 		fiq_return_handler);
 	if (current_handler->resume)
 		current_handler->resume(current_handler);
diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c
index 98ca3e3fa847..eff3d2d89d59 100644
--- a/arch/arm/kernel/fiq.c
+++ b/arch/arm/kernel/fiq.c
@@ -94,8 +94,9 @@ int show_fiq_list(struct seq_file *p, int prec)
 void set_fiq_handler(void *start, unsigned int length)
 {
 	void *base = vectors_page;
-	unsigned offset = FIQ_OFFSET;
+	unsigned int volatile offset = FIQ_OFFSET;
 
+	offset &= 0xfffffffc;
 	memcpy(base + offset, start, length);
 	if (!cache_is_vipt_nonaliasing())
 		flush_icache_range((unsigned long)base + offset,
diff --git a/drivers/staging/android/fiq_debugger/Kconfig b/drivers/staging/android/fiq_debugger/Kconfig
index 74bf574d671c..7927b294511c 100644
--- a/drivers/staging/android/fiq_debugger/Kconfig
+++ b/drivers/staging/android/fiq_debugger/Kconfig
@@ -70,3 +70,11 @@ config RK_CONSOLE_THREAD
 	default n
 	help
 	  Normal kernel printk will write out to UART by "kconsole" kthread
+
+config FIQ_DEBUGGER_FIQ_GLUE
+	bool "Uart FIQ is captured by linux"
+	depends on FIQ_DEBUGGER
+	select FIQ_GLUE
+	default n
+	help
+	  It is for ARM v7 arch without trust zone.