Add SMP aware cache flushing functions, which operate on a single physical

page.  These send IPIs if necessary in order to keep the caches in sync on
all cpus.

7 files changed, 327 insertions, 17 deletions

diff --git a/sys/sparc64/include/cache.h b/sys/sparc64/include/cache.h
index 0ef20fe..1ebffc1 100644
--- a/sys/sparc64/include/cache.h
+++ b/sys/sparc64/include/cache.h
@@ -104,25 +104,48 @@ void	ecache_flush(vm_offset_t, vm_offset_t);
 void	ecache_inval_phys(vm_offset_t, vm_offset_t);
 #endif
 
+void	dcache_page_inval(vm_offset_t pa);
+void	icache_page_inval(vm_offset_t pa);
+
+#define	DC_TAG_SHIFT	2
+#define	DC_VALID_SHIFT	0
+
+#define	DC_TAG_BITS	28
+#define	DC_VALID_BITS	2
+
+#define	DC_TAG_MASK	((1 << DC_TAG_BITS) - 1)
+#define	DC_VALID_MASK	((1 << DC_VALID_BITS) - 1)
+
+#define	IC_TAG_SHIFT	7
+#define	IC_VALID_SHIFT	36
+
+#define	IC_TAG_BITS	28
+#define	IC_VALID_BITS	1
+
+#define	IC_TAG_MASK	((1 << IC_TAG_BITS) - 1)
+#define	IC_VALID_MASK	((1 << IC_VALID_BITS) - 1)
+
 /*
  * Cache control information.
  */
 struct cacheinfo {
-	int	c_enabled;		/* true => cache is enabled */
-	int 	ic_size;		/* instruction cache */
-	int	ic_set;
-	int	ic_l2set;
-	int 	ic_assoc;
-	int 	ic_linesize;
-	int 	dc_size;		/* data cache */
-	int	dc_l2size;
-	int 	dc_assoc;
-	int 	dc_linesize;
-	int	ec_size;		/* external cache info */
-	int 	ec_assoc;
-	int	ec_l2set;
-	int	ec_linesize;
-	int	ec_l2linesize;
+	u_int	c_enabled;		/* true => cache is enabled */
+	u_int 	ic_size;		/* instruction cache */
+	u_int	ic_set;
+	u_int	ic_l2set;
+	u_int 	ic_assoc;
+	u_int 	ic_linesize;
+	u_int 	dc_size;		/* data cache */
+	u_int	dc_l2size;
+	u_int 	dc_assoc;
+	u_int 	dc_linesize;
+	u_int	ec_size;		/* external cache info */
+	u_int 	ec_assoc;
+	u_int	ec_l2set;
+	u_int	ec_linesize;
+	u_int	ec_l2linesize;
 };
 
+extern struct cacheinfo cache;
+
 #endif	/* !_MACHINE_CACHE_H_ */
diff --git a/sys/sparc64/include/smp.h b/sys/sparc64/include/smp.h
index 958fa0c..03fbc04 100644
--- a/sys/sparc64/include/smp.h
+++ b/sys/sparc64/include/smp.h
@@ -57,6 +57,11 @@ struct cpu_start_args {
 	struct	tte csa_ttes[PCPU_PAGES];
 };
 
