/*
 * Hibernation support specific for ARM
 *
 * Copyright (C) 2010 Nokia Corporation
 * Copyright (C) 2010 Texas Instruments, Inc.
 * Copyright (C) 2006 Rafael J. Wysocki <rjw <at> sisk.pl>
 *
 * Contact: Hiroshi DOYU <Hiroshi.DOYU <at> nokia.com>
 *
 * License terms: GNU General Public License (GPL) version 2
 */
#include "pm_types.h"
#include "./pm.h"

unsigned long saved_context_r13_sys[SYS_CONTEXT_SIZE];
unsigned long saved_cpsr_svc;
unsigned long saved_context_r12_svc[SVC_CONTEXT_SIZE];
unsigned long saved_spsr_svc;
unsigned long saved_context_r13_fiq[FIQ_CONTEXT_SIZE];
unsigned long saved_spsr_fiq;
unsigned long saved_context_r13_abt[ABT_CONTEXT_SIZE];
unsigned long saved_spsr_abt;
unsigned long saved_context_r13_irq[IRQ_CONTEXT_SIZE];
unsigned long saved_spsr_irq;
unsigned long saved_context_r13_und[UND_CONTEXT_SIZE];
unsigned long saved_spsr_und;
unsigned long saved_context_r13_mon[MON_CONTEXT_SIZE];
unsigned long saved_spsr_mon;
unsigned long saved_empty_context_svc[EMPTY_CONTEXT_SIZE];

static struct saved_context saved_context;

static struct saved_context default_copro_value = {
	/* CR0 */
	.cssr = 0x00000002,	/* Cache Size Selection */
	/* CR1 */
#ifdef CORTEX_A8
	.cr = 0x00C52078,	/* Control */
	.acr = 0x00000002,	/* Auxiliary Control Register */
	.cacr = 0x00000000,	/* Coprocessor Access Control */
	.sccfgr = 0x00000000,	/* Secure Config Register */
	.scdbgenblr = 0x00000000,	/* Secure Debug Enable Register */
	.nonscacctrlr = 0x00000000,	/* Nonsecure Access Control Register */
#elif defined(CORTEX_A9)
#elif defined(CORTEX_A7)
	.cr = 0x00C50878,	/* Control */
	.acr = 0x00006040,	/* Auxiliary Control Register: needed for smp */
	.cacr = 0x00000000,	/* Coprocessor Access Control */
	.sccfgr = 0x00000000,	/* Secure Config Register */
	.scdbgenblr = 0x00000000,	/* Secure Debug Enable Register */
	.nonscacctrlr = 0x00000000,	/* Nonsecure Access Control Register */
#endif

	/* CR2 */
	.ttb_0r = 0x00000000,	/* Translation Table Base 0 */
	.ttb_1r = 0x00000000,	/* Translation Table Base 1 */
	.ttbcr = 0x00000000,	/* Translation Talbe Base Control */
	/* CR3 */
	.dacr = 0x00000000,	/* Domain Access Control */
	/* CR5 */
	.d_fsr = 0x00000000,	/* Data Fault Status */
	.i_fsr = 0x00000000,	/* Instruction Fault Status */
	.d_afsr = 0x00000000,	/* Data Auxilirary Fault Status */
	.i_afsr = 0x00000000,	/* Instruction Auxilirary Fault Status */
	/* CR6 */
	.d_far = 0x00000000,	/* Data Fault Address */
	.i_far = 0x00000000,	/* Instruction Fault Address */
	/* CR7 */
	.par = 0x00000000,	/* Physical Address */
	/* CR9  FIXME: Are they necessary */
	.pmcontrolr = 0x00000000,	/* Performance Monitor Control */
	.cesr = 0x00000000,	/* Count Enable Set */
	.cecr = 0x00000000,	/* Count Enable Clear */
	.ofsr = 0x00000000,	/* Overflow Flag Status */
#ifdef CORTEX_A8
	.sir = 0x00000000,	/* Software Increment */
#elif defined(CORTEX_A7)
	.sir = 0x00000000,	/* Software Increment */
#endif
	.pcsr = 0x00000000,	/* Performance Counter Selection */
	.ccr = 0x00000000,	/* Cycle Count */
	.esr = 0x00000000,	/* Event Selection */
	.pmcountr = 0x00000000,	/* Performance Monitor Count */
	.uer = 0x00000000,	/* User Enable */
	.iesr = 0x00000000,	/* Interrupt Enable Set */
	.iecr = 0x00000000,	/* Interrupt Enable Clear */
#ifdef CORTEX_A8
	.l2clr = 0x00000000,	/* L2 Cache Lockdown */
	.l2cauxctrlr = 0x00000042,	/* L2 Cache Auxiliary Control */
#elif defined(CORTEX_A7)
#endif

