PL310 driver update:

- Add pl310.disable tunable to disable L2 cache altogether. In
    order to make sure that it's 100% disabled we use cache event
    counters for cache line eviction and read allocate events
    and panic if any of these counters increased. This is purely
    for debugging purpose
- Direct access DEBUG_CTRL and CTRL might be unavailable in
    unsecure mode, so use platform-specific functions for
    these registers
- Replace #if 1 with proper erratum numbers
- Add erratum 753970 workaround
- Remove wait function for atomic operations
- Protect cache operations with spin mutex in order to prevent race condition
- Disable instruction cache prefetch and make sure data cache
    prefetch is enabled in OMAP4-specific intialization

4 files changed, 366 insertions, 110 deletions

diff --git a/sys/arm/arm/pl310.c b/sys/arm/arm/pl310.c
index 7c43b31..a5af23c 100644
--- a/sys/arm/arm/pl310.c
+++ b/sys/arm/arm/pl310.c
@@ -39,62 +39,39 @@ __FBSDID("$FreeBSD$");
 #include <sys/lock.h>
 #include <sys/mutex.h>
 #include <machine/intr.h>
-#include <vm/vm.h>
-#include <vm/pmap.h>
-#include <machine/pl310.h>
+
 #include <machine/bus.h>
+#include <machine/pl310.h>
 
 #include <dev/fdt/fdt_common.h>
 #include <dev/ofw/openfirm.h>
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
 
-/**
- *	PL310 - L2 Cache Controller register offsets.
- *
+/*
+ * Define this if you need to disable PL310 for debugging purpose
+ * Spec: 
+ * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0246e/DDI0246E_l2c310_r3p1_trm.pdf
+ */
+
+/* 
+ * Hardcode errata for now
+ * http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0246b/pr01s02s02.html
  */
-#define PL310_CACHE_ID              0x000
-#define PL310_CACHE_TYPE            0x004
-#define PL310_CTRL                  0x100
-#define PL310_AUX_CTRL              0x104
-#define PL310_EVENT_COUNTER_CTRL    0x200
-#define PL310_EVENT_COUNTER1_CONF   0x204
-#define PL310_EVENT_COUNTER0_CONF   0x208
-#define PL310_EVENT_COUNTER1_VAL    0x20C
-#define PL310_EVENT_COUNTER0_VAL    0x210
-#define PL310_INTR_MASK             0x214
-#define PL310_MASKED_INTR_STAT      0x218
-#define PL310_RAW_INTR_STAT         0x21C
-#define PL310_INTR_CLEAR            0x220
-#define PL310_CACHE_SYNC            0x730
-#define PL310_INV_LINE_PA           0x770
-#define PL310_INV_WAY               0x77C
-#define PL310_CLEAN_LINE_PA         0x7B0
-#define PL310_CLEAN_LINE_IDX        0x7B8
-#define PL310_CLEAN_WAY             0x7BC
-#define PL310_CLEAN_INV_LINE_PA     0x7F0
-#define PL310_CLEAN_INV_LINE_IDX    0x7F8
-#define PL310_CLEAN_INV_WAY         0x7FC
-#define PL310_LOCKDOWN_D_WAY(x)    (0x900 + ((x) * 8))
-#define PL310_LOCKDOWN_I_WAY(x)    (0x904 + ((x) * 8))
-#define PL310_LOCKDOWN_LINE_ENABLE  0x950
-#define PL310_UNLOCK_ALL_LINES_WAY  0x954
-#define PL310_ADDR_FILTER_START     0xC00
-#define PL310_ADDR_FILTER_END       0xC04
-#define PL310_DEBUG_CTRL            0xF40
-
-
-#define PL310_AUX_CTRL_MASK                      0xc0000fff
-#define PL310_AUX_CTRL_ASSOCIATIVITY_SHIFT       16
-#define PL310_AUX_CTRL_WAY_SIZE_SHIFT            17
-#define PL310_AUX_CTRL_WAY_SIZE_MASK             (0x7 << 17)
-#define PL310_AUX_CTRL_SHARE_OVERRIDE_SHIFT      22
-#define PL310_AUX_CTRL_NS_LOCKDOWN_SHIFT         26
-#define PL310_AUX_CTRL_NS_INT_CTRL_SHIFT         27
-#define PL310_AUX_CTRL_DATA_PREFETCH_SHIFT       28
-#define PL310_AUX_CTRL_INSTR_PREFETCH_SHIFT      29
-#define PL310_AUX_CTRL_EARLY_BRESP_SHIFT         30
+#define	PL310_ERRATA_588369
+#define	PL310_ERRATA_753970
+#define	PL310_ERRATA_727915
+
+#define	PL310_LOCK(sc) do {		\
+	mtx_lock_spin(&(sc)->sc_mtx);	\
+} while(0);
 