+struct ipi_cache_args {
+	u_int	ica_mask;
+	u_long	ica_pa;
+};
+
 struct ipi_tlb_args {
 	u_int	ita_mask;
 	u_long	ita_tlb;
@@ -80,6 +85,7 @@ void	ipi_all_but_self(u_int ipi);
 
 vm_offset_t mp_tramp_alloc(void);
 
+extern	struct	ipi_cache_args ipi_cache_args;
 extern	struct	ipi_level_args ipi_level_args;
 extern	struct	ipi_tlb_args ipi_tlb_args;
 
@@ -91,6 +97,8 @@ extern	u_long mp_tramp_func;
 
 extern	void mp_startup(void);
 
+extern	char tl_ipi_dcache_page_inval[];
+extern	char tl_ipi_icache_page_inval[];
 extern	char tl_ipi_level[];
 extern	char tl_ipi_test[];
 extern	char tl_ipi_tlb_context_demap[];
@@ -99,6 +107,36 @@ extern	char tl_ipi_tlb_range_demap[];
 
 #ifdef SMP
 
+static __inline void *
+ipi_dcache_page_inval(vm_offset_t pa)
+{
+	struct ipi_cache_args *ica;
+
+	if (smp_cpus == 1)
+		return (NULL);
+	ica = &ipi_cache_args;
+	ica->ica_mask = all_cpus;
+	ica->ica_pa = pa;
+	cpu_ipi_selected(all_cpus, 0, (u_long)tl_ipi_dcache_page_inval,
+	    (u_long)ica);
+	return (&ica->ica_mask);
+}
+
+static __inline void *
+ipi_icache_page_inval(vm_offset_t pa)
+{
+	struct ipi_cache_args *ica;
+
+	if (smp_cpus == 1)
+		return (NULL);
+	ica = &ipi_cache_args;
+	ica->ica_mask = all_cpus;
+	ica->ica_pa = pa;
+	cpu_ipi_selected(all_cpus, 0, (u_long)tl_ipi_icache_page_inval,
+	    (u_long)ica);
+	return (&ica->ica_mask);
+}
+
 #ifdef _MACHINE_PMAP_H_
 
 static __inline void *
@@ -174,6 +212,18 @@ ipi_wait(void *cookie)
 #else
 
 static __inline void *
+ipi_dcache_page_inval(vm_offset_t pa)
+{
+	return (NULL);
+}
+
+static __inline void *
+ipi_icache_page_inval(vm_offset_t pa)
+{
+	return (NULL);
+}
+
+static __inline void *
 ipi_tlb_context_demap(struct pmap *pm)
 {
 	return (NULL);
diff --git a/sys/sparc64/sparc64/cache.c b/sys/sparc64/sparc64/cache.c
index 44f9e12..912662e 100644
--- a/sys/sparc64/sparc64/cache.c
+++ b/sys/sparc64/sparc64/cache.c
@@ -152,6 +152,7 @@
 
 #include <sys/param.h>
 #include <sys/proc.h>
+#include <sys/smp.h>
 #include <sys/systm.h>
 
 #include <vm/vm.h>
@@ -163,12 +164,12 @@
 #include <machine/fsr.h>
 #include <machine/pcb.h>
 #include <machine/pmap.h>
+#include <machine/smp.h>
 #include <machine/tte.h>
 #include <machine/ver.h>
 #include <machine/vmparam.h>
 
-static struct cacheinfo cache;
-extern vm_offset_t cache_tmp_va;
+struct cacheinfo cache;
 
 /* Read to %g0, needed for E$ access. */
 #define	CDIAG_RDG0(asi, addr)						\
@@ -215,6 +216,64 @@ cache_init(phandle_t node)
 	cache.c_enabled = 1; /* enable cache flushing */
 }
 
+void
+dcache_page_inval(vm_offset_t pa)
+{
+	u_long target;
+	void *cookie;
+	u_long addr;
+	u_long tag;
+
+	KASSERT((pa & PAGE_MASK) == 0,
+	    ("dcache_page_inval: pa not page aligned"));
+
+	if (!cache.c_enabled)
+		return;
+	target = pa >> (PAGE_SHIFT - DC_TAG_SHIFT);
+	critical_enter();
+	cookie = ipi_dcache_page_inval(pa);
+	for (addr = 0; addr < cache.dc_size; addr += cache.dc_linesize) {
+		tag = ldxa(addr, ASI_DCACHE_TAG);
+		if (((tag >> DC_VALID_SHIFT) & DC_VALID_MASK) == 0)
+			continue;
+		tag &= DC_TAG_MASK << DC_TAG_SHIFT;
+		if (tag == target)
+			stxa_sync(addr, ASI_DCACHE_TAG, tag);
+	}
+	ipi_wait(cookie);
+	critical_exit();
+}
+
+void
+icache_page_inval(vm_offset_t pa)
+{
+	register u_long tag __asm("%g1");
+	u_long target;
+	void *cookie;
+	u_long addr;
+
+	KASSERT((pa & PAGE_MASK) == 0,
+	    ("icache_page_inval: pa not page aligned"));
+
+	if (!cache.c_enabled)
+		return;
+	target = pa >> (PAGE_SHIFT - IC_TAG_SHIFT);
+	critical_enter();
+	cookie = ipi_icache_page_inval(pa);
+	for (addr = 0; addr < cache.ic_size; addr += cache.ic_linesize) {
+		__asm __volatile("ldda [%1] %2, %%g0" /*, %g1 */
+		    : "=r" (tag) : "r" (addr), "n" (ASI_ICACHE_TAG));
+		if (((tag >> IC_VALID_SHIFT) & IC_VALID_MASK) == 0)
+			continue;
+		tag &= IC_TAG_MASK << IC_TAG_SHIFT;
+		if (tag == target)
+			stxa_sync(addr, ASI_ICACHE_TAG, tag);
+	}
+	ipi_wait(cookie);
+	critical_exit();
+}
+
+
 /* Flush a range of addresses from I$ using the flush instruction. */
 void
 icache_flush(vm_offset_t start, vm_offset_t end)
diff --git a/sys/sparc64/sparc64/genassym.c b/sys/sparc64/sparc64/genassym.c
index 1d71d6e..7c975f6 100644
--- a/sys/sparc64/sparc64/genassym.c
+++ b/sys/sparc64/sparc64/genassym.c
@@ -48,6 +48,7 @@
 #include <vm/vm_map.h>
 
 #include <machine/asi.h>
+#include <machine/cache.h>
 #include <machine/vmparam.h>
 #include <machine/cpufunc.h>
 #include <machine/fp.h>
@@ -117,6 +118,22 @@ ASSYM(CSA_TICK, offsetof(struct cpu_start_args, csa_tick));
 ASSYM(CSA_VER, offsetof(struct cpu_start_args, csa_ver));
 ASSYM(CSA_TTES, offsetof(struct cpu_start_args, csa_ttes));
 
+ASSYM(DC_TAG_SHIFT, DC_TAG_SHIFT);
+ASSYM(DC_TAG_MASK, DC_TAG_MASK);
+ASSYM(DC_VALID_SHIFT, DC_VALID_SHIFT);
+ASSYM(DC_VALID_MASK, DC_VALID_MASK);
+ASSYM(IC_TAG_SHIFT, IC_TAG_SHIFT);
+ASSYM(IC_TAG_MASK, IC_TAG_MASK);
+ASSYM(IC_VALID_SHIFT, IC_VALID_SHIFT);
+ASSYM(IC_VALID_MASK, IC_VALID_MASK);
+
+ASSYM(DC_SIZE, offsetof(struct cacheinfo, dc_size));
+ASSYM(DC_LINESIZE, offsetof(struct cacheinfo, dc_linesize));
+ASSYM(IC_SIZE, offsetof(struct cacheinfo, ic_size));
+ASSYM(IC_LINESIZE, offsetof(struct cacheinfo, ic_linesize));
+
+ASSYM(ICA_PA, offsetof(struct ipi_cache_args, ica_pa));
+
 ASSYM(KTR_PROC, KTR_PROC);
 ASSYM(KTR_TRAP, KTR_TRAP);
 ASSYM(KTR_SMP, KTR_SMP);
diff --git a/sys/sparc64/sparc64/mp_exception.S b/sys/sparc64/sparc64/mp_exception.S
index 652021c..d214513 100644
--- a/sys/sparc64/sparc64/mp_exception.S
+++ b/sys/sparc64/sparc64/mp_exception.S
@@ -44,6 +44,86 @@
 	 nop
 
 /*
+ * Invalidate a phsyical page in the data cache.
+ */
+ENTRY(tl_ipi_dcache_page_inval)
+#if KTR_COMPILE & KTR_SMP
+	CATR(KTR_SMP, "ipi_dcache_page_inval: pa=%#lx"
+	    , %g1, %g2, %g3, 7, 8, 9)
+	ldx	[%g5 + ICA_PA], %g2
+	stx	%g2, [%g1 + KTR_PARM1]
+9:
+#endif
+
+	ldx	[%g5 + ICA_PA], %g6
+	srlx	%g6, PAGE_SHIFT - DC_TAG_SHIFT, %g6
+
+	SET(cache, %g3, %g2)
+	lduw	[%g2 + DC_SIZE], %g3
+	lduw	[%g2 + DC_LINESIZE], %g4
+	sub	%g3, %g4, %g2
+
+1:	ldxa	[%g2] ASI_DCACHE_TAG, %g1
+	srlx	%g1, DC_VALID_SHIFT, %g3
+	andcc	%g3, DC_VALID_MASK, %g0
+	bz,pt	%xcc, 2f
+	 set	DC_TAG_MASK, %g3
+	sllx	%g3, DC_TAG_SHIFT, %g3
+	and	%g1, %g3, %g1
+	cmp	%g1, %g6
+	bne,a,pt %xcc, 2f
+	 nop
+	stxa	%g1, [%g2] ASI_DCACHE_TAG
+	membar	#Sync
+
+2:	brgz,pt	%g2, 1b
+	 sub	%g2, %g4, %g2
+
+	IPI_WAIT(%g5, %g1, %g2, %g3)
+	retry
+END(tl_ipi_dcache_page_inval)
+
+/*
+ * Invalidate a phsyical page in the instruction cache.
+ */
+ENTRY(tl_ipi_icache_page_inval)
+#if KTR_COMPILE & KTR_SMP
+	CATR(KTR_SMP, "ipi_icache_page_inval: pa=%#lx"
+	    , %g1, %g2, %g3, 7, 8, 9)
+	ldx	[%g5 + ICA_PA], %g2
+	stx	%g2, [%g1 + KTR_PARM1]
+9:
+#endif
+
+	ldx	[%g5 + ICA_PA], %g6
+	srlx	%g6, PAGE_SHIFT - IC_TAG_SHIFT, %g6
+
+	SET(cache, %g3, %g2)
+	lduw	[%g2 + IC_SIZE], %g3
+	lduw	[%g2 + IC_LINESIZE], %g4
+	sub	%g3, %g4, %g2
+
+1:	ldda	[%g2] ASI_ICACHE_TAG, %g0 /*, %g1 */
+	srlx	%g1, IC_VALID_SHIFT, %g3
+	andcc	%g3, IC_VALID_MASK, %g0
+	bz,pt	%xcc, 2f
+	 set	IC_TAG_MASK, %g3
+	sllx	%g3, IC_TAG_SHIFT, %g3
+	and	%g1, %g3, %g1
+	cmp	%g1, %g6
+	bne,a,pt %xcc, 2f
+	 nop
+	stxa	%g1, [%g2] ASI_ICACHE_TAG
+	membar	#Sync
+
+2:	brgz,pt	%g2, 1b
+	 sub	%g2, %g4, %g2
+
+	IPI_WAIT(%g5, %g1, %g2, %g3)
+	retry
+END(tl_ipi_icache_page_inval)
+
+/*
  * Trigger a softint at the desired level.
  */
 ENTRY(tl_ipi_level)
diff --git a/sys/sparc64/sparc64/mp_exception.s b/sys/sparc64/sparc64/mp_exception.s
index 652021c..d214513 100644
--- a/sys/sparc64/sparc64/mp_exception.s
+++ b/sys/sparc64/sparc64/mp_exception.s
@@ -44,6 +44,86 @@
 	 nop
 
 /*
+ * Invalidate a phsyical page in the data cache.
+ */
+ENTRY(tl_ipi_dcache_page_inval)
+#if KTR_COMPILE & KTR_SMP
+	CATR(KTR_SMP, "ipi_dcache_page_inval: pa=%#lx"
+	    , %g1, %g2, %g3, 7, 8, 9)
+	ldx	[%g5 + ICA_PA], %g2
+	stx	%g2, [%g1 + KTR_PARM1]
+9:
+#endif
+
+	ldx	[%g5 + ICA_PA], %g6
+	srlx	%g6, PAGE_SHIFT - DC_TAG_SHIFT, %g6
+
+	SET(cache, %g3, %g2)
+	lduw	[%g2 + DC_SIZE], %g3
+	lduw	[%g2 + DC_LINESIZE], %g4
+	sub	%g3, %g4, %g2
+
+1:	ldxa	[%g2] ASI_DCACHE_TAG, %g1
+	srlx	%g1, DC_VALID_SHIFT, %g3
+	andcc	%g3, DC_VALID_MASK, %g0
+	bz,pt	%xcc, 2f
+	 set	DC_TAG_MASK, %g3
+	sllx	%g3, DC_TAG_SHIFT, %g3
+	and	%g1, %g3, %g1
+	cmp	%g1, %g6
+	bne,a,pt %xcc, 2f
+	 nop
+	stxa	%g1, [%g2] ASI_DCACHE_TAG
+	membar	#Sync
+
+2:	brgz,pt	%g2, 1b
+	 sub	%g2, %g4, %g2
+
+	IPI_WAIT(%g5, %g1, %g2, %g3)
+	retry
+END(tl_ipi_dcache_page_inval)
+
+/*
+ * Invalidate a phsyical page in the instruction cache.
+ */
+ENTRY(tl_ipi_icache_page_inval)
+#if KTR_COMPILE & KTR_SMP
+	CATR(KTR_SMP, "ipi_icache_page_inval: pa=%#lx"
+	    , %g1, %g2, %g3, 7, 8, 9)
+	ldx	[%g5 + ICA_PA], %g2
+	stx	%g2, [%g1 + KTR_PARM1]
+9:
+#endif
+
+	ldx	[%g5 + ICA_PA], %g6
+	srlx	%g6, PAGE_SHIFT - IC_TAG_SHIFT, %g6
+
+	SET(cache, %g3, %g2)
+	lduw	[%g2 + IC_SIZE], %g3
+	lduw	[%g2 + IC_LINESIZE], %g4
+	sub	%g3, %g4, %g2
+
+1:	ldda	[%g2] ASI_ICACHE_TAG, %g0 /*, %g1 */
+	srlx	%g1, IC_VALID_SHIFT, %g3
+	andcc	%g3, IC_VALID_MASK, %g0
+	bz,pt	%xcc, 2f
+	 set	IC_TAG_MASK, %g3
+	sllx	%g3, IC_TAG_SHIFT, %g3
+	and	%g1, %g3, %g1
+	cmp	%g1, %g6
+	bne,a,pt %xcc, 2f
+	 nop
+	stxa	%g1, [%g2] ASI_ICACHE_TAG
+	membar	#Sync
+
+2:	brgz,pt	%g2, 1b
+	 sub	%g2, %g4, %g2
+
+	IPI_WAIT(%g5, %g1, %g2, %g3)
+	retry
+END(tl_ipi_icache_page_inval)
+
+/*
  * Trigger a softint at the desired level.
  */
 ENTRY(tl_ipi_level)
diff --git a/sys/sparc64/sparc64/mp_machdep.c b/sys/sparc64/sparc64/mp_machdep.c
index 5bee754..8cd1dfe 100644
--- a/sys/sparc64/sparc64/mp_machdep.c
+++ b/sys/sparc64/sparc64/mp_machdep.c
@@ -95,6 +95,7 @@ static ih_func_t cpu_ipi_stop;
  * kernel.
  */
 struct	cpu_start_args cpu_start_args = { 0, -1, -1, 0, 0 };
+struct	ipi_cache_args ipi_cache_args;
 struct	ipi_tlb_args ipi_tlb_args;
 
 vm_offset_t mp_tramp;