	/* CR10 */
#ifdef CORTEX_A8
	.d_tlblr = 0x00000000,	/* Data TLB Lockdown Register */
	.i_tlblr = 0x00000000,	/* Instruction TLB Lockdown Register */
#elif defined(CORTEX_A7)
#endif
	.prrr = 0x000A81A4,	/* Primary Region Remap Register: tex[0], c, b = 010, + A => normal memory
				 *                        tex[0], c, b = 100 || 101, + 1 => strong order or device
				 */
	.nrrr = 0x44E048E0,	/* Normal Memory Remap Register */
	/* .prrr = 0xFF0A81A8,            Primary Region Remap Register  */
	/* .nrrr = 0x40E040E0,            Normal Memory Remap Register  */
	/* CR11 */
#ifdef CORTEX_A8
	.pleuar = 0x00000000,	/* PLE User Accessibility */
	.plecnr = 0x00000000,	/* PLE Channel Number */
	.plecr = 0x00000000,	/* PLE Control */
	.pleisar = 0x00000000,	/* PLE Internal Start Address */
	.pleiear = 0x00000000,	/* PLE Internal End Address */
	.plecidr = 0x00000000,	/* PLE Context ID */
#elif defined(CORTEX_A7)
#endif

	/* CR12 */
#ifdef CORTEX_A8
	.snsvbar = 0x00000000,	/* Secure or Nonsecure Vector Base Address */
	.monvecbar = 0x00000000,	/*Monitor Vector Base */
#elif defined(CORTEX_A9)
#elif defined(CORTEX_A7)
	/* only accessable from secure world. */

#endif

	/* CR13 */
	.fcse = 0x00000000,	/* FCSE PID */
	.cid = 0x00000000,	/* Context ID */
	.urwtpid = 0x00000000,	/* User read/write Thread and Process ID */
	.urotpid = 0x00000000,	/* User read-only Thread and Process ID */
	.potpid = 0x00000000,	/* Privileged only Thread and Process ID */

};

