ARMv7: Add support for the Xenon state saving/restoring

Signed-off-by: Catalin Marinas <catalin.marinas@arm.com>

arch/arm/kernel/Makefile

@@ -20,6 +20,7 @@ obj-$(CONFIG_PCI)		+= bios32.o isa.o
 obj-$(CONFIG_SMP)		+= smp.o
 obj-$(CONFIG_KEXEC)		+= machine_kexec.o relocate_kernel.o
 obj-$(CONFIG_OABI_COMPAT)	+= sys_oabi-compat.o
+obj-$(CONFIG_ARM_THUMBEE)	+= thumbee.o
 
 obj-$(CONFIG_CRUNCH)		+= crunch.o crunch-bits.o
 AFLAGS_crunch-bits.o		:= -Wa,-mcpu=ep9312

arch/arm/kernel/asm-offsets.c

@@ -58,6 +58,9 @@ int main(void)
   DEFINE(TI_TP_VALUE,		offsetof(struct thread_info, tp_value));
   DEFINE(TI_FPSTATE,		offsetof(struct thread_info, fpstate));
   DEFINE(TI_VFPSTATE,		offsetof(struct thread_info, vfpstate));
+#ifdef CONFIG_ARM_THUMBEE
+  DEFINE(TI_THUMBEE_STATE,	offsetof(struct thread_info, thumbee_state));
+#endif
 #ifdef CONFIG_IWMMXT
   DEFINE(TI_IWMMXT_STATE,	offsetof(struct thread_info, fpstate.iwmmxt));
 #endif

arch/arm/kernel/entry-armv.S

@@ -628,6 +628,12 @@ ENTRY(__switch_to)
 	add	ip, r1, #TI_CPU_SAVE
 	ldr	r3, [r2, #TI_TP_VALUE]
 	stmia	ip!, {r4 - sl, fp, sp, lr}	@ Store most regs on stack
+#ifdef CONFIG_ARM_XENON
+	mrc	p14, 6, r4, c1, c0, 0		@ current xenon state
+	ldr	r5, [r2, #TI_XENONSTATE]	@ value to restore
+	str	r4, [r1, #TI_XENONSTATE]	@ save current
+	mcr	p14, 6, r5, c1, c0, 0		@ restore new value
+#endif
 #ifdef CONFIG_MMU
 	ldr	r6, [r2, #TI_CPU_DOMAIN]
 #endif

arch/arm/kernel/thumbee.c

+/*
+ * arch/arm/kernel/thumbee.c
+ *
+ * Copyright (C) 2008 ARM Limited
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+
+#include <asm/thread_notify.h>
+
+/*
+ * Access to the ThumbEE Handler Base register
+ */
+static inline unsigned long teehbr_read()
+{
+	unsigned long v;
+	asm("mrc	p14, 6, %0, c1, c0, 0\n" : "=r" (v));
+	return v;
+}
+
+static inline void teehbr_write(unsigned long v)
+{
+	asm("mcr	p14, 6, %0, c1, c0, 0\n" : : "r" (v));
+}
+
+static int thumbee_notifier(struct notifier_block *self, unsigned long cmd, void *t)
+{
+	struct thread_info *thread = t;
+
+	switch (cmd) {
+	case THREAD_NOTIFY_FLUSH:
+		thread->thumbee_state = 0;
+		break;
+	case THREAD_NOTIFY_SWITCH:
+		current_thread_info()->thumbee_state = teehbr_read();
+		teehbr_write(thread->thumbee_state);
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block thumbee_notifier_block = {
+	.notifier_call	= thumbee_notifier,
+};
+
+static int __init thumbee_init(void)
+{
+	unsigned long pfr0;
+	unsigned int cpu_arch = cpu_architecture();
+
+	if (cpu_arch < CPU_ARCH_ARMv7)
+		return 0;
+
+	/* processor feature register 0 */
+	asm("mrc	p15, 0, %0, c0, c1, 0\n" : "=r" (pfr0));
+	if ((pfr0 & 0x0000f000) != 0x00001000)
+		return 0;
+
+	printk(KERN_INFO "ThumbEE CPU extension supported.\n");
+	elf_hwcap |= HWCAP_THUMBEE;
+	thread_register_notifier(&thumbee_notifier_block);
+
+	return 0;
+}
+
+late_initcall(thumbee_init);

arch/arm/mm/Kconfig

@@ -550,6 +550,13 @@ config ARM_THUMB
 
 	  If you don't know what this all is, saying Y is a safe choice.
 
+config ARM_THUMBEE
+	bool "Enable ThumbEE CPU extension"
+	depends on CPU_V7
+	help
+	  Say Y here if you have a CPU with the ThumbEE extension and code to
+	  make use of it. Say N for code that can run on CPUs without ThumbEE.
+
 config CPU_BIG_ENDIAN
 	bool "Build big-endian kernel"
 	depends on ARCH_SUPPORTS_BIG_ENDIAN

include/asm-arm/hwcap.h

@@ -15,6 +15,7 @@
 #define HWCAP_JAVA	256
 #define HWCAP_IWMMXT	512
 #define HWCAP_CRUNCH	1024
+#define HWCAP_THUMBEE	2048
 
 #if defined(__KERNEL__) && !defined(__ASSEMBLY__)
 /*

include/asm-arm/thread_info.h

@@ -62,6 +62,9 @@ struct thread_info {
 	struct crunch_state	crunchstate;
 	union fp_state		fpstate __attribute__((aligned(8)));
 	union vfp_state		vfpstate;
+#ifdef CONFIG_ARM_THUMBEE
+	unsigned long		thumbee_state;	/* ThumbEE Handler Base register */
+#endif
 	struct restart_block	restart_block;
 };