diff --git a/firmware/export/as3525.h b/firmware/export/as3525.h
index 4a9ab5f..abfcbab 100644
--- a/firmware/export/as3525.h
+++ b/firmware/export/as3525.h
@@ -20,6 +20,8 @@
 #ifndef __AS3525_H__
 #define __AS3525_H__
 
+#define CACHE_SIZE 8 /* in kilobytes */
+
 #define UART_CHANNELS 1
 
 
diff --git a/firmware/export/s3c2440.h b/firmware/export/s3c2440.h
index da2ea89..73db407 100644
--- a/firmware/export/s3c2440.h
+++ b/firmware/export/s3c2440.h
@@ -21,6 +21,8 @@
 #ifndef __S3C2440_H__
 #define __S3C2440_H__
 
+#define CACHE_SIZE 16 /* in kilobytes */
+
 #define LCD_BUFFER_SIZE (320*240*2)
 #define TTB_SIZE (0x4000)
 /* must be 16Kb (0x4000) aligned */
diff --git a/firmware/target/arm/mmu-arm.S b/firmware/target/arm/mmu-arm.S
index 00e4c2b..274cfc2 100644
--- a/firmware/target/arm/mmu-arm.S
+++ b/firmware/target/arm/mmu-arm.S
@@ -274,7 +274,13 @@ clean_dcache_range:
     .global     cpucache_flush      @ Alias
 clean_dcache:
 cpucache_flush:
-    @ Index format: 31:26 = index, 7:5 = segment, remainder = SBZ
+#ifdef HAVE_TEST_AND_CLEAN_CACHE
+    mrc     p15, 0, r15, c7, c10, 3 @ test and clean dcache
+    bne     clean_dcache
+    mov     r0, #0
+#else
+    @ Index format: 31:26 = index, N:5 = segment, remainder = SBZ, assume 64-way set associative separate I/D caches
+    @ N = log2(cache size in bytes / cache line size in bytes == 32) - 6 /* index bits */ + 4 /* start offset */
     mov     r0, #0x00000000         @
 1:  @ clean_start                   @
     mcr     p15, 0, r0, c7, c10, 2  @ Clean entry by index
@@ -284,6 +290,7 @@ cpucache_flush:
     mcr     p15, 0, r0, c7, c10, 2  @ Clean entry by index
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c10, 2  @ Clean entry by index
+#if CACHE_SIZE == 16
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c10, 2  @ Clean entry by index
     add     r0, r0, #0x00000020     @
@@ -293,8 +300,14 @@ cpucache_flush:
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c10, 2  @ Clean entry by index
     sub     r0, r0, #0x000000e0     @
+#elif CACHE_SIZE == 8
+    sub     r0, r0, #0x00000060
+#else
+#error CACHE_SIZE not defined
+#endif
     adds    r0, r0, #0x04000000     @ will wrap to zero at loop end
     bne     1b @ clean_start        @
+#endif /* HAVE_TEST_AND_CLEAN_CACHE */
     mcr     p15, 0, r0, c7, c10, 4  @ Drain write buffer
     bx      lr                      @ 
     .size   clean_dcache, .-clean_dcache
@@ -309,7 +322,13 @@ cpucache_flush:
     .global     invalidate_dcache
     .type       invalidate_dcache, %function
 invalidate_dcache:
-    @ Index format: 31:26 = index, 7:5 = segment, remainder = SBZ
+#ifdef HAVE_TEST_AND_CLEAN_CACHE
+    mrc     p15, 0, r15, c7, c14, 3 @ test, clean and invalidate dcache
+    bne     invalidate_dcache
+    mov     r0, #0
+#else
+    @ Index format: 31:26 = index, N:5 = segment, remainder = SBZ, assume 64-way set associative separate I/D caches
+    @ N = log2(cache size in bytes / cache line size in bytes == 32) - 6 /* index bits */ + 4 /* start offset */
     mov     r0, #0x00000000         @
 1:  @ inv_start                     @
     mcr     p15, 0, r0, c7, c14, 2  @ Clean and invalidate entry by index
@@ -319,6 +338,7 @@ invalidate_dcache:
     mcr     p15, 0, r0, c7, c14, 2  @ Clean and invalidate entry by index
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c14, 2  @ Clean and invalidate entry by index
+#if CACHE_SIZE == 16
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c14, 2  @ Clean and invalidate entry by index
     add     r0, r0, #0x00000020     @
@@ -328,8 +348,14 @@ invalidate_dcache:
     add     r0, r0, #0x00000020     @
     mcr     p15, 0, r0, c7, c14, 2  @ Clean and invalidate entry by index
     sub     r0, r0, #0x000000e0     @
+#elif CACHE_SIZE == 8
+    sub     r0, r0, #0x00000060
+#else
+#error CACHE_SIZE not defined
+#endif
     adds    r0, r0, #0x04000000     @ will wrap to zero at loop end
     bne     1b @ inv_start          @
+#endif /* HAVE_TEST_AND_CLEAN_CACHE */
     mcr     p15, 0, r0, c7, c10, 4  @ Drain write buffer
     bx      lr                      @
     .size   invalidate_dcache, .-invalidate_dcache
diff --git a/tools/configure b/tools/configure
index 8764614..c96925b 100755
--- a/tools/configure
+++ b/tools/configure
@@ -314,7 +314,7 @@ arm946cc () {
 
 arm926ejscc () {
  findarmgcc
- GCCOPTS="$CCOPTS -mcpu=arm926ej-s"
+ GCCOPTS="$CCOPTS -mcpu=arm926ej-s -DHAVE_TEST_AND_CLEAN_CACHE"
  if test "$t_manufacturer" != "as3525" -a "$ARG_ARM_EABI" != "1"; then
   GCCOPTS="$GCCOPTS -mlong-calls"
  fi