1 files changed, 65 insertions, 32 deletions

diff --git a/lib/ultralib/src/os/invaldcache.s b/lib/ultralib/src/os/invaldcache.s
index ea6a657..cabad5b 100644
--- a/lib/ultralib/src/os/invaldcache.s
+++ b/lib/ultralib/src/os/invaldcache.s
@@ -3,56 +3,89 @@
 #include "sys/regdef.h"
 
 .text
+/**
+ *  void osInvalDCache(void* vaddr, s32 nbytes);
+ *
+ *  Invalidates the CPU Data Cache for `nbytes` at `vaddr`.
+ *  The cache is not automatically synced with physical memory, so cache
+ *  lines must be invalidated to ensure old data is not used in place of
+ *  newly available data supplied by an external agent in a DMA operation.
+ *
+ *  If `vaddr` is not aligned to a cache line boundary, or nbytes is not a
+ *  multiple of the data cache line size (16 bytes) a larger region is
+ *  invalidated.
+ *
+ *  If the amount to invalidate is at least the data cache size (DCACHE_SIZE),
+ *  the entire data cache is invalidated.
+ */
 LEAF(osInvalDCache)
-    blez a1, 3f
-    li t3, DCACHE_SIZE
-    bgeu a1, t3, 4f
+    /* If the amount to invalidate is less than or equal to 0, return immediately */
+    blez    a1, 3f
+    /*
+     * If the amount to invalidate is as large as or larger than
+     * the data cache size, invalidate all
+     */
+    li      t3, DCACHE_SIZE
+    bgeu    a1, t3, 4f
 
+    /*
+     * Ensure end address does not wrap around and end up smaller
+     * than the start address
+     */
+    move    t0, a0
+    addu    t1, a0, a1
+    bgeu    t0, t1, 3f
 
-    move t0, a0
-    addu t1, a0, a1
-    bgeu t0, t1, 3f
+    /* Mask start with cache line */
+    addiu   t1, t1, -DCACHE_LINESIZE
+    andi    t2, t0, DCACHE_LINEMASK
+    /* If mask is not zero, the start is not cache aligned */
+    beqz    t2, 1f
 
+    /* Subtract mask result to align to cache line */
+    subu    t0, t0, t2
+    /* Hit-Writeback-Invalidate unaligned part */
+    CACHE(  (C_HWBINV | CACH_PD), (t0))
+    /* If that is all there is to do, return early */
+    bgeu    t0, t1, 3f
 
-    addiu t1, t1, -DCACHE_LINESIZE
-    andi t2, t0, DCACHE_LINEMASK
-    beqz t2, 1f
 
-
-    subu t0, t0, t2
-    CACHE((C_HWBINV|CACH_PD), (t0))
-    bgeu t0, t1, 3f
-
-
-    addiu t0, t0, DCACHE_LINESIZE
+    addiu   t0, t0, DCACHE_LINESIZE
 1:
-    andi t2, t1, DCACHE_LINEMASK
-    beqz t2, 2f
+    /* Mask end with cache line */
+    andi    t2, t1, DCACHE_LINEMASK
+    /* If mask is not zero, the end is not cache aligned */
+    beqz    t2, 2f
+
+    /* Subtract mask result to align to cache line */
+    subu    t1, t1, t2
 
-    subu t1, t1, t2
-    CACHE((C_HWBINV|CACH_PD), 0x10(t1))
-    bltu t1, t0, 3f
+    /* Hit-Writeback-Invalidate unaligned part */
+    CACHE(  (C_HWBINV | CACH_PD), DCACHE_LINESIZE(t1))
+    bltu    t1, t0, 3f
 
 2:
-    CACHE((C_HINV|CACH_PD), (t0))
+    /* Hit-Invalidate */
+    CACHE(  (C_HINV | CACH_PD), (t0))
     .set noreorder
-    bltu t0, t1, 2b
-    addiu t0, t0, DCACHE_LINESIZE
+    bltu    t0, t1, 2b
+     addiu  t0, t0, DCACHE_LINESIZE
     .set reorder
 3:
-    jr ra
+    jr      ra
 
 4:
-    li t0, KUSIZE
-    addu t1, t0, t3
-    addiu t1, t1, -DCACHE_LINESIZE
+    li      t0, K0BASE
+    addu    t1, t0, t3
+    addiu   t1, t1, -DCACHE_LINESIZE
 5:
-    CACHE((C_IINV|CACH_PD), (t0))
+    /* Index-Writeback-Invalidate */
+    CACHE(  (C_IINV | CACH_PD), (t0))
     .set noreorder
-    bltu t0, t1, 5b
-    addiu t0, t0, DCACHE_LINESIZE
+    bltu    t0, t1, 5b
+     addiu  t0, t0, DCACHE_LINESIZE
     .set reorder
 
-    jr ra
+    jr      ra
 
 END(osInvalDCache)