+#define	PL310_UNLOCK(sc) do {		\
+	mtx_unlock_spin(&(sc)->sc_mtx);	\
+} while(0);
+
+static int pl310_enabled = 1;
+TUNABLE_INT("pl310.enabled", &pl310_enabled);
 
 void omap4_l2cache_wbinv_range(vm_paddr_t physaddr, vm_size_t size);
 void omap4_l2cache_inv_range(vm_paddr_t physaddr, vm_size_t size);
@@ -112,34 +89,31 @@ static uint32_t g_l2cache_size;
 
 static struct pl310_softc *pl310_softc;
 
-/**
- *	pl310_read4 - read a 32-bit value from the PL310 registers
- *	pl310_write4 - write a 32-bit value from the PL310 registers
- *	@off: byte offset within the register set to read from
- *	@val: the value to write into the register
- *	
- *
- *	LOCKING:
- *	None
- *
- *	RETURNS:
- *	nothing in case of write function, if read function returns the value read.
- */
-static __inline uint32_t
-pl310_read4(bus_size_t off)
-{
-	return bus_read_4(pl310_softc->sc_mem_res, off);
-}
-static __inline void
-pl310_write4(bus_size_t off, uint32_t val)
+static int
+pl310_filter(void *arg)
 {
-	bus_write_4(pl310_softc->sc_mem_res, off, val);
+	struct pl310_softc *sc = arg;
+	uint32_t intr;
+
+	intr = pl310_read4(sc, PL310_INTR_MASK);
+
+	if (!sc->sc_enabled && (intr & INTR_MASK_ECNTR)) {
+		/*
+		 * This is for debug purpose, so be blunt about it
+		 * We disable PL310 only when something fishy is going
+		 * on and we need to make sure L2 cache is 100% disabled
+		 */
+		panic("pl310: caches disabled but cache event detected\n");
+	}
+
+	return (FILTER_HANDLED);
 }
 
 static __inline void
 pl310_wait_background_op(uint32_t off, uint32_t mask)
 {
-	while (pl310_read4(off) & mask);
+
+	while (pl310_read4(pl310_softc, off) & mask);
 }
 
 