/*__save_processor_state: store the co-processor state into to location point by ctxt
*@ctxt: indicate where to store co-processor state.
*/
void __save_processor_state(struct saved_context *ctxt)
{
	/* CR0 */
	/* save_mem_status(0x101); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 2, %0, c0, c0, 0" : "=r" (ctxt->cssr));

	/* CR1 */
#ifdef CORTEX_A8
	/* save_mem_status(0x102); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r" (ctxt->cr));
	asm volatile ("mrc p15, 0, %0, c1, c0, 2" : "=r" (ctxt->cacr));
#elif defined(CORTEX_A9)
	asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r" (ctxt->cr));
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c1, c0, 1" : "=r" (ctxt->actlr));
	asm volatile ("mrc p15, 0, %0, c1, c0, 2" : "=r" (ctxt->cacr));
	asm volatile ("mrc p15, 0, %0, c1, c1, 1" : "=r" (ctxt->sder));
	asm volatile ("mrc p15, 0, %0, c1, c1, 3" : "=r" (ctxt->vcr));
#elif defined(CORTEX_A7)
	asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r" (ctxt->cr));
	asm volatile ("mrc p15, 0, %0, c1, c0, 2" : "=r" (ctxt->cacr));
#endif

	/* CR2 */
	/* save_mem_status(0x103); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c2, c0, 0" : "=r" (ctxt->ttb_0r));
	asm volatile ("mrc p15, 0, %0, c2, c0, 1" : "=r" (ctxt->ttb_1r));
	asm volatile ("mrc p15, 0, %0, c2, c0, 2" : "=r" (ctxt->ttbcr));
	/* CR3 */
	/* save_mem_status(0x104); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c3, c0, 0" : "=r" (ctxt->dacr));
	/* CR5 */
	/* save_mem_status(0x105); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c5, c0, 0" : "=r" (ctxt->d_fsr));
	asm volatile ("mrc p15, 0, %0, c5, c0, 1" : "=r" (ctxt->i_fsr));
	asm volatile ("mrc p15, 0, %0, c5, c1, 0" : "=r" (ctxt->d_afsr));
	asm volatile ("mrc p15, 0, %0, c5, c1, 1" : "=r" (ctxt->i_afsr));
	/* save_mem_status(0x106); */
	/* busy_waiting(); */
	/* CR6 */
	asm volatile ("mrc p15, 0, %0, c6, c0, 0" : "=r" (ctxt->d_far));
	asm volatile ("mrc p15, 0, %0, c6, c0, 2" : "=r" (ctxt->i_far));
	/* CR7 */
	/* save_mem_status(0x107); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c7, c4, 0" : "=r" (ctxt->par));
	/* CR9 */
	/* save_mem_status(0x108); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c9, c12, 0" : "=r" (ctxt->pmcontrolr));
	asm volatile ("mrc p15, 0, %0, c9, c12, 1" : "=r" (ctxt->cesr));
	asm volatile ("mrc p15, 0, %0, c9, c12, 2" : "=r" (ctxt->cecr));
	asm volatile ("mrc p15, 0, %0, c9, c12, 3" : "=r" (ctxt->ofsr));
#ifdef CORTEX_A8
	/* save_mem_status(0x109); */
	/* busy_waiting(); */
	/* asm volatile ("mrc p15, 0, %0, c9, c12, 4" : "=r"(ctxt->sir)); */
#elif defined(CORTEX_A7)
	/* busy_waiting(); */
	/* asm volatile ("mrc p15, 0, %0, c9, c12, 4" : "=r"(ctxt->sir)); */
#endif
	/* save_mem_status(0x10a); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c9, c12, 5" : "=r" (ctxt->pcsr));
	asm volatile ("mrc p15, 0, %0, c9, c13, 0" : "=r" (ctxt->ccr));
	asm volatile ("mrc p15, 0, %0, c9, c13, 1" : "=r" (ctxt->esr));
	asm volatile ("mrc p15, 0, %0, c9, c13, 2" : "=r" (ctxt->pmcountr));
	asm volatile ("mrc p15, 0, %0, c9, c14, 0" : "=r" (ctxt->uer));
	asm volatile ("mrc p15, 0, %0, c9, c14, 1" : "=r" (ctxt->iesr));
	asm volatile ("mrc p15, 0, %0, c9, c14, 2" : "=r" (ctxt->iecr));
#ifdef CORTEX_A8
	/* save_mem_status(0x10b); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 1, %0, c9, c0, 0" : "=r" (ctxt->l2clr));
	asm volatile ("mrc p15, 1, %0, c9, c0, 2" : "=r" (ctxt->l2cauxctrlr));
#elif defined(CORTEX_A7)
#endif

	/* CR10 */
#ifdef CORTEX_A8
	/* save_mem_status(0x10c); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c10, c0, 0" : "=r" (ctxt->d_tlblr));
	/* save_mem_status(0x10d); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 0, %0, c10, c0, 1" : "=r" (ctxt->i_tlblr));
#elif defined(CORTEX_A7)

#endif
	asm volatile ("mrc p15, 0, %0, c10, c2, 0" : "=r" (ctxt->prrr));
	asm volatile ("mrc p15, 0, %0, c10, c2, 1" : "=r" (ctxt->nrrr));

	/* CR11 */
#ifdef CORTEX_A8
	asm volatile ("mrc p15, 0, %0, c11, c1, 0" : "=r" (ctxt->pleuar));
	asm volatile ("mrc p15, 0, %0, c11, c2, 0" : "=r" (ctxt->plecnr));
	asm volatile ("mrc p15, 0, %0, c11, c4, 0" : "=r" (ctxt->plecr));
	asm volatile ("mrc p15, 0, %0, c11, c5, 0" : "=r" (ctxt->pleisar));
	asm volatile ("mrc p15, 0, %0, c11, c7, 0" : "=r" (ctxt->pleiear));
	asm volatile ("mrc p15, 0, %0, c11, c15, 0" : "=r" (ctxt->plecidr));
#elif defined(CORTEX_A7)
#endif

	/* CR12 */
#ifdef CORTEX_A8
	asm volatile ("mrc p15, 0, %0, c12, c0, 0" : "=r" (ctxt->snsvbar));
	asm volatile ("mrc p15, 0, %0, c12, c0, 1" : "=r" (ctxt->monvecbar));
#elif defined(CORTEX_A9)
	asm volatile ("mrc p15, 0, %0, c12, c0, 0" : "=r" (ctxt->vbar));
	asm volatile ("mrc p15, 0, %0, c12, c0, 1" : "=r" (ctxt->mvbar));
	asm volatile ("mrc p15, 0, %0, c12, c1, 1" : "=r" (ctxt->vir));
#elif defined(CORTEX_A7)
	/* only accessable from secure world. */
	asm volatile ("mrc p15, 0, %0, c12, c0, 0" : "=r" (ctxt->vbar));
	/* asm volatile ("mrc p15, 0, %0, c12, c0, 1" : "=r"(ctxt->mvbar)); */
	/* asm volatile ("mrc p15, 0, %0, c12, c1, 0" : "=r"(ctxt->isr)); */
#endif

	/* CR13 */
	/* save_mem_status(0x10e); */
	/* busy_waiting(); */
	/* asm volatile ("mrc p15, 0, %0, c13, c0, 0" : "=r"(ctxt->fcse)); */
	asm volatile ("mrc p15, 0, %0, c13, c0, 1" : "=r" (ctxt->cid));
	asm volatile ("mrc p15, 0, %0, c13, c0, 2" : "=r" (ctxt->urwtpid));
	asm volatile ("mrc p15, 0, %0, c13, c0, 3" : "=r" (ctxt->urotpid));
	asm volatile ("mrc p15, 0, %0, c13, c0, 4" : "=r" (ctxt->potpid));
	/* CR15 */
#ifdef CORTEX_A9
	/* save_mem_status(0x10f); */
	/* busy_waiting(); */
	asm volatile ("mrc p15, 5, %0, c15, c7, 2" : "=r" (ctxt->mtlbar));
#endif
}

