Ticket #3007: cache_invalidate.diff

c/src/lib/libbsp/arm/shared/arm-l2c-310/cache_.h

@@ -945 +945 @@
 static inline void
 l2c_310_invalidate_range( const void* d_addr, const size_t n_bytes )
 {
-  /* Back starting address up to start of a line and invalidate until ADDR_LAST */
-  uint32_t       adx               = (uint32_t)d_addr
-    & ~L2C_310_DATA_LINE_MASK;
-  const uint32_t ADDR_LAST         =
-    (uint32_t)( (size_t)d_addr + n_bytes - 1 );
-  uint32_t       block_end         =
-    L2C_310_MIN( ADDR_LAST, adx + L2C_310_MAX_LOCKING_BYTES );
+  uint32_t       adx               = (uint32_t)d_addr;
+  uint32_t       end               =
+    (uint32_t)( (size_t)d_addr + n_bytes );
   volatile L2CC *l2cc = (volatile L2CC *) BSP_ARM_L2C_310_BASE;
 
   if ( n_bytes == 0 ) {
     return;
   }
 
-  for (;
-       adx      <= ADDR_LAST;
-       adx       = block_end + 1,
-       block_end = L2C_310_MIN( ADDR_LAST, adx + L2C_310_MAX_LOCKING_BYTES )) {
+  if ( adx & L2C_310_DATA_LINE_MASK ) {
+    /* flush start line in case of misalignment */
+    adx &= ~L2C_310_DATA_LINE_MASK;
+    l2c_310_flush_1_line( l2cc, adx );
+    adx += CPU_DATA_CACHE_ALIGNMENT;
+  }
+
+  if ( end & L2C_310_DATA_LINE_MASK ) {
+    /* flush end line in case of misalignment */
+    end &= ~L2C_310_DATA_LINE_MASK;
+    l2c_310_flush_1_line( l2cc, end );
+  }
+
+  while (adx < end) {
+    uint32_t block_end = L2C_310_MIN( end, adx + L2C_310_MAX_LOCKING_BYTES );
     rtems_interrupt_lock_context lock_context;
 
     rtems_interrupt_lock_acquire( &l2c_310_lock, &lock_context );
 
-    for (; adx <= block_end; adx += CPU_DATA_CACHE_ALIGNMENT ) {
+    for (; adx < block_end; adx += CPU_DATA_CACHE_ALIGNMENT ) {
       /* Invalidate L2 cache line */
       l2cc->inv_pa = adx;
     }

c/src/lib/libbsp/arm/shared/include/arm-cache-l1.h

@@ -268 +268 @@
 )
 {
   if ( n_bytes != 0 ) {
-    uint32_t       adx = (uint32_t) d_addr
-                         & ~ARM_CACHE_L1_DATA_LINE_MASK;
-    const uint32_t end =
-      (uint32_t)( (size_t)d_addr + n_bytes -1);
+    uint32_t       adx = (uint32_t) d_addr;
+    uint32_t       end =
+      (uint32_t)( (size_t)d_addr + n_bytes );
 
     arm_cache_l1_errata_764369_handler();
     
-    /* Back starting address up to start of a line and invalidate until end */
-    for (;
-         adx <= end;
-         adx += ARM_CACHE_L1_CPU_DATA_ALIGNMENT ) {
-        /* Invalidate the Instruction cache line */
-        arm_cp15_data_cache_invalidate_line( (void*)adx );
+    if (adx & ARM_CACHE_L1_DATA_LINE_MASK) {
+      /* flush start line in case of misalignment */
+      adx &= ~ARM_CACHE_L1_DATA_LINE_MASK;
+      arm_cp15_data_cache_clean_and_invalidate_line( (void*)adx );
+      adx += ARM_CACHE_L1_CPU_DATA_ALIGNMENT;
     }
+
+    if (end & ARM_CACHE_L1_DATA_LINE_MASK) {
+      /* flush end line in case of misalignment */
+      end &= ~ARM_CACHE_L1_DATA_LINE_MASK;
+      arm_cp15_data_cache_clean_and_invalidate_line( (void*)end );
+    }
+
+    while (adx < end) {
+      /* Invalidate the Data cache line */
+      arm_cp15_data_cache_invalidate_line( (void*)adx );
+      adx += ARM_CACHE_L1_CPU_DATA_ALIGNMENT;
+    }
+
     /* Wait for L1 invalidate to complete */
     _ARM_Data_synchronization_barrier();
   }