@@ -157,29 +131,46 @@ pl310_wait_background_op(uint32_t off, uint32_t mask)
 static __inline void
 pl310_cache_sync(void)
 {
-	pl310_write4(PL310_CACHE_SYNC, 0);
+	if ((pl310_softc == NULL) || !pl310_softc->sc_enabled)
+		return;
+
+#ifdef PL310_ERRATA_753970
+	/* Write uncached PL310 register */
+	pl310_write4(pl310_softc, 0x740, 0xffffffff);
+#else
+	pl310_write4(pl310_softc, PL310_CACHE_SYNC, 0xffffffff);
+#endif
 }
 
 
 static void
 pl310_wbinv_all(void)
 {
-#if 1
-	pl310_write4(PL310_DEBUG_CTRL, 3);
+
+	if ((pl310_softc == NULL) || !pl310_softc->sc_enabled)
+		return;
+
+	PL310_LOCK(pl310_softc);
+#ifdef PL310_ERRATA_727915
+	platform_pl310_write_debug(pl310_softc, 3);
 #endif
-	pl310_write4(PL310_CLEAN_INV_WAY, g_l2cache_way_mask);
+	pl310_write4(pl310_softc, PL310_CLEAN_INV_WAY, g_l2cache_way_mask);
 	pl310_wait_background_op(PL310_CLEAN_INV_WAY, g_l2cache_way_mask);
 	pl310_cache_sync();
-#if 1
-	pl310_write4(PL310_DEBUG_CTRL, 0);
+#ifdef PL310_ERRATA_727915
+	platform_pl310_write_debug(pl310_softc, 0);
 #endif
-		
+	PL310_UNLOCK(pl310_softc);
 }
 
 static void
 pl310_wbinv_range(vm_paddr_t start, vm_size_t size)
 {
-	
+
+	if ((pl310_softc == NULL) || !pl310_softc->sc_enabled)
+		return;
+
+	PL310_LOCK(pl310_softc);
 	if (start & g_l2cache_align_mask) {
 		size += start & g_l2cache_align_mask;
 		start &= ~g_l2cache_align_mask;
@@ -188,12 +179,13 @@ pl310_wbinv_range(vm_paddr_t start, vm_size_t size)
 		size &= ~g_l2cache_align_mask;
 	   	size += g_l2cache_line_size;
 	}
-#if 1
 
-	pl310_write4(PL310_DEBUG_CTRL, 3);
+
+#ifdef PL310_ERRATA_727915
+	platform_pl310_write_debug(pl310_softc, 3);
 #endif
 	while (size > 0) {
-#if 1
+#ifdef PL310_ERRATA_588369
 		/* 
 		 * Errata 588369 says that clean + inv may keep the 
 		 * cache line if it was clean, the recommanded workaround
@@ -201,48 +193,58 @@ pl310_wbinv_range(vm_paddr_t start, vm_size_t size)
 		 * write-back and cache linefill disabled
 		 */
 		   
-		pl310_write4(PL310_CLEAN_LINE_PA, start);
-		pl310_write4(PL310_INV_LINE_PA, start);
+		pl310_write4(pl310_softc, PL310_CLEAN_LINE_PA, start);
+		pl310_write4(pl310_softc, PL310_INV_LINE_PA, start);
 #else
-		pl310_write4(PL310_CLEAN_INV_LINE_PA, start);
+		pl310_write4(pl310_softc, PL310_CLEAN_INV_LINE_PA, start);
 #endif
 		start += g_l2cache_line_size;
 		size -= g_l2cache_line_size;
 	}
-#if 1
-	pl310_write4(PL310_DEBUG_CTRL, 0);
+#ifdef PL310_ERRATA_727915
+	platform_pl310_write_debug(pl310_softc, 0);
 #endif
-	pl310_wait_background_op(PL310_CLEAN_INV_LINE_PA, 1);
+
 	pl310_cache_sync();
-		
+	PL310_UNLOCK(pl310_softc);
 }
 
 static void
 pl310_wb_range(vm_paddr_t start, vm_size_t size)
 {
-	
+
+	if ((pl310_softc == NULL) || !pl310_softc->sc_enabled)
+		return;
+
+	PL310_LOCK(pl310_softc);
 	if (start & g_l2cache_align_mask) {
 		size += start & g_l2cache_align_mask;
 		start &= ~g_l2cache_align_mask;
 	}
+
 	if (size & g_l2cache_align_mask) {
 		size &= ~g_l2cache_align_mask;
 		size += g_l2cache_line_size;
 	}
+
 	while (size > 0) {
-		pl310_write4(PL310_CLEAN_LINE_PA, start);
+		pl310_write4(pl310_softc, PL310_CLEAN_LINE_PA, start);
 		start += g_l2cache_line_size;
 		size -= g_l2cache_line_size;
 	}
-	pl310_cache_sync();
-	pl310_wait_background_op(PL310_CLEAN_LINE_PA, 1);
 
+	pl310_cache_sync();
+	PL310_UNLOCK(pl310_softc);
 }
 
 static void
 pl310_inv_range(vm_paddr_t start, vm_size_t size)
 {
 
+	if ((pl310_softc == NULL) || !pl310_softc->sc_enabled)
+		return;
+
+	PL310_LOCK(pl310_softc);
 	if (start & g_l2cache_align_mask) {
 		size += start & g_l2cache_align_mask;
 		start &= ~g_l2cache_align_mask;
@@ -252,13 +254,13 @@ pl310_inv_range(vm_paddr_t start, vm_size_t size)
 		size += g_l2cache_line_size;
 	}
 	while (size > 0) {
-		pl310_write4(PL310_INV_LINE_PA, start);
+		pl310_write4(pl310_softc, PL310_INV_LINE_PA, start);
 		start += g_l2cache_line_size;
 		size -= g_l2cache_line_size;
 	}
-	pl310_cache_sync();
-	pl310_wait_background_op(PL310_INV_LINE_PA, 1);
 
+	pl310_cache_sync();
+	PL310_UNLOCK(pl310_softc);
 }
 
 static int
