Split the cmmu code routines into single 88110 (MVME197LE) and 88110+88410

combos (MVME197SP/DP), and implement supposedly smarter cache routines.

There is still room for improvement, however, cache flush operation errata
permissing.

Tested on 197LE and 197DP.

3 files changed, 364 insertions, 173 deletions

diff --git a/sys/arch/mvme88k/include/m88110.h b/sys/arch/mvme88k/include/m88110.h
index b727a6447d4..7b7b9f3f452 100644
--- a/sys/arch/mvme88k/include/m88110.h
+++ b/sys/arch/mvme88k/include/m88110.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: m88110.h,v 1.20 2007/11/22 23:31:55 miod Exp $ */
+/*	$OpenBSD: m88110.h,v 1.21 2007/11/22 23:33:41 miod Exp $ */
 
 #ifndef	__MACHINE_M88110_H__
 #define	__MACHINE_M88110_H__
@@ -6,31 +6,6 @@
 /*
  *	88110 CMMU definitions
  */
-#define CMMU_ICMD 0
-#define CMMU_ICTL 1
-#define CMMU_ISAR 2
-#define CMMU_ISAP 3
-#define CMMU_IUAP 4
-#define CMMU_IIR  5
-#define CMMU_IBP  6
-#define CMMU_IPPU 7
-#define CMMU_IPPL 8
-#define CMMU_ISR  9
-#define CMMU_ILAR 10
-#define CMMU_IPAR 11
-
-#define CMMU_DCMD 12
-#define CMMU_DCTL 13
-#define CMMU_DSAR 14
-#define CMMU_DSAP 15
-#define CMMU_DUAP 16
-#define CMMU_DIR  17
-#define CMMU_DBP  18
-#define CMMU_DPPU 19
-#define CMMU_DPPL 20
-#define CMMU_DSR  21
-#define CMMU_DLAR 22
-#define CMMU_DPAR 23
 
 #define CMMU_ICMD_INV_ITIC       0x001    /* Invalidate Inst Cache & TIC */
 #define CMMU_ICMD_INV_TIC        0x002    /* Invalidate TIC */
@@ -105,11 +80,6 @@
 #define CMMU_DSR_WA              0x000002 /* Write-Allocate Bus Error */
 #define CMMU_DSR_BE              0x000001 /* Bus Error */
 
-#define CMMU_READ 0
-#define CMMU_WRITE 1
-#define CMMU_DATA 1
-#define CMMU_INST 0
-
 /* definitions for use of the BATC */
 #define BATC_512K	(0x00 << BATC_BLKSHIFT)
 #define BATC_1M		(0x01 << BATC_BLKSHIFT)
@@ -120,8 +90,11 @@
 #define BATC_32M	(0x3f << BATC_BLKSHIFT)
 #define BATC_64M	(0x7f << BATC_BLKSHIFT)
 
-#define CLINE_MASK	0x1f
-#define CLINE_SIZE	(8 * 32)
+/*
+ * Cache line information
+ */
+#define	MC88110_CACHE_SHIFT	5
+#define	MC88110_CACHE_LINE	(1 << MC88110_CACHE_SHIFT)
 
 #ifndef	_LOCORE
 
@@ -168,92 +141,75 @@ u_int get_dppu(void);
 u_int get_dppl(void);
 u_int get_dsr(void);
 
-/* Cache inlines */
-
-#define line_addr(x)	(paddr_t)((x) & ~CLINE_MASK)
-#define page_addr(x)	(paddr_t)((x) & ~PAGE_MASK)
-
 /*
- * 88110 general information #22:
- * ``Issuing a command to flush and invalidate the data cache while the
- *   dcache is disabled (CEN = 0 in dctl) will cause problems.  Do not
- *   flush a disabled data cache.  In general, there is no reason to
- *   perform this operation with the cache disabled, since it may be
- *   incoherent with the proper state of memory.  Before 5.0 the flush
- *   command was treated like a nop when the cache was disabled.  This
- *   is no longer the case.''
+ * The following inlines expect their address to be line-aligned for line
+ * operations, and page aligned for page operations.
  */
 
