From e238ddfc426bd1f8b2ac3b5f396a31150cbe7e5c Mon Sep 17 00:00:00 2001
From: Leo Yan <leoy@marvell.com>
Date: Mon, 23 Sep 2013 17:06:20 +0800
Subject: [PATCH 2/2] ARM64: add cpu tear down function for A53's power mode

Signed-off-by: Leo Yan <leoy@marvell.com>
---
 arch/arm64/kernel/Makefile        |    2 +-
 arch/arm64/kernel/cpu_tear_down.S |   90 +++++++++++++++++++++++++++++++++++++
 2 files changed, 91 insertions(+), 1 deletion(-)
 create mode 100644 arch/arm64/kernel/cpu_tear_down.S

diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 010550a..046fbf0 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -18,7 +18,7 @@ arm64-obj-$(CONFIG_SMP)			+= smp.o smp_spin_table.o smp_psci.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)+= hw_breakpoint.o
 arm64-obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
-arm64-obj-$(CONFIG_ARM_CPU_SUSPEND)	+= sleep.o suspend.o
+arm64-obj-$(CONFIG_ARM_CPU_SUSPEND)	+= sleep.o suspend.o cpu_tear_down.o
 
 arm64-obj-$(CONFIG_SUSPEND)		+= fake_suspend.o
 obj-y					+= $(arm64-obj-y) vdso/
diff --git a/arch/arm64/kernel/cpu_tear_down.S b/arch/arm64/kernel/cpu_tear_down.S
new file mode 100644
index 0000000..9f3d5d0
--- /dev/null
+++ b/arch/arm64/kernel/cpu_tear_down.S
@@ -0,0 +1,90 @@
+/*
+ * arch/arm64/mach-vexpress/sleep.S
+ *
+ * Copyright (c) 2013 Marvell Semiconductor Inc.
+ *
+ * Author: Leo Yan <leoy@marvell.com>
+ *
+ * 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.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/assembler.h>
+
+/*
+ * Exits SMP coherency.
+ */
+ENTRY(arm64_cpu_tear_down)
+	mov	x12, lr
+
+	mrs	x0, sctlr_el1
+	bic	x0, x0, #1 << 2			// clear SCTLR.C
+	msr	sctlr_el1, x0
+	isb
+
+	dsb	sy				// ensure ordering with previous memory accesses
+	mrs	x0, clidr_el1			// read clidr
+	and	x3, x0, #0x7000000		// extract loc from clidr
+	lsr	x3, x3, #23			// left align loc bit field
+	cbz	x3, finished			// if loc is 0, then no need to clean
+	mov	x10, #0				// start clean at cache level 0
+
+	add	x2, x10, x10, lsr #1		// work out 3x current cache level
+	lsr	x1, x0, x2			// extract cache type bits from clidr
+	and	x1, x1, #7			// mask of the bits for current cache only
+	cmp	x1, #2				// see what cache we have at this level
+	b.lt	skip				// skip if no cache, or just i-cache
+	save_and_disable_irqs x9		// make CSSELR and CCSIDR access atomic
+	msr	csselr_el1, x10			// select current cache level in csselr
+	isb					// isb to sych the new cssr&csidr
+	mrs	x1, ccsidr_el1			// read the new ccsidr
+	restore_irqs x9
+	and	x2, x1, #7			// extract the length of the cache lines
+	add	x2, x2, #4			// add 4 (line length offset)
+	mov	x4, #0x3ff
+	and	x4, x4, x1, lsr #3		// find maximum number on the way size
+	clz	w5, w4				// find bit position of way size increment
+	mov	x7, #0x7fff
+	and	x7, x7, x1, lsr #13		// extract max number of the index size
+loop2:
+	mov	x9, x4				// create working copy of max way size
+loop3:
+	lsl	x6, x9, x5
+	orr	x11, x10, x6			// factor way and cache number into x11
+	lsl	x6, x7, x2
+	orr	x11, x11, x6			// factor index number into x11
+	dc	cisw, x11			// clean & invalidate by set/way
+	subs	x9, x9, #1			// decrement the way
+	b.ge	loop3
+	subs	x7, x7, #1			// decrement the index
+	b.ge	loop2
+
+skip:
+finished:
+	mov	x10, #0				// swith back to cache level 0
+	msr	csselr_el1, x10			// select current cache level in csselr
+	dsb	sy
+	isb
+
+	mrs	x0, S3_1_C15_C2_1
+	bic	x0, x0, #0x1 << 6		// disable SMP bit
+	msr	S3_1_C15_C2_1, x0
+	dsb	sy
+	isb
+
+	ret	x12
+ENDPROC(arm64_cpu_tear_down)
+
-- 
1.7.9.5

