SAMA5: More cache and mmu inline utility functions

5 files changed, 265 insertions, 11 deletions

diff --git a/nuttx/arch/arm/src/armv7-a/cache.h b/nuttx/arch/arm/src/armv7-a/cache.h
index 1845ebde4..50e9be2c1 100644
--- a/nuttx/arch/arm/src/armv7-a/cache.h
+++ b/nuttx/arch/arm/src/armv7-a/cache.h
@@ -194,6 +194,27 @@
 #ifdef __ASSEMBLY__
 
 /************************************************************************************
+ * Name: cp15_disable_caches
+ *
+ * Description:
+ *   Disable L1 Caches
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ************************************************************************************/
+
+	.macro	cp15_invalidate_icache_inner_sharable, tmp
+	mrc		p15, 0, \tmp, c1, c0, 0		/* Read SCTLR */
+	bic		\tmp, \tmp, #(0x1 << 12)	/* Disable I cache */
+	bic		\tmp, \tmp, #(0x1 << 2)		/* Disable D cache */
+	mcr		p15, 0, \tmp, c1, c0, 0		/* Updagte the SCTLR */
+	.endm
+
+/************************************************************************************
  * Name: cp15_invalidate_icache_inner_sharable
  *
  * Description:
@@ -441,6 +462,34 @@
 #ifndef __ASSEMBLY__
 
 /************************************************************************************
+ * Name: cp15_disable_caches
+ *
+ * Description:
+ *   Disable L1 Caches
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ************************************************************************************/
+
+static inline void cp15_disable_caches(void)
+{
+  __asm__ __volatile__
+    (
+      "\tmrc  p15, 0, r0, c1, c0, 0\n"  /* Read SCTLR */
+      "\tbic  r0, r0, #(0x1 << 12)\n"   /* Disable I cache */
+      "\tbic  r0, r0, #(0x1 << 2)\n"    /* Disable D cache */
+      "\tmcr  p15, 0, r0, c1, c0, 0\n"  /* Updagte the SCTLR */
+      :
+      :
+      : "r0", "memory"
+    );
+}
+
+/************************************************************************************
  * Name: cp15_invalidate_icache_inner_sharable
  *
  * Description:
@@ -834,6 +883,22 @@ void cp15_coherent_dcache(uintptr_t start, uintptr_t end);
 void cp15_invalidate_dcache(uintptr_t start, uintptr_t end);
 
 /****************************************************************************
+ * Name: cp15_invalidate_dcache_all
+ *
+ * Description:
+ *   Invalidate the entire contents of D cache.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void cp15_invalidate_dcache_all(void);
+
+/****************************************************************************
  * Name: cp15_clean_dcache
  *
  * Description:
diff --git a/nuttx/arch/arm/src/armv7-a/cp15_invalidate_dcache_all.S b/nuttx/arch/arm/src/armv7-a/cp15_invalidate_dcache_all.S
new file mode 100755
index 000000000..c6bbc74ab
--- /dev/null
+++ b/nuttx/arch/arm/src/armv7-a/cp15_invalidate_dcache_all.S
@@ -0,0 +1,114 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/cp15_invalidate_dcache_all.S
+ *
+ *   Copyright (C) 2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * References:
+ *
+ *  "Cortex-A5™ MPCore, Technical Reference Manual", Revision: r0p1,
+ *   Copyright © 2010  ARM. All rights reserved. ARM DDI 0434B (ID101810)
+ *  "ARM® Architecture Reference Manual, ARMv7-A and ARMv7-R edition",
+ *   Copyright © 1996-1998, 2000, 2004-2012 ARM. All rights reserved. ARM
+ *   DDI 0406C.b (ID072512)
+ *
+ * Portions of this file derive from Atmel sample code for the SAMA5D3 Cortex-A5
+ * which also has a modified BSD-style license:
+ *
+ *   Copyright (c) 2012, Atmel Corporation
+ *   All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor Atmel nor the names of the contributors may
+ *    be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "cp15.h"
+
+	.file	"cp15_invalidate_dcache_all.S"
+
+/****************************************************************************
+ * Preprocessor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Symbols
+ ****************************************************************************/
+
+	.globl	cp15_invalidate_dcache_all
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+	.text
+
+/****************************************************************************
+ * Name: cp15_invalidate_dcache_all
+ *
+ * Description:
+ *   Invalidate the entire contents of D cache.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+	.globl	cp15_invalidate_dcache_all
+	.type	cp15_invalidate_dcache_all, function
+
+cp15_invalidate_dcache_all:
+
+	mrc		CP15_CCSIDR(r0)			/* Read the Cache Size Identification Register */
+	ldr		r3, =0xffff				/* Isolate the NumSets field (bits 13-27) */
+	and		r0, r3, r0, lsr #13		/* r0=NumSets (number of sets - 1) */
+
+	mov		r1, #0					/* r1 = way loop counter */
+way_loop:
+	mov		r3, #0					/* r3 = set loop counter */
+set_loop:
+	mov		r2, r1, lsl #30			/* r2 = way loop counter << 30 */
+	orr		r2, r3, lsl #5			/* r2 = set/way cache operation format */
+	mcr		CP15_DCISW(r2)			/* Data Cache Invalidate by Set/Way */
+	add		r3, r3, #1				/* Increment set counter */
+	cmp		r0, r3					/* Last set? */
+	bne		set_loop				/* Keep looping if not */
+	add		r1, r1, #1				/* Increment the way counter */
+	cmp		r1, #4					/* Last way? (four ways assumed) */
+	bne		way_loop				/* Keep looping if not */
+
+	dsb
+	bx		lr
+	.size cp15_invalidate_dcache_all, . - cp15_invalidate_dcache_all
+	.end
diff --git a/nuttx/arch/arm/src/armv7-a/mmu.h b/nuttx/arch/arm/src/armv7-a/mmu.h
index 5012920c8..4cd7fad10 100644
--- a/nuttx/arch/arm/src/armv7-a/mmu.h
+++ b/nuttx/arch/arm/src/armv7-a/mmu.h
@@ -794,6 +794,38 @@
 #ifdef __ASSEMBLY__
 
 /************************************************************************************
+ * Name: cp15_disable_mmu
+ *
+ * Description:
+ *   Disable the MMU
+ *
+ * Inputs:
+ *   None
+ *
+ ************************************************************************************/
+
+	.macro	cp15_disable_mmu, scratch
+	mrc		p15, 0, \scratch, c1, c0, 0
+	bic		\scratch, \scratch, #1
+	mcr		p15, 0, \scratch, c1, c0, 0
+	.endm
+
+/************************************************************************************
+ * Name: cp15_invalidate_tlbs
+ *
+ * Description:
+ *   Invalidate TLBs
+ *
+ * Inputs:
+ *   None
+ *
+ ************************************************************************************/
+
+	.macro	cp15_invalidate_tlbs, scratch
+	mcr		p15, 0, \scratch, c8, c7, 0	/* TLBIALL */
+	.endm
+
+/************************************************************************************
  * Name: cp15_wrdacr
  *
  * Description:
@@ -1000,13 +1032,51 @@
 
 #ifndef __ASSEMBLY__
 
-#endif /* __ASSEMBLY__ */
+/************************************************************************************
+ * Name: cp15_disable_mmu
+ *
+ * Description:
+ *   Disable the MMU
+ *
+ * Inputs:
+ *   None
+ *
+ ************************************************************************************/
 
-/********************************************************************************************
- * Inline Functions
+static inline void cp15_disable_mmu(void)
+{
+  __asm__ __volatile__
+    (
+      "\tmrc p15, 0, r0, c1, c0, 0\n"
+      "\tbic r0, r0, #1\n"
+      "\tmcr p15, 0, r0, c1, c0, 0\n"
+      :
+      :
+      : "r0", "memory"
+    );
+}
+
+/************************************************************************************
+ * Name: cp15_invalidate_tlbs
+ *
+ * Description:
+ *   Invalidate TLBs
+ *
+ * Inputs:
+ *   None
+ *
  ************************************************************************************/
 
-#ifndef __ASSEMBLY__
+static inline void cp15_invalidate_tlbs(void)
+{
+  __asm__ __volatile__
+    (
+      "\tmcr p15, 0, r0, c8, c7, 0\n" /* TLBIALL */
+      :
+      :
+      : "r0", "memory"
+    );
+}
 
 /************************************************************************************
  * Name: cp15_wrdacr
diff --git a/nuttx/arch/arm/src/sama5/Make.defs b/nuttx/arch/arm/src/sama5/Make.defs
index 8f740a730..101e3f80c 100644
--- a/nuttx/arch/arm/src/sama5/Make.defs
+++ b/nuttx/arch/arm/src/sama5/Make.defs
@@ -39,7 +39,7 @@ CMN_ASRCS  = arm_head.S
 CMN_ASRCS += arm_vectors.S arm_fpuconfig.S arm_fullcontextrestore.S
 CMN_ASRCS += arm_saveusercontext.S arm_vectoraddrexcptn.S arm_vfork.S
 CMN_ASRCS += cp15_coherent_dcache.S cp15_invalidate_dcache.S
-CMN_ASRCS += cp15_clean_dcache.S cp15_flush_dcache.S
+CMN_ASRCS += cp15_clean_dcache.S cp15_flush_dcache.S cp15_invalidate_dcache_all.S
 
 CMN_CSRCS  = up_initialize.c up_idle.c up_interruptcontext.c up_exit.c
 CMN_CSRCS += up_createstack.c up_releasestack.c up_usestack.c up_vfork.c
diff --git a/nuttx/configs/sama5d3x-ek/src/nor_main.c b/nuttx/configs/sama5d3x-ek/src/nor_main.c
index 96d881dfb..96b576ac2 100644
--- a/nuttx/configs/sama5d3x-ek/src/nor_main.c
+++ b/nuttx/configs/sama5d3x-ek/src/nor_main.c
@@ -43,7 +43,8 @@
 #include <debug.h>
 
 #include "up_arch.h"
-#include "sctlr.h"
+#include "mmu.h"
+#include "cache.h"
 #include "sam_periphclks.h"
 #include "chip/sam_hsmc.h"
 
@@ -120,11 +121,15 @@ int nor_main(int argc, char *argv)
    * virtual addressing.
    */
 
-#if 0 /* Causes a crash */
-  printf("Disabling the caches and the MMU\n");
-  regval  = cp15_rdsctlr();
-  regval &= ~(SCTLR_M | SCTLR_C | SCTLR_I);
-  cp15_wrsctlr(regval);
+#if 0 /* Causes crashes */
+  cp15_disable_mmu();
+  cp15_disable_caches();
+
+  /* Invalidate caches and TLBs */
+
+  cp15_invalidate_icache();
+  cp15_invalidate_dcache_all();
+  cp15_invalidate_tlbs();
 #endif
 
 #ifdef SAMA5_NOR_START