-static __inline__ void mc88110_flush_data_line(paddr_t x)
+/*
+static __inline__ void
+mc88110_flush_data_line(paddr_t x)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dsar(line_addr(x));
-		set_dcmd(CMMU_DCMD_FLUSH_LINE);
-	}
+	set_dsar(x);
+	set_dcmd(CMMU_DCMD_FLUSH_LINE);
 }
 
-static __inline__ void mc88110_flush_data_page(paddr_t x)
+static __inline__ void
+mc88110_flush_data_page(paddr_t x)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dsar(page_addr(x));
-		set_dcmd(CMMU_DCMD_FLUSH_PG);
-	}
+	set_dsar(x);
+	set_dcmd(CMMU_DCMD_FLUSH_PG);
 }
+*/
 
-static __inline__ void mc88110_flush_data(void)
+static __inline__ void
+mc88110_flush_data(void)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dcmd(CMMU_DCMD_FLUSH_ALL);
-	}
+	set_dcmd(CMMU_DCMD_FLUSH_ALL);
 }
 
-static __inline__ void mc88110_inval_data_line(paddr_t x)
+static __inline__ void
+mc88110_inval_data_line(paddr_t x)
 {
-	set_dsar(line_addr(x));
+	set_dsar(x);
 	set_dcmd(CMMU_DCMD_INV_LINE);
 }
 
-static __inline__ void mc88110_inval_data(void)
+static __inline__ void
+mc88110_inval_data(void)
 {
 	set_dcmd(CMMU_DCMD_INV_ALL);
 }
 
-static __inline__ void mc88110_sync_data_line(paddr_t x)
+static __inline__ void
+mc88110_sync_data_line(paddr_t x)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dsar(line_addr(x));
-		set_dcmd(CMMU_DCMD_FLUSH_LINE_INV);
-	}
+	set_dsar(x);
+	set_dcmd(CMMU_DCMD_FLUSH_LINE_INV);
 }
 
-static __inline__ void mc88110_sync_data_page(paddr_t x)
+static __inline__ void
+mc88110_sync_data_page(paddr_t x)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dsar(page_addr(x));
-		set_dcmd(CMMU_DCMD_FLUSH_PG_INV);
-	}
+	set_dsar(x);
+	set_dcmd(CMMU_DCMD_FLUSH_PG_INV);
 }
 
-static __inline__ void mc88110_sync_data(void)
+static __inline__ void
+mc88110_sync_data(void)
 {
-	u_int dctl = get_dctl();
-	if (dctl & CMMU_DCTL_CEN) {
-		set_dcmd(CMMU_DCMD_FLUSH_ALL_INV);
-	}
+	set_dcmd(CMMU_DCMD_FLUSH_ALL_INV);
 }
 
-static __inline__ void mc88110_inval_inst_line(paddr_t x)
+static __inline__ void
+mc88110_inval_inst_line(paddr_t x)
 {
-	set_isar(line_addr(x));
+	set_isar(x);
 	set_icmd(CMMU_ICMD_INV_LINE);
 }
 
-static __inline__ void mc88110_inval_inst(void)
+static __inline__ void
+mc88110_inval_inst(void)
 {
 	set_icmd(CMMU_ICMD_INV_ITIC);
 }
diff --git a/sys/arch/mvme88k/mvme88k/m197_machdep.c b/sys/arch/mvme88k/mvme88k/m197_machdep.c
index 6a1addea5f1..b915efbb5f6 100644
--- a/sys/arch/mvme88k/mvme88k/m197_machdep.c
+++ b/sys/arch/mvme88k/mvme88k/m197_machdep.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: m197_machdep.c,v 1.16 2007/11/17 05:36:23 miod Exp $	*/
+/*	$OpenBSD: m197_machdep.c,v 1.17 2007/11/22 23:33:42 miod Exp $	*/
 /*
  * Copyright (c) 1998, 1999, 2000, 2001 Steve Murphree, Jr.
  * Copyright (c) 1996 Nivas Madhur
@@ -56,6 +56,7 @@
 #include <machine/cpu.h>
 #include <machine/reg.h>
 #include <machine/trap.h>
+#include <machine/m88410.h>
 #include <machine/mvme197.h>
 
 #include <mvme88k/dev/busswreg.h>
@@ -282,9 +283,14 @@ void
 m197_bootstrap()
 {
 	extern struct cmmu_p cmmu88110;
+	extern struct cmmu_p cmmu88410;
 	extern void set_tcfp(void);
 
-	cmmu = &cmmu88110;
+	if (mc88410_present())
+		cmmu = &cmmu88410;	/* 197SP/197DP */
+	else
+		cmmu = &cmmu88110;	/* 197LE */
+
 	md_interrupt_func_ptr = m197_ext_int;
 	md_getipl = m197_getipl;
 	md_setipl = m197_setipl;