@@ -280,39 +282,98 @@ pl310_attach(device_t dev)
 	uint32_t way_size;
 	uint32_t ways_assoc;
 	uint32_t ctrl_value;
+	uint32_t cache_id;
 
+	sc->sc_dev = dev;
 	sc->sc_mem_res = bus_alloc_resource_any(dev, SYS_RES_MEMORY, &rid, 
 	    RF_ACTIVE);
 	if (sc->sc_mem_res == NULL)
 		panic("%s: Cannot map registers", device_get_name(dev));
-	pl310_softc = sc;
 
-	platform_init_pl310(sc);
-	aux_value = pl310_read4(PL310_AUX_CTRL);
-	way_size = (aux_value & PL310_AUX_CTRL_WAY_SIZE_MASK) >>
-	    PL310_AUX_CTRL_WAY_SIZE_SHIFT;
+	/* Allocate an IRQ resource */
+	rid = 0;
+	sc->sc_irq_res = bus_alloc_resource_any(dev, SYS_RES_IRQ, &rid,
+	                                        RF_ACTIVE | RF_SHAREABLE);
+	if (sc->sc_irq_res == NULL) {
+		panic("Cannot allocate IRQ\n");
+	}
+
+	pl310_softc = sc;
+	mtx_init(&sc->sc_mtx, "pl310lock", NULL, MTX_SPIN);
+	sc->sc_enabled = pl310_enabled;
+
+	/* activate the interrupt */
+	bus_setup_intr(dev, sc->sc_irq_res, INTR_TYPE_MISC | INTR_MPSAFE,
+				pl310_filter, NULL, sc, &sc->sc_irq_h);
+
+	cache_id = pl310_read4(sc, PL310_CACHE_ID);
+	device_printf(dev, "Part number: 0x%x, release: 0x%x\n",
+	    (cache_id >> CACHE_ID_PARTNUM_SHIFT) & CACHE_ID_PARTNUM_MASK,
+	    (cache_id >> CACHE_ID_RELEASE_SHIFT) & CACHE_ID_RELEASE_MASK);
+	aux_value = pl310_read4(sc, PL310_AUX_CTRL);
+	way_size = (aux_value & AUX_CTRL_WAY_SIZE_MASK) >>
+	    AUX_CTRL_WAY_SIZE_SHIFT;
 	way_size = 1 << (way_size + 13);
-	if (aux_value & (1 << PL310_AUX_CTRL_ASSOCIATIVITY_SHIFT))
+	if (aux_value & (1 << AUX_CTRL_ASSOCIATIVITY_SHIFT))
 		ways_assoc = 16;
 	else
 		ways_assoc = 8;
 	g_l2cache_way_mask = (1 << ways_assoc) - 1;
 	g_l2cache_size = way_size * ways_assoc;
 	/* Print the information */
-	printf("  L2 Cache: %uKB/%dB %d ways\n", (g_l2cache_size / 1024),
+	device_printf(dev, "L2 Cache: %uKB/%dB %d ways\n", (g_l2cache_size / 1024),
 	       g_l2cache_line_size, ways_assoc);
-	ctrl_value = pl310_read4(PL310_CTRL);
-	if (!(ctrl_value & 0x1)) {
+
+	ctrl_value = pl310_read4(sc, PL310_CTRL);
+
+	if (sc->sc_enabled && !(ctrl_value & CTRL_ENABLED)) {
 		/* Enable the L2 cache if disabled */
-		pl310_write4(PL310_CTRL, ctrl_value & 0x1);
+		platform_pl310_write_ctrl(sc, CTRL_ENABLED);
+	} 
+
+	if (!sc->sc_enabled && (ctrl_value & CTRL_ENABLED)) {
+		/*
+		 * Set counters so when cache event happens
+		 * we'll get interrupt and be warned that something 
+		 * is off
+		 */
+
+		/* Cache Line Eviction for Counter 0 */
+		pl310_write4(sc, PL310_EVENT_COUNTER0_CONF, 
+		    EVENT_COUNTER_CONF_INCR | EVENT_COUNTER_CONF_CO);
+		/* Data Read Request for Counter 1 */
+		pl310_write4(sc, PL310_EVENT_COUNTER1_CONF, 
+		    EVENT_COUNTER_CONF_INCR | EVENT_COUNTER_CONF_DRREQ);
+
+		/* Temporary switch on for final flush*/
+		sc->sc_enabled = 1;
+		pl310_wbinv_all();
+		sc->sc_enabled = 0;
+		platform_pl310_write_ctrl(sc, CTRL_DISABLED);
+
+		/* Enable and clear pending interrupts */
+		pl310_write4(sc, PL310_INTR_CLEAR, INTR_MASK_ECNTR);
+		pl310_write4(sc, PL310_INTR_MASK, INTR_MASK_ALL);
+
+		/* Enable counters and reset C0 and C1 */
+		pl310_write4(sc, PL310_EVENT_COUNTER_CTRL, 
+		    EVENT_COUNTER_CTRL_ENABLED | 
+		    EVENT_COUNTER_CTRL_C0_RESET | 
+		    EVENT_COUNTER_CTRL_C1_RESET);
+
 	}
+
+	if (sc->sc_enabled)
+		platform_pl310_init(sc);
+
 	pl310_wbinv_all();
-	
+
 	/* Set the l2 functions in the set of cpufuncs */
 	cpufuncs.cf_l2cache_wbinv_all = pl310_wbinv_all;
 	cpufuncs.cf_l2cache_wbinv_range = pl310_wbinv_range;
 	cpufuncs.cf_l2cache_inv_range = pl310_inv_range;
 	cpufuncs.cf_l2cache_wb_range = pl310_wb_range;
+
 	return (0);
 }
 
@@ -330,4 +391,3 @@ static driver_t pl310_driver = {
 static devclass_t pl310_devclass;
 
 DRIVER_MODULE(pl310, simplebus, pl310_driver, pl310_devclass, 0, 0);
-
diff --git a/sys/arm/include/pl310.h b/sys/arm/include/pl310.h
index 98df53c..9a6b159 100644
--- a/sys/arm/include/pl310.h
+++ b/sys/arm/include/pl310.h
@@ -29,10 +29,131 @@
 
 #ifndef PL310_H_
 #define PL310_H_
+
+/**
+ *	PL310 - L2 Cache Controller register offsets.
+ *
+ */
+#define PL310_CACHE_ID			0x000
+#define 	CACHE_ID_RELEASE_SHIFT		0
+#define 	CACHE_ID_RELEASE_MASK		0x3f
+#define 	CACHE_ID_PARTNUM_SHIFT		6
+#define 	CACHE_ID_PARTNUM_MASK		0xf
+#define PL310_CACHE_TYPE		0x004
+#define PL310_CTRL			0x100
+#define		CTRL_ENABLED			0x01
+#define		CTRL_DISABLED			0x00
+#define PL310_AUX_CTRL			0x104
+#define 	AUX_CTRL_MASK			0xc0000fff
+#define 	AUX_CTRL_ASSOCIATIVITY_SHIFT	16
+#define 	AUX_CTRL_WAY_SIZE_SHIFT		17
+#define 	AUX_CTRL_WAY_SIZE_MASK		(0x7 << 17)
+#define 	AUX_CTRL_SHARE_OVERRIDE		(1 << 22)
+#define 	AUX_CTRL_NS_LOCKDOWN		(1 << 26)
+#define 	AUX_CTRL_NS_INT_CTRL		(1 << 27)
+#define 	AUX_CTRL_DATA_PREFETCH		(1 << 28)
+#define 	AUX_CTRL_INSTR_PREFETCH		(1 << 29)
+#define 	AUX_CTRL_EARLY_BRESP		(1 << 30)
+#define PL310_EVENT_COUNTER_CTRL	0x200
+#define		EVENT_COUNTER_CTRL_ENABLED	(1 << 0)
+#define		EVENT_COUNTER_CTRL_C0_RESET	(1 << 1)
+#define		EVENT_COUNTER_CTRL_C1_RESET	(1 << 2)
+#define PL310_EVENT_COUNTER1_CONF	0x204
+#define PL310_EVENT_COUNTER0_CONF	0x208
+#define		EVENT_COUNTER_CONF_NOINTR	0
+#define		EVENT_COUNTER_CONF_INCR		1
+#define		EVENT_COUNTER_CONF_OVFW		2
+#define		EVENT_COUNTER_CONF_NOEV		(0 << 2)
+#define		EVENT_COUNTER_CONF_CO		(1 << 2)
+#define		EVENT_COUNTER_CONF_DRHIT	(2 << 2)
+#define		EVENT_COUNTER_CONF_DRREQ	(3 << 2)
+#define		EVENT_COUNTER_CONF_DWHIT	(4 << 2)
+#define		EVENT_COUNTER_CONF_DWREQ	(5 << 2)
+#define		EVENT_COUNTER_CONF_DWTREQ	(6 << 2)
+#define		EVENT_COUNTER_CONF_DIRHIT	(7 << 2)
+#define		EVENT_COUNTER_CONF_DIRREQ	(8 << 2)
+#define		EVENT_COUNTER_CONF_WA		(9 << 2)
+#define PL310_EVENT_COUNTER1_VAL	0x20C
+#define PL310_EVENT_COUNTER0_VAL	0x210
+#define PL310_INTR_MASK			0x214
+#define PL310_MASKED_INTR_STAT		0x218
+#define PL310_RAW_INTR_STAT		0x21C
+#define PL310_INTR_CLEAR		0x220
+#define		INTR_MASK_ALL			((1 << 9) - 1)
+#define		INTR_MASK_ECNTR			(1 << 0)
+#define		INTR_MASK_PARRT			(1 << 1)
+#define		INTR_MASK_PARRD			(1 << 2)
+#define		INTR_MASK_ERRWT			(1 << 3)
+#define		INTR_MASK_ERRWD			(1 << 4)
+#define		INTR_MASK_ERRRT			(1 << 5)
+#define		INTR_MASK_ERRRD			(1 << 6)
+#define		INTR_MASK_SLVERR		(1 << 7)
+#define		INTR_MASK_DECERR		(1 << 8)
+#define PL310_CACHE_SYNC		0x730
+#define PL310_INV_LINE_PA		0x770
+#define PL310_INV_WAY			0x77C
+#define PL310_CLEAN_LINE_PA		0x7B0
+#define PL310_CLEAN_LINE_IDX		0x7B8
+#define PL310_CLEAN_WAY			0x7BC
+#define PL310_CLEAN_INV_LINE_PA		0x7F0
+#define PL310_CLEAN_INV_LINE_IDX	0x7F8
+#define PL310_CLEAN_INV_WAY		0x7FC
+#define PL310_LOCKDOWN_D_WAY(x)		(0x900 + ((x) * 8))
+#define PL310_LOCKDOWN_I_WAY(x)		(0x904 + ((x) * 8))
+#define PL310_LOCKDOWN_LINE_ENABLE	0x950
+#define PL310_UNLOCK_ALL_LINES_WAY	0x954
+#define PL310_ADDR_FILTER_STAR		0xC00
+#define PL310_ADDR_FILTER_END		0xC04
+#define PL310_DEBUG_CTRL		0xF40
+#define PL310_PREFETCH_CTRL		0xF60
+#define		PREFETCH_CTRL_OFFSET_MASK	(0x1f)
+#define		PREFETCH_CTRL_NOTSAMEID		(1 << 21)
+#define		PREFETCH_CTRL_INCR_DL		(1 << 23)
+#define		PREFETCH_CTRL_PREFETCH_DROP	(1 << 24)
+#define		PREFETCH_CTRL_DL_ON_WRAP	(1 << 27)
+#define		PREFETCH_CTRL_DATA_PREFETCH	(1 << 28)
+#define		PREFETCH_CTRL_INSTR_PREFETCH	(1 << 29)
+#define		PREFETCH_CTRL_DL		(1 << 30)
+#define PL310_POWER_CTRL		0xF60
+
 struct pl310_softc {
+	device_t	sc_dev;
 	struct resource *sc_mem_res;
+	struct resource *sc_irq_res;
+	void*		sc_irq_h;
+	int		sc_enabled;
+	struct mtx	sc_mtx;
 };
 
-void platform_init_pl310(struct pl310_softc *sc);
+/**
+ *	pl310_read4 - read a 32-bit value from the PL310 registers
+ *	pl310_write4 - write a 32-bit value from the PL310 registers
+ *	@off: byte offset within the register set to read from
+ *	@val: the value to write into the register
+ *	
+ *
+ *	LOCKING:
+ *	None
+ *
+ *	RETURNS:
+ *	nothing in case of write function, if read function returns the value read.
+ */
+static __inline uint32_t
+pl310_read4(struct pl310_softc *sc, bus_size_t off)
+{
+
+	return bus_read_4(sc->sc_mem_res, off);
+}
+
+static __inline void
+pl310_write4(struct pl310_softc *sc, bus_size_t off, uint32_t val)
+{
+
+	bus_write_4(sc->sc_mem_res, off, val);
+}
+
+void platform_pl310_init(struct pl310_softc *);
+void platform_pl310_write_ctrl(struct pl310_softc *, uint32_t);
+void platform_pl310_write_debug(struct pl310_softc *, uint32_t);
 
 #endif /* PL310_H_ */
diff --git a/sys/arm/ti/omap4/omap4_l2cache.c b/sys/arm/ti/omap4/omap4_l2cache.c
index b663a1a..67678bf 100644
--- a/sys/arm/ti/omap4/omap4_l2cache.c
+++ b/sys/arm/ti/omap4/omap4_l2cache.c
@@ -27,13 +27,88 @@ __FBSDID("$FreeBSD$");
 #include <sys/types.h>
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/rman.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+
 #include <arm/ti/ti_smc.h>
 #include <arm/ti/omap4/omap4_smc.h>
+#include <machine/bus.h>
 #include <machine/pl310.h>
 
 void
-platform_init_pl310(struct pl310_softc *softc)
+platform_pl310_init(struct pl310_softc *sc)
+{
+	uint32_t aux, prefetch;
+
+	aux = pl310_read4(sc, PL310_AUX_CTRL);
+	prefetch = pl310_read4(sc, PL310_PREFETCH_CTRL);
+
+	if (bootverbose) {
+		device_printf(sc->sc_dev, "Early BRESP response: %s\n",
+			(aux & AUX_CTRL_EARLY_BRESP) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Instruction prefetch: %s\n",
+			(aux & AUX_CTRL_INSTR_PREFETCH) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Data prefetch: %s\n",
+			(aux & AUX_CTRL_DATA_PREFETCH) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Non-secure interrupt control: %s\n",
+			(aux & AUX_CTRL_NS_INT_CTRL) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Non-secure lockdown: %s\n",
+			(aux & AUX_CTRL_NS_LOCKDOWN) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Share override: %s\n",
+			(aux & AUX_CTRL_SHARE_OVERRIDE) ? "enabled" : "disabled");
+
+		device_printf(sc->sc_dev, "Double linefil: %s\n",
+			(prefetch & PREFETCH_CTRL_DL) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Instruction prefetch: %s\n",
+			(prefetch & PREFETCH_CTRL_INSTR_PREFETCH) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Data prefetch: %s\n",
+			(prefetch & PREFETCH_CTRL_DATA_PREFETCH) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Double linefill on WRAP request: %s\n",
+			(prefetch & PREFETCH_CTRL_DL_ON_WRAP) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Prefetch drop: %s\n",
+			(prefetch & PREFETCH_CTRL_PREFETCH_DROP) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Incr double Linefill: %s\n",
+			(prefetch & PREFETCH_CTRL_INCR_DL) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Not same ID on exclusive sequence: %s\n",
+			(prefetch & PREFETCH_CTRL_NOTSAMEID) ? "enabled" : "disabled");
+		device_printf(sc->sc_dev, "Prefetch offset: %d\n",
+			(prefetch & PREFETCH_CTRL_OFFSET_MASK));
+	}
+
+	/*
+	 * Disable instruction prefetch
+	 */
+	prefetch &= ~PREFETCH_CTRL_INSTR_PREFETCH;
+	aux &= ~AUX_CTRL_INSTR_PREFETCH;
+
+	// prefetch &= ~PREFETCH_CTRL_DATA_PREFETCH;
+	// aux &= ~AUX_CTRL_DATA_PREFETCH;
+
+	/*
+	 * Make sure data prefetch is on
+	 */
+	prefetch |= PREFETCH_CTRL_DATA_PREFETCH;
+	aux |= AUX_CTRL_DATA_PREFETCH;
+
+	/*
+	 * TODO: add tunable for prefetch offset
+	 * and experiment with performance
+	 */
+
+	ti_smc0(aux, 0, WRITE_AUXCTRL_REG);
+	ti_smc0(prefetch, 0, WRITE_PREFETCH_CTRL_REG);
+}
+
+void
+platform_pl310_write_ctrl(struct pl310_softc *sc, uint32_t val)
 {
-	ti_smc0(1, 0, L2CACHE_ENABLE_L2);
+	ti_smc0(val, 0, L2CACHE_WRITE_CTRL_REG);
 }
 
+void
+platform_pl310_write_debug(struct pl310_softc *sc, uint32_t val)
+{
+	ti_smc0(val, 0, L2CACHE_WRITE_DEBUG_REG);
+}
diff --git a/sys/arm/ti/omap4/omap4_smc.h b/sys/arm/ti/omap4/omap4_smc.h
index 11ff853..5fd14aa 100644
--- a/sys/arm/ti/omap4/omap4_smc.h
+++ b/sys/arm/ti/omap4/omap4_smc.h
@@ -31,7 +31,7 @@
 /* Define the various function IDs used by the OMAP4 */
 #define L2CACHE_WRITE_DEBUG_REG		0x100
 #define L2CACHE_CLEAN_INV_RANG		0x101
-#define L2CACHE_ENABLE_L2		0x102
+#define L2CACHE_WRITE_CTRL_REG		0x102
 #define READ_AUX_CORE_REGS		0x103
 #define MODIFY_AUX_CORE_0		0x104
 #define WRITE_AUX_CORE_1		0x105