1 files changed, 71 insertions, 44 deletions

diff --git a/cpukit/score/cpu/arm/aarch32-psma-init.c b/cpukit/score/cpu/arm/aarch32-psma-init.c
index f5792e9c24..93a3673a98 100644
--- a/cpukit/score/cpu/arm/aarch32-psma-init.c
+++ b/cpukit/score/cpu/arm/aarch32-psma-init.c
@@ -10,7 +10,7 @@
  */
 
 /*
- * Copyright (C) 2020 embedded brains GmbH (http://www.embedded-brains.de)
+ * Copyright (C) 2020 embedded brains GmbH & Co. KG
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,15 +45,9 @@
 #include <rtems/score/aarch32-system-registers.h>
 #include <rtems/score/cpu.h>
 
-#define AARCH32_PSMA_REGION_MAX \
+#define AARCH32_PMSA_REGION_MAX \
   ( ( AARCH32_MPUIR_REGION_MASK >> AARCH32_MPUIR_REGION_SHIFT ) + 1 )
 
-typedef struct {
-  uint32_t base;
-  uint32_t limit;
-  uint32_t attributes;
-} AArch32_PMSA_Region;
-
 static void _AArch32_PMSA_Configure(
   const AArch32_PMSA_Region *regions,
   size_t                     region_used,
@@ -96,41 +90,36 @@ static void _AArch32_PMSA_Configure(
   _ARM_Instruction_synchronization_barrier();
 }
 
-void _AArch32_PMSA_Initialize(
-  uint32_t                    memory_attributes_0,
-  uint32_t                    memory_attributes_1,
+size_t _AArch32_PMSA_Map_sections_to_regions(
   const AArch32_PMSA_Section *sections,
-  size_t                      section_count
+  size_t                      section_count,
+  AArch32_PMSA_Region        *regions,
+  size_t                      region_max
 )
 {
-  AArch32_PMSA_Region regions[ AARCH32_PSMA_REGION_MAX ];
   size_t ri;
   size_t si;
   size_t region_used;
-  size_t region_max;
 
-  _AArch32_Write_mair0( memory_attributes_0 );
-  _AArch32_Write_mair1( memory_attributes_1 );
-
-  region_max = ( _AArch32_Read_mpuir() & AARCH32_MPUIR_REGION_MASK ) >>
-    AARCH32_MPUIR_REGION_SHIFT;
   region_used = 0;
 
   for ( si = 0; si < section_count; ++si ) {
     uint32_t base;
-    uint32_t limit;
+    uint32_t end;
     uint32_t attr;
+    uint32_t limit;
 
     base = sections[ si ].begin;
-    limit = sections[ si ].end;
+    end = sections[ si ].end;
     attr = sections[ si ].attributes;
 
-    if ( base == limit ) {
+    if ( base == end ) {
       continue;
     }
 
     base = RTEMS_ALIGN_DOWN( base, AARCH32_PMSA_MIN_REGION_ALIGN );
-    limit = RTEMS_ALIGN_DOWN( limit - 1, AARCH32_PMSA_MIN_REGION_ALIGN );
+    end = RTEMS_ALIGN_UP( end, AARCH32_PMSA_MIN_REGION_ALIGN );
+    limit = end - AARCH32_PMSA_MIN_REGION_ALIGN;
 
     for ( ri = 0; ri < region_used; ++ri ) {
       uint32_t region_base;
@@ -141,29 +130,38 @@ void _AArch32_PMSA_Initialize(
       region_limit = regions[ ri ].limit;
       region_attr = regions[ ri ].attributes;
 
-      if (
-        limit + AARCH32_PMSA_MIN_REGION_ALIGN == region_base &&
-          attr == region_attr
-      ) {
-        /* Merge section with existing region */
-        regions[ ri ].base = base;
-        break;
-      } else if (
-        base == region_limit + AARCH32_PMSA_MIN_REGION_ALIGN &&
-          attr == region_attr
-      ) {
-        /* Merge section with existing region */
-        regions[ ri ].limit = limit;
-        break;
-      } else if ( limit < region_base ) {
+      if ( attr == region_attr ) {
+        uint32_t region_end;
+
+        if ( end - region_base <= AARCH32_PMSA_MIN_REGION_ALIGN ) {
+          /* Extend the region */
+          regions[ ri ].base = base;
+          break;
+        }
+
+        region_end = region_limit + AARCH32_PMSA_MIN_REGION_ALIGN;
+
+        if ( region_end - base <= AARCH32_PMSA_MIN_REGION_ALIGN ) {
+          /* Extend the region */
+          regions[ ri ].limit = limit;
+          break;
+        }
+
+        if ( base >= region_base && end <= region_end ) {
+          /* The section is contained in the region */
+          break;
+        }
+      }
+
+      if ( base <= region_base ) {
         size_t i;
 
         if ( region_used >= region_max ) {
-          return;
+          return 0;
         }
 
-        for ( i = ri; i < region_used; ++i ) {
-          regions[ i + 1 ] = regions[ i ];
+        for ( i = region_used; i > ri; --i ) {
+          regions[ i ] = regions[ i - 1 ];
         }
 
         /* New first region */
@@ -177,18 +175,47 @@ void _AArch32_PMSA_Initialize(
 
     if ( ri == region_used ) {
       if ( region_used >= region_max ) {
-        return;
+        return 0;
       }
 
       /* New last region */
+      ++region_used;
       regions[ ri ].base = base;
       regions[ ri ].limit = limit;
       regions[ ri ].attributes = attr;
-      ++region_used;
     }
   }
 
-  _AArch32_PMSA_Configure( regions, region_used, region_max );
+  return region_used;
+}
+
+void _AArch32_PMSA_Initialize(
+  uint32_t                    memory_attributes_0,
+  uint32_t                    memory_attributes_1,
+  const AArch32_PMSA_Section *sections,
+  size_t                      section_count
+)
+{
+  AArch32_PMSA_Region regions[ AARCH32_PMSA_REGION_MAX ];
+  size_t region_max;
+  size_t region_used;
+
+  _AArch32_Write_mair0( memory_attributes_0 );
+  _AArch32_Write_mair1( memory_attributes_1 );
+
+  region_max = ( _AArch32_Read_mpuir() & AARCH32_MPUIR_REGION_MASK ) >>
+    AARCH32_MPUIR_REGION_SHIFT;
+
+  region_used = _AArch32_PMSA_Map_sections_to_regions(
+    sections,
+    section_count,
+    regions,
+    region_max
+  );
+
+  if ( region_used > 0 ) {
+    _AArch32_PMSA_Configure( regions, region_used, region_max );
+  }
 }
 
 #endif