diff --git a/sys/arch/mvme88k/mvme88k/m88110.c b/sys/arch/mvme88k/mvme88k/m88110.c
index d6d64fdf4cd..e3c041a03c1 100644
--- a/sys/arch/mvme88k/mvme88k/m88110.c
+++ b/sys/arch/mvme88k/mvme88k/m88110.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: m88110.c,v 1.43 2007/11/22 05:53:57 miod Exp $	*/
+/*	$OpenBSD: m88110.c,v 1.44 2007/11/22 23:33:42 miod Exp $	*/
 /*
  * Copyright (c) 1998 Steve Murphree, Jr.
  * All rights reserved.
@@ -73,22 +73,33 @@
 #include <mvme88k/dev/busswreg.h>
 
 cpuid_t	m88110_init(void);
+cpuid_t	m88410_init(void);
 void	m88110_setup_board_config(void);
+void	m88410_setup_board_config(void);
 void	m88110_cpu_configuration_print(int);
+void	m88410_cpu_configuration_print(int);
 void	m88110_shutdown(void);
 cpuid_t	m88110_cpu_number(void);
 void	m88110_set_sapr(apr_t);
 void	m88110_set_uapr(apr_t);
 void	m88110_flush_tlb(cpuid_t, u_int, vaddr_t, u_int);
+void	m88410_flush_tlb(cpuid_t, u_int, vaddr_t, u_int);
 void	m88110_flush_cache(cpuid_t, paddr_t, psize_t);
+void	m88410_flush_cache(cpuid_t, paddr_t, psize_t);
 void	m88110_flush_inst_cache(cpuid_t, paddr_t, psize_t);
+void	m88410_flush_inst_cache(cpuid_t, paddr_t, psize_t);
 void	m88110_dma_cachectl(pmap_t, vaddr_t, vsize_t, int);
+void	m88410_dma_cachectl(pmap_t, vaddr_t, vsize_t, int);
 void	m88110_dma_cachectl_pa(paddr_t, psize_t, int);
+void	m88410_dma_cachectl_pa(paddr_t, psize_t, int);
 void	m88110_initialize_cpu(cpuid_t);
 
-/* This is the function table for the MC88110 built-in CMMUs */
+/*
+ * This is the function table for the MC88110 built-in CMMUs without
+ * external 88410.
+ */
 struct cmmu_p cmmu88110 = {
-        m88110_init,
+	m88110_init,
 	m88110_setup_board_config,
 	m88110_cpu_configuration_print,
 	m88110_shutdown,
@@ -105,7 +116,30 @@ struct cmmu_p cmmu88110 = {
 #endif
 };
 
+/*
+ * This is the function table for the MC88110 built-in CMMUs with
+ * external 88410.
+ */
+struct cmmu_p cmmu88410 = {
+	m88410_init,
+	m88410_setup_board_config,
+	m88410_cpu_configuration_print,
+	m88110_shutdown,
+	m88110_cpu_number,
+	m88110_set_sapr,
+	m88110_set_uapr,
+	m88110_flush_tlb,
+	m88410_flush_cache,
+	m88410_flush_inst_cache,
+	m88410_dma_cachectl,
+	m88410_dma_cachectl_pa,
+#ifdef MULTIPROCESSOR
+	m88110_initialize_cpu,
+#endif
+};
+
 void patc_clear(void);
+
 void m88110_cmmu_sync_cache(paddr_t, psize_t);
 void m88110_cmmu_sync_inval_cache(paddr_t, psize_t);
 void m88110_cmmu_inval_cache(paddr_t, psize_t);
@@ -129,6 +163,12 @@ patc_clear(void)
 void
 m88110_setup_board_config(void)
 {
+	max_cpus = 1;
+}
+
+void
+m88410_setup_board_config(void)
+{
 #ifdef MULTIPROCESSOR
 	max_cpus = 2;
 #else
@@ -156,14 +196,19 @@ m88110_cpu_configuration_print(int master)
 		    proctype, procvers);
 		break;
 	case ARN_88110:
-		printf("M88110 version 0x%x", procvers);
-		if (mc88410_present())
-			printf(", external M88410 cache controller");
+		printf("M88110 version 0x%x, 8K I/D caches", procvers);
 		break;
 	}
 	printf("\n");
 }
 