/*__restore_processor_state: restore the co-processor state according the info point by ctxt
*@ctxt: indicate where to get the backuped co-processor state.
*/
void __restore_processor_state(struct saved_context *ctxt)
{
	/* CR0 */
	asm volatile ("mcr p15, 2, %0, c0, c0, 0" : : "r" (ctxt->cssr));
	/* CR1 */
#if defined(CORTEX_A8)
	/* busy_waiting(); */
	/* asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r"(ctxt->cr)); //will effect visible addr space, cache, instruction prediction */
	asm volatile ("mcr p15, 0, %0, c1, c0, 2" : : "r" (ctxt->cacr));
#elif defined(CORTEX_A9)
	/* asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r"(ctxt->cr)); */
	/* busy_waiting(); */
	asm volatile ("mcr p15, 0, %0, c1, c0, 1" : : "r" (ctxt->actlr));
	asm volatile ("mcr p15, 0, %0, c1, c0, 2" : : "r" (ctxt->cacr));
	asm volatile ("mcr p15, 0, %0, c1, c1, 1" : : "r" (ctxt->sder));
	asm volatile ("mcr p15, 0, %0, c1, c1, 3" : : "r" (ctxt->vcr));
#elif defined(CORTEX_A7)
	asm volatile ("mcr p15, 0, %0, c1, c0, 2" : : "r" (ctxt->cacr));
#endif

	/* CR2 */
	asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r" (ctxt->ttb_0r));
	asm volatile ("dsb");
	asm volatile ("isb");
	asm volatile ("mcr p15, 0, %0, c2, c0, 1" : : "r" (ctxt->ttb_1r));
	asm volatile ("dsb");
	asm volatile ("isb");
	asm volatile ("mcr p15, 0, %0, c2, c0, 2" : : "r" (ctxt->ttbcr));
	asm volatile ("dsb");
	asm volatile ("isb");

	/* flush_tlb_all(); */
	/* CR3 */
	asm volatile ("mcr p15, 0, %0, c3, c0, 0" : : "r" (ctxt->dacr));
	/* CR7 */
	asm volatile ("mcr p15, 0, %0, c7, c4, 0" : : "r" (ctxt->par));

	/* CR9 */
	asm volatile ("mcr p15, 0, %0, c9, c12, 5" : : "r" (ctxt->pcsr));
	asm volatile ("mcr p15, 0, %0, c9, c13, 0" : : "r" (ctxt->ccr));
	asm volatile ("mcr p15, 0, %0, c9, c13, 1" : : "r" (ctxt->esr));
	asm volatile ("mcr p15, 0, %0, c9, c13, 2" : : "r" (ctxt->pmcountr));
	asm volatile ("mcr p15, 0, %0, c9, c14, 0" : : "r" (ctxt->uer));
	asm volatile ("mcr p15, 0, %0, c9, c14, 1" : : "r" (ctxt->iesr));
	asm volatile ("mcr p15, 0, %0, c9, c14, 2" : : "r" (ctxt->iecr));
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 1, %0, c9, c0, 0" : : "r" (ctxt->l2clr));
	asm volatile ("mcr p15, 1, %0, c9, c0, 2" : : "r" (ctxt->l2cauxctrlr));
#elif defined(CORTEX_A7)
#endif

	/* CR10 */
	asm volatile ("mcr p15, 0, %0, c10, c2, 0" : : "r" (ctxt->prrr));
	asm volatile ("mcr p15, 0, %0, c10, c2, 1" : : "r" (ctxt->nrrr));

	/* CR11 */
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 0, %0, c11, c1, 0" : : "r" (ctxt->pleuar));
	asm volatile ("mcr p15, 0, %0, c11, c2, 0" : : "r" (ctxt->plecnr));
	asm volatile ("mcr p15, 0, %0, c11, c4, 0" : : "r" (ctxt->plecr));
	asm volatile ("mcr p15, 0, %0, c11, c5, 0" : : "r" (ctxt->pleisar));
	asm volatile ("mcr p15, 0, %0, c11, c7, 0" : : "r" (ctxt->pleiear));
	asm volatile ("mcr p15, 0, %0, c11, c15, 0" : : "r" (ctxt->plecidr));
#elif defined(CORTEX_A7)
#endif

	/* CR12 */
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 0, %0, c12, c0, 0" : : "r" (ctxt->snsvbar));
	asm volatile ("mcr p15, 0, %0, c12, c0, 1" : : "r" (ctxt->monvecbar));
#elif defined(CORTEX_A9)
	asm volatile ("mcr p15, 0, %0, c12, c0, 0" : : "r" (ctxt->vbar));
	asm volatile ("mcr p15, 0, %0, c12, c0, 1" : : "r" (ctxt->mvbar));
	asm volatile ("mcr p15, 0, %0, c12, c1, 1" : : "r" (ctxt->vir));
#elif defined(CORTEX_A7)
	/* only accessable from secure world. */
	asm volatile ("mcr p15, 0, %0, c12, c0, 0" : : "r" (ctxt->vbar));
	/* asm volatile ("mcr p15, 0, %0, c12, c0, 1" : : "r"(ctxt->mvbar)); */
	/* asm volatile ("mcr p15, 0, %0, c12, c1, 0" : : "r"(ctxt->isr)); */
#endif

	/* CR13 */
	/* asm volatile ("mcr p15, 0, %0, c13, c0, 0" : : "r"(ctxt->fcse)); */
	asm volatile ("mcr p15, 0, %0, c13, c0, 1" : : "r" (ctxt->cid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 2" : : "r" (ctxt->urwtpid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 3" : : "r" (ctxt->urotpid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 4" : : "r" (ctxt->potpid));

	/* CR15 */
#ifdef CORTEX_A9
	asm volatile ("mcr p15, 5, %0, c15, c7, 2" : : "r" (ctxt->mtlbar));
#endif

	asm volatile ("dsb");
	asm volatile ("isb");
}

/*disable_cache_invalidate: disable invalidate cache when cpu reset.
*/
void disable_cache_invalidate(void)
{
#define CPU_CONFIG_REG (0XF1C20D3C)
	__u32 val = *(volatile __u32 *)(CPU_CONFIG_REG);
	val &= (~0x3);
	val |= 0x03;		/* disable invalidate */
	*(volatile __u32 *)(CPU_CONFIG_REG) = val;

	return;
}

/*set_copro_default: set co-processor to default state.
*note: the api will effect visible addr space, be careful to call it.
*/
void set_copro_default(void)
{
	struct saved_context *ctxt = &default_copro_value;
	/* CR0 */
	asm volatile ("mcr p15, 2, %0, c0, c0, 0" : : "r" (ctxt->cssr));
	/* CR1 */
#ifdef CORTEX_A8
	/* busy_waiting(); */
	asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r" (ctxt->cr));	/* will effect visible addr space */
	asm volatile ("mcr p15, 0, %0, c1, c0, 1" : : "r" (ctxt->acr));	/* ? */
	asm volatile ("mcr p15, 0, %0, c1, c0, 2" : : "r" (ctxt->cacr));
	asm volatile ("mcr p15, 0, %0, c1, c1, 0" : : "r" (ctxt->sccfgr));	/* ? */
	asm volatile ("mcr p15, 0, %0, c1, c1, 1" : : "r" (ctxt->scdbgenblr));	/* ? */
	asm volatile ("mcr p15, 0, %0, c1, c1, 2" : : "r" (ctxt->nonscacctrlr));	/* ? */
#elif defined(CORTEX_A7)
	asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r" (ctxt->cr));	/* will effect visible addr space */
	asm volatile ("mcr p15, 0, %0, c1, c0, 1" : : "r" (ctxt->acr));	/* ? */
	asm volatile ("mcr p15, 0, %0, c1, c0, 2" : : "r" (ctxt->cacr));
#endif

	/* CR2 */
	asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r" (ctxt->ttb_0r));
	/* flush_tlb_all(); */
	asm volatile ("mcr p15, 0, %0, c2, c0, 1" : : "r" (ctxt->ttb_1r));
	asm volatile ("mcr p15, 0, %0, c2, c0, 2" : : "r" (ctxt->ttbcr));
	/* CR3 */
	asm volatile ("mcr p15, 0, %0, c3, c0, 0" : : "r" (ctxt->dacr));
	/* CR5 */
	asm volatile ("mcr p15, 0, %0, c5, c0, 0" : : "r" (ctxt->d_fsr));
	asm volatile ("mcr p15, 0, %0, c5, c0, 1" : : "r" (ctxt->i_fsr));
	asm volatile ("mcr p15, 0, %0, c5, c1, 0" : : "r" (ctxt->d_afsr));
	asm volatile ("mcr p15, 0, %0, c5, c1, 1" : : "r" (ctxt->i_afsr));
	/* CR6 */
	asm volatile ("mcr p15, 0, %0, c6, c0, 0" : : "r" (ctxt->d_far));
	asm volatile ("mcr p15, 0, %0, c6, c0, 2" : : "r" (ctxt->i_far));
	/* CR7 */
	asm volatile ("mcr p15, 0, %0, c7, c4, 0" : : "r" (ctxt->par));

	/* CR9 */
	asm volatile ("mcr p15, 0, %0, c9, c14, 0" : : "r" (ctxt->uer));
	asm volatile ("mcr p15, 0, %0, c9, c14, 1" : : "r" (ctxt->iesr));
	asm volatile ("mcr p15, 0, %0, c9, c14, 2" : : "r" (ctxt->iecr));
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 1, %0, c9, c0, 0" : : "r" (ctxt->l2clr));
	asm volatile ("mcr p15, 1, %0, c9, c0, 2" : : "r" (ctxt->l2cauxctrlr));	/* ? */
#elif defined(CORTEX_A7)
#endif

	/* CR10 */
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 0, %0, c10, c0, 0" : : "r" (ctxt->d_tlblr));
	asm volatile ("mcr p15, 0, %0, c10, c0, 1" : : "r" (ctxt->i_tlblr));
#elif defined(CORTEX_A7)
#endif
	asm volatile ("mcr p15, 0, %0, c10, c2, 0" : : "r" (ctxt->prrr));
	asm volatile ("mcr p15, 0, %0, c10, c2, 1" : : "r" (ctxt->nrrr));

	/* CR11 */
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 0, %0, c11, c1, 0" : : "r" (ctxt->pleuar));
	asm volatile ("mcr p15, 0, %0, c11, c2, 0" : : "r" (ctxt->plecnr));
	asm volatile ("mcr p15, 0, %0, c11, c4, 0" : : "r" (ctxt->plecr));
	asm volatile ("mcr p15, 0, %0, c11, c5, 0" : : "r" (ctxt->pleisar));
	asm volatile ("mcr p15, 0, %0, c11, c7, 0" : : "r" (ctxt->pleiear));
	asm volatile ("mcr p15, 0, %0, c11, c15, 0" : : "r" (ctxt->plecidr));
#elif defined(CORTEX_A7)
#endif

	/* CR12 */
#ifdef CORTEX_A8
	asm volatile ("mcr p15, 0, %0, c12, c0, 0" : : "r" (ctxt->snsvbar));
	asm volatile ("mcr p15, 0, %0, c12, c0, 1" : : "r" (ctxt->monvecbar));	/* ? */
#elif defined(CORTEX_A7)
	/* asm volatile ("mcr p15, 0, %0, c12, c1, 0" : : "r"(ctxt->isr)); */
#endif

	/* CR13 */
	/* asm volatile ("mcr p15, 0, %0, c13, c0, 0" : : "r"(ctxt->fcse)); */
	asm volatile ("mcr p15, 0, %0, c13, c0, 1" : : "r" (ctxt->cid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 2" : : "r" (ctxt->urwtpid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 3" : : "r" (ctxt->urotpid));
	asm volatile ("mcr p15, 0, %0, c13, c0, 4" : : "r" (ctxt->potpid));

	asm volatile ("dsb");
	asm volatile ("isb");

	return;
}

/*save_processor_state: save current co-proccessor state.*/
void save_processor_state(void)
{
	__save_processor_state(&saved_context);
}

/*restore_processor_state: restore */
void restore_processor_state(void)
{
	__restore_processor_state(&saved_context);
}

void restore_processor_ttbr0(void)
{
	asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r" (saved_context.ttb_0r));
	return;
}

void set_ttbr0(void)
{
	__u32 ttb = 0;
	/* read ttbr1 */
	asm volatile ("mrc p15, 0, %0, c2, c0, 1" : "=r" (ttb));
	/* write ttbr0 */
	asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r" (ttb));
	asm volatile ("isb");
	return;
}