From 0db7da1858248c079d3b2ed65cce0944aac53236 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Tue, 26 Aug 2014 14:53:19 -0600
Subject: [PATCH] Add up_addrenv_coherent which will be called before address
 environment switches

---
 arch/arm/src/armv7-a/arm_addrenv.c | 50 ++++++++++++++++++++++++++++++
 arch/z80/src/z180/z180_mmu.c       | 23 ++++++++++++++
 binfmt/libelf/libelf_bind.c        | 17 ++++++++--
 include/nuttx/arch.h               | 20 ++++++++++++
 4 files changed, 108 insertions(+), 2 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_addrenv.c b/arch/arm/src/armv7-a/arm_addrenv.c
index a9e6806192..83d4f38563 100644
--- a/arch/arm/src/armv7-a/arm_addrenv.c
+++ b/arch/arm/src/armv7-a/arm_addrenv.c
@@ -661,6 +661,56 @@ int up_addrenv_restore(FAR const save_addrenv_t *oldenv)
   return OK;
 }
 
+/****************************************************************************
+ * Name: up_addrenv_coherent
+ *
+ * Description:
+ *   Flush D-Cache and invalidate I-Cache in preparation for a change in
+ *   address environments.  This should immediately precede a call to
+ *   up_addrenv_select();
+ *
+ * Input Parameters:
+ *   addrenv - Describes the address environment to be made coherent.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int up_addrenv_coherent(FAR const group_addrenv_t *addrenv)
+{
+  uintptr_t vaddr;
+  int i;
+
+  bvdbg("addrenv=%p\n", addrenv);
+  DEBUGASSERT(addrenv);
+
+  /* Invalidate I-Cache */
+
+  cp15_invalidate_icache();
+
+  /* Clean D-Cache in each region. */
+
+#warning REVISIT... causes crashes
+#if 0
+  arch_clean_dcache(CONFIG_ARCH_TEXT_VBASE,
+                    CONFIG_ARCH_TEXT_VBASE +
+                    CONFIG_ARCH_TEXT_NPAGES * MM_PGSIZE - 1);
+
+  arch_clean_dcache(CONFIG_ARCH_DATA_VBASE,
+                    CONFIG_ARCH_DATA_VBASE +
+                    CONFIG_ARCH_DATA_NPAGES * MM_PGSIZE - 1);
+
+#if 0 /* Not yet implemented */
+  arch_clean_dcache(CONFIG_ARCH_HEAP_VBASE,
+                    CONFIG_ARCH_HEAP_VBASE +
+                    CONFIG_ARCH_HEAP_NPAGES * MM_PGSIZE - 1);
+#endif
+#endif
+
+  return OK;
+}
+
 /****************************************************************************
  * Name: up_addrenv_clone
  *
diff --git a/arch/z80/src/z180/z180_mmu.c b/arch/z80/src/z180/z180_mmu.c
index 3aeb644688..e1373851db 100644
--- a/arch/z80/src/z180/z180_mmu.c
+++ b/arch/z80/src/z180/z180_mmu.c
@@ -457,6 +457,29 @@ int up_addrenv_restore(FAR const save_addrenv_t *oldenv)
   return OK;
 }
 
+/****************************************************************************
+ * Name: up_addrenv_coherent
+ *
+ * Description:
+ *   Flush D-Cache and invalidate I-Cache in preparation for a change in
+ *   address environments.  This should immediately precede a call to
+ *   up_addrenv_select();
+ *
+ * Input Parameters:
+ *   addrenv - Describes the address environment to be made coherent.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int up_addrenv_coherent(FAR const group_addrenv_t *addrenv)
+{
+  /* There are no caches */
+
+  return OK;
+}
+
 /****************************************************************************
  * Name: up_addrenv_clone
  *
diff --git a/binfmt/libelf/libelf_bind.c b/binfmt/libelf/libelf_bind.c
index da2dcd4cea..608f65ef7a 100644
--- a/binfmt/libelf/libelf_bind.c
+++ b/binfmt/libelf/libelf_bind.c
@@ -312,16 +312,19 @@ int elf_bind(FAR struct elf_loadinfo_s *loadinfo,
         }
     }
 
-#ifdef CONFIG_ARCH_HAVE_COHERENT_DCACHE
+#if defined(CONFIG_ARCH_ADDRENV)
   /* Ensure that the I and D caches are coherent before starting the newly
    * loaded module by cleaning the D cache (i.e., flushing the D cache
    * contents to memory and invalidating the I cache).
    */
 
+#if 0 /* REVISIT... has some problems */
+  (void)up_addrenv_coherent(&loadinfo->addrenv);
+#else
   up_coherent_dcache(loadinfo->textalloc, loadinfo->textsize);
+  up_coherent_dcache(loadinfo->dataalloc, loadinfo->datasize);
 #endif
 
-#ifdef CONFIG_ARCH_ADDRENV
   /* Restore the original address environment */
 
   status = elf_addrenv_restore(loadinfo);
@@ -333,6 +336,16 @@ int elf_bind(FAR struct elf_loadinfo_s *loadinfo,
           ret = status;
         }
     }
+
+#elif defined(CONFIG_ARCH_HAVE_COHERENT_DCACHE)
+  /* Ensure that the I and D caches are coherent before starting the newly
+   * loaded module by cleaning the D cache (i.e., flushing the D cache
+   * contents to memory and invalidating the I cache).
+   */
+
+  up_coherent_dcache(loadinfo->textalloc, loadinfo->textsize);
+  up_coherent_dcache(loadinfo->dataalloc, loadinfo->datasize);
+
 #endif
 
   return ret;
diff --git a/include/nuttx/arch.h b/include/nuttx/arch.h
index 483ee8e94d..d4b0768212 100644
--- a/include/nuttx/arch.h
+++ b/include/nuttx/arch.h
@@ -858,6 +858,26 @@ int up_addrenv_select(FAR const group_addrenv_t *addrenv,
 int up_addrenv_restore(FAR const save_addrenv_t *oldenv);
 #endif
 
+/****************************************************************************
+ * Name: up_addrenv_coherent
+ *
+ * Description:
+ *   Flush D-Cache and invalidate I-Cache in preparation for a change in
+ *   address environments.  This should immediately precede a call to
+ *   up_addrenv_select();
+ *
+ * Input Parameters:
+ *   addrenv - Describes the address environment to be made coherent.
+ *
+ * Returned Value:
+ *   Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_ADDRENV
+int up_addrenv_coherent(FAR const group_addrenv_t *addrenv);
+#endif
+
 /****************************************************************************
  * Name: up_addrenv_clone
  *