+void
+m88410_cpu_configuration_print(int master)
+{
+	m88110_cpu_configuration_print(master);
+	printf("cpu%d: external M88410 cache controller\n", cpu_number());
+}
+
 /*
  * CMMU initialization routine
  */
@@ -177,6 +222,26 @@ m88110_init(void)
 	return (cpu);
 }
 
+cpuid_t
+m88410_init(void)
+{
+	cpuid_t cpu;
+
+	cpu = m88110_init();
+	mc88410_inval();	/* clear external data cache */
+	return (cpu);
+}
+
+cpuid_t
+m88110_cpu_number(void)
+{
+	u_int16_t gcsr;
+
+	gcsr = *(volatile u_int16_t *)(BS_BASE + BS_GCSR);
+
+	return ((gcsr & BS_GCSR_CPUID) != 0 ? 1 : 0);
+}
+
 void
 m88110_initialize_cpu(cpuid_t cpu)
 {
@@ -200,7 +265,7 @@ m88110_initialize_cpu(cpuid_t cpu)
 	 *   Suggested fix: Clear the PREN bit of the ICTL.  This will
 	 *   disable branch prediction.''
 	 */
-	set_ictl(BATC_32M
+	set_ictl(BATC_512K
 		 | CMMU_ICTL_DID	/* Double instruction disable */
 		 | CMMU_ICTL_MEN
 		 | CMMU_ICTL_CEN
@@ -226,9 +291,9 @@ m88110_initialize_cpu(cpuid_t cpu)
 	 *   Work-Around: do not set the xmem bit in dctl, or separate st
 	 *   from xmem instructions.''
 	 */
-	set_dctl(BATC_32M
-                 | CMMU_DCTL_RSVD1	/* Data Matching Disable */
-                 | CMMU_DCTL_MEN
+	set_dctl(BATC_512K
+		 | CMMU_DCTL_RSVD1	/* Data Matching Disable */
+		 | CMMU_DCTL_MEN
 		 | CMMU_DCTL_CEN
 		 | CMMU_DCTL_SEN
 		 | CMMU_DCTL_ADS
@@ -236,8 +301,6 @@ m88110_initialize_cpu(cpuid_t cpu)
 
 	mc88110_inval_inst();		/* clear instruction cache & TIC */
 	mc88110_inval_data();		/* clear data cache */
-	if (mc88410_present())
-		mc88410_inval();	/* clear external data cache */
 
 	set_dcmd(CMMU_DCMD_INV_SATC);	/* invalidate ATCs */
 
@@ -253,16 +316,6 @@ m88110_shutdown(void)
 {
 }
 
-cpuid_t
-m88110_cpu_number(void)
-{
-	u_int16_t gcsr;
-
-	gcsr = *(volatile u_int16_t *)(BS_BASE + BS_GCSR);
-
-	return ((gcsr & BS_GCSR_CPUID) != 0 ? 1 : 0);
-}
-
 void
 m88110_set_sapr(apr_t ap)
 {
@@ -275,8 +328,8 @@ m88110_set_sapr(apr_t ap)
 	dctl = get_dctl();
 
 	/* disable translation */
-	set_ictl((ictl & ~CMMU_ICTL_MEN));
-	set_dctl((dctl & ~CMMU_DCTL_MEN));
+	set_ictl(ictl & ~CMMU_ICTL_MEN);
+	set_dctl(dctl & ~CMMU_DCTL_MEN);
 
 	set_isap(ap);
 	set_dsap(ap);
@@ -334,33 +387,26 @@ m88110_flush_tlb(cpuid_t cpu, u_int kernel, vaddr_t vaddr, u_int count)
 
 /*
  *	Functions that invalidate caches.
- *
- * Cache invalidates require physical addresses.  Care must be exercised when
- * using segment invalidates.  This implies that the starting physical address
- * plus the segment length should be invalidated.  A typical mistake is to
- * extract the first physical page of a segment from a virtual address, and
- * then expecting to invalidate when the pages are not physically contiguous.
- *
- * We don't push Instruction Caches prior to invalidate because they are not
- * snooped and never modified (I guess it doesn't matter then which form
- * of the command we use then).
  */
 
-/*
- * Care must be taken to avoid flushing the data cache when
- * the data cache is not on!  From the 0F92L Errata Documentation
- * Package, Version 1.1
- */
+#define	trunc_cache_line(a)	((a) & ~(MC88110_CACHE_LINE - 1))
+#define	round_cache_line(a)	trunc_cache_line((a) + MC88110_CACHE_LINE - 1)
 
 /*
- * XXX These routines are really suboptimal because they invalidate
- * way too much...
- * Improve them once the 197 support is really working...
+ * 88110 general information #22:
+ * ``Issuing a command to flush and invalidate the data cache while the
+ *   dcache is disabled (CEN = 0 in dctl) will cause problems.  Do not
+ *   flush a disabled data cache.  In general, there is no reason to
+ *   perform this operation with the cache disabled, since it may be
+ *   incoherent with the proper state of memory.  Before 5.0 the flush
+ *   command was treated like a nop when the cache was disabled.  This
+ *   is no longer the case.''
  */
 
 /*
- *	flush both Instruction and Data caches
+ * Flush both Instruction and Data caches
  */
+
 void
 m88110_flush_cache(cpuid_t cpu, paddr_t pa, psize_t size)
 {
@@ -370,17 +416,15 @@ m88110_flush_cache(cpuid_t cpu, paddr_t pa, psize_t size)
 	set_psr(psr | PSR_IND);
 
 	mc88110_inval_inst();
-	mc88110_flush_data();
-	if (mc88410_present())
-		mc88410_flush();
+	/* flush all data to avoid errata invalidate */
+	if (get_dctl() & CMMU_DCTL_CEN)
+		mc88110_flush_data();
+
 	set_psr(psr);
 }
 
-/*
- *	flush Instruction caches
- */
 void
-m88110_flush_inst_cache(cpuid_t cpu, paddr_t pa, psize_t size)
+m88410_flush_cache(cpuid_t cpu, paddr_t pa, psize_t size)
 {
 	u_int32_t psr;
 
@@ -388,14 +432,27 @@ m88110_flush_inst_cache(cpuid_t cpu, paddr_t pa, psize_t size)
 	set_psr(psr | PSR_IND);
 
 	mc88110_inval_inst();
+	/* flush all data to avoid errata invalidate */
+	if (get_dctl() & CMMU_DCTL_CEN)
+		mc88110_flush_data();
+	mc88410_flush();
+
 	set_psr(psr);
 }
 
 /*
- * sync dcache (and icache too)
+ * Flush Instruction caches
  */
+
 void
-m88110_cmmu_sync_cache(paddr_t pa, psize_t size)
+m88110_flush_inst_cache(cpuid_t cpu, paddr_t pa, psize_t size)
+{
+	/* atomic so no psr games */
+	mc88110_inval_inst();
+}
+
+void
+m88410_flush_inst_cache(cpuid_t cpu, paddr_t pa, psize_t size)
 {
 	u_int32_t psr;
 
@@ -403,87 +460,259 @@ m88110_cmmu_sync_cache(paddr_t pa, psize_t size)
 	set_psr(psr | PSR_IND);
 
 	mc88110_inval_inst();
-	mc88110_flush_data();
-	if (mc88410_present())
-		mc88410_flush();
+	mc88410_flush();
+
 	set_psr(psr);
 }
 
+/*
+ * Sync dcache - icache is never dirty but needs to be invalidated as well.
+ */
+
+void
+m88110_cmmu_sync_cache(paddr_t pa, psize_t size)
+{
+	/* flush all data to avoid errata invalidate */
+	mc88110_flush_data();
+}
+
 void
 m88110_cmmu_sync_inval_cache(paddr_t pa, psize_t size)
 {
+	if (size <= MC88110_CACHE_LINE)
+		mc88110_sync_data_line(pa);
+	else
+		mc88110_sync_data_page(pa);
+}
+
+void
+m88110_cmmu_inval_cache(paddr_t pa, psize_t size)
+{
+	/*
+	 * I'd love to do this...
+
+	if (size <= MC88110_CACHE_LINE)
+		mc88110_inval_data_line(pa);
+	else
+		mc88110_inval_data_page(pa);
+
+	 * ... but there is no mc88110_inval_data_page(). Callers know
+	 * this and turn invalidates into syncs for areas larger than
+	 * a page.
+	 */
+	mc88110_inval_data_line(pa);
+}
+
+/*
+ * High level cache handling functions (used by bus_dma).
+ */
+
+void
+m88110_dma_cachectl(pmap_t pmap, vaddr_t _va, vsize_t _size, int op)
+{
 	u_int32_t psr;
+	vaddr_t va;
+	paddr_t pa;
+	psize_t size, count;
+	void (*flusher)(paddr_t, psize_t);
+
+	va = trunc_cache_line(_va);
+	size = round_cache_line(_va + size) - va;
+
+	switch (op) {
+	case DMA_CACHE_SYNC:
+		flusher = m88110_cmmu_sync_cache;
+		size = MC88110_CACHE_LINE;	/* leave loop after one call */
+		break;
+	case DMA_CACHE_SYNC_INVAL:
+		flusher = m88110_cmmu_sync_inval_cache;
+		break;
+	default:
+		if (va != _va || size != _size || size >= PAGE_SIZE)
+			flusher = m88110_cmmu_sync_inval_cache;
+		else
+			flusher = m88110_cmmu_inval_cache;
+		break;
+	}
 
 	psr = get_psr();
 	set_psr(psr | PSR_IND);
 
-	mc88110_sync_data();
-	if (mc88410_present())
-		mc88410_sync();
+	if (!ISSET(get_dctl(), CMMU_DCTL_CEN))
+		size = 0;
+
+	mc88110_inval_inst();
+	while (size != 0) {
+		count = (va & PAGE_MASK) == 0 && size >= PAGE_SIZE ?
+		    PAGE_SIZE : MC88110_CACHE_LINE;
+
+		if (pmap_extract(pmap, va, &pa) != FALSE) {
+			(*flusher)(pa, count);
+		}
+
+		va += count;
+		size -= count;
+	}
+
 	set_psr(psr);
 }
 
 void
-m88110_cmmu_inval_cache(paddr_t pa, psize_t size)
+m88410_dma_cachectl(pmap_t pmap, vaddr_t _va, vsize_t _size, int op)
 {
 	u_int32_t psr;
+	vaddr_t va;
+	paddr_t pa;
+	psize_t size, count;
+	void (*flusher)(paddr_t, psize_t);
+	void (*ext_flusher)(void);
+
+	va = trunc_cache_line(_va);
+	size = round_cache_line(_va + size) - va;
+
+	switch (op) {
+	case DMA_CACHE_SYNC:
+		flusher = m88110_cmmu_sync_cache;
+		ext_flusher = mc88410_flush;
+		size = MC88110_CACHE_LINE;	/* leave loop after one call */
+		break;
+	case DMA_CACHE_SYNC_INVAL:
+		flusher = m88110_cmmu_sync_inval_cache;
+		ext_flusher = mc88410_sync;
+		break;
+	default:
+		if (va != _va || size != _size || size >= PAGE_SIZE) {
+			flusher = m88110_cmmu_sync_inval_cache;
+			ext_flusher = mc88410_sync;
+		} else {
+			flusher = m88110_cmmu_inval_cache;
+			ext_flusher = mc88410_inval;
+		}
+		break;
+	}
 
 	psr = get_psr();
 	set_psr(psr | PSR_IND);
 
+	if (!ISSET(get_dctl(), CMMU_DCTL_CEN))
+		size = 0;
+
 	mc88110_inval_inst();
-	mc88110_inval_data();
-	if (mc88410_present())
-		mc88410_inval();
+	while (size != 0) {
+		count = (va & PAGE_MASK) == 0 && size >= PAGE_SIZE ?
+		    PAGE_SIZE : MC88110_CACHE_LINE;
+
+		if (pmap_extract(pmap, va, &pa) != FALSE) {
+			(*flusher)(pa, count);
+		}
+
+		va += count;
+		size -= count;
+	}
+
+	(*ext_flusher)();
+
 	set_psr(psr);
 }
 
 void
-m88110_dma_cachectl(pmap_t pmap, vaddr_t va, vsize_t size, int op)
+m88110_dma_cachectl_pa(paddr_t _pa, psize_t _size, int op)
 {
+	u_int32_t psr;
 	paddr_t pa;
+	psize_t size, count;
+	void (*flusher)(paddr_t, psize_t);
 
-	if (pmap_extract(pmap, va, &pa) == FALSE) {
-#ifdef DIAGNOSTIC
-		printf("cachectl: pmap_extract(%p, %p) failed\n", pmap, va);
-#endif
-		pa = 0;
-		size = ~0;
-	}
+	pa = trunc_cache_line(_pa);
+	size = round_cache_line(_pa + size) - pa;
 
 	switch (op) {
 	case DMA_CACHE_SYNC:
-		m88110_cmmu_sync_cache(pa, size);
+		flusher = m88110_cmmu_sync_cache;
+		size = MC88110_CACHE_LINE;	/* leave loop after one call */
 		break;
 	case DMA_CACHE_SYNC_INVAL:
-		m88110_cmmu_sync_inval_cache(pa, size);
+		flusher = m88110_cmmu_sync_inval_cache;
 		break;
 	default:
-#if 0
-		m88110_cmmu_inval_cache(pa, size);
-#else
-		m88110_cmmu_sync_inval_cache(pa, size);
-#endif
+		if (pa != _pa || size != _size || size >= PAGE_SIZE)
+			flusher = m88110_cmmu_sync_inval_cache;
+		else
+			flusher = m88110_cmmu_inval_cache;
 		break;
 	}
+
+	psr = get_psr();
+	set_psr(psr | PSR_IND);
+
+	if (!ISSET(get_dctl(), CMMU_DCTL_CEN))
+		size = 0;
+
+	mc88110_inval_inst();
+	while (size != 0) {
+		count = (pa & PAGE_MASK) == 0 && size >= PAGE_SIZE ?
+		    PAGE_SIZE : MC88110_CACHE_LINE;
+
+		(*flusher)(pa, count);
+
+		pa += count;
+		size -= count;
+	}
+
+	set_psr(psr);
 }
 
 void
-m88110_dma_cachectl_pa(paddr_t pa, psize_t size, int op)
+m88410_dma_cachectl_pa(paddr_t _pa, psize_t _size, int op)
 {
+	u_int32_t psr;
+	paddr_t pa;
+	psize_t size, count;
+	void (*flusher)(paddr_t, psize_t);
+	void (*ext_flusher)(void);
+
+	pa = trunc_cache_line(_pa);
+	size = round_cache_line(_pa + size) - pa;
+
 	switch (op) {
 	case DMA_CACHE_SYNC:
-		m88110_cmmu_sync_cache(pa, size);
+		flusher = m88110_cmmu_sync_cache;
+		ext_flusher = mc88410_flush;
+		size = MC88110_CACHE_LINE;	/* leave loop after one call */
 		break;
 	case DMA_CACHE_SYNC_INVAL:
-		m88110_cmmu_sync_inval_cache(pa, size);
+		flusher = m88110_cmmu_sync_inval_cache;
+		ext_flusher = mc88410_sync;
 		break;
 	default:
-#if 0
-		m88110_cmmu_inval_cache(pa, size);
-#else
-		m88110_cmmu_sync_inval_cache(pa, size);
-#endif
+		if (pa != _pa || size != _size || size >= PAGE_SIZE) {
+			flusher = m88110_cmmu_sync_inval_cache;
+			ext_flusher = mc88410_sync;
+		} else {
+			flusher = m88110_cmmu_inval_cache;
+			ext_flusher = mc88410_inval;
+		}
 		break;
 	}
+
+	psr = get_psr();
+	set_psr(psr | PSR_IND);
+
+	if (!ISSET(get_dctl(), CMMU_DCTL_CEN))
+		size = 0;
+
+	mc88110_inval_inst();
+	while (size != 0) {
+		count = (pa & PAGE_MASK) == 0 && size >= PAGE_SIZE ?
+		    PAGE_SIZE : MC88110_CACHE_LINE;
+
+		(*flusher)(pa, count);
+
+		pa += count;
+		size -= count;
+	}
+
+	(*ext_flusher)();
+
+	set_psr(psr);
 }