--- old/usr/src/uts/armv7/os/cpuid.c	2015-03-18 09:16:45.085324245 -0400
+++ new/usr/src/uts/armv7/os/cpuid.c	2015-03-18 09:16:45.090845597 -0400
@@ -11,6 +11,7 @@
 
 /*
  * Copyright (c) 2014 Joyent, Inc.  All rights reserved.
+ * Copyright (c) 2015 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
  */
 
 #include <sys/cpuid_impl.h>
@@ -32,10 +33,10 @@
 
 static arm_cpuid_t cpuid_data0;
 
-static void
-cpuid_parse_stage(uint32_t line, uint32_t mask, uint32_t shift, int *out)
+static uint32_t
+extract(uint32_t line, uint32_t mask, uint32_t shift)
 {
-	*out = (line & mask) >> shift;
+	return ((line & mask) >> shift);
 }
 
 static void
@@ -63,92 +64,117 @@
 	cpd->ac_mvfr[1] = arm_cpuid_mvfr1();
 }
 
-#define	CACHE_LEN_MASK		0x003
-#define	CACHE_M_BIT		0x004
-#define	CACHE_ASSOC_MASK	0x038
-#define	CACHE_ASSOC_SHIFT	3
-#define	CACHE_SIZE_MASK		0x3c0
-#define	CACHE_SIZE_SHIFT	6
-#define	CACHE_COLOR_BIT		0x800
-#define	CACHE_MASK		0xfff
-#define	CACHE_DCACHE_SHIFT	12
-#define	CACHE_SEPARATE		0x1000000
+#define	CCSIDR_WT		0x80000000
+#define	CCSIDR_WB		0x40000000
+#define	CCSIDR_RA		0x20000000
+#define	CCSIDR_WA		0x10000000
+#define	CCSIDR_NUMSETS_MASK	0x0fffe000
+#define	CCSIDR_NUMSETS_SHIFT	13
+#define	CCSIDR_ASSOC_MASK	0x00001ff8
+#define	CCSIDR_ASSOC_SHIFT	3
+#define	CCSIDR_LINESIZE_MASK	0x00000007
+#define	CCSIDR_LINESIZE_SHIFT	0
+
+static void
+cpuid_fill_onecache(arm_cpuid_t *cpd, int level, boolean_t icache)
+{
+	arm_cpuid_cache_t *cache = &cpd->ac_caches[icache][level];
+	uint32_t ccsidr;
+
+	ccsidr = arm_cpuid_ccsidr(level, icache);
+	cpd->ac_ccsidr[icache][level] = ccsidr;
+
+	cache->acc_exists = B_TRUE;
+	cache->acc_wt = (ccsidr & CCSIDR_WT) == CCSIDR_WT;
+	cache->acc_wb = (ccsidr & CCSIDR_WB) == CCSIDR_WB;
+	cache->acc_ra = (ccsidr & CCSIDR_RA) == CCSIDR_RA;
+	cache->acc_wa = (ccsidr & CCSIDR_WA) == CCSIDR_WA;
+	cache->acc_sets = extract(ccsidr, CCSIDR_NUMSETS_MASK,
+	    CCSIDR_NUMSETS_SHIFT) + 1;
+	cache->acc_assoc = extract(ccsidr, CCSIDR_ASSOC_MASK,
+	    CCSIDR_ASSOC_SHIFT) + 1;
+	cache->acc_linesz = sizeof (uint32_t) << (extract(ccsidr,
+	    CCSIDR_LINESIZE_MASK, CCSIDR_LINESIZE_SHIFT) + 2);
 
 /*
- * On ARMv6 the value of the cache size and the cache associativity depends on
- * the value of the M bit, which modifies the value that's in the actual index.
+ * XXX?
+#warning "set acc_size?"
  */
-static uint32_t armv6_cpuid_cache_sizes[2][9] = {
-	{ 0x200, 0x400, 0x800, 0x1000, 0x2000, 0x4000, 0x8000, 0x100000,
-		0x20000 },
-	{ 0x300, 0x600, 0xc00, 0x1800, 0x3000, 0x6000, 0xc000, 0x18000,
-		0x30000 }
-};
-
-static int8_t armv6_cpuid_cache_assoc[2][9] = {
-	{ 1, 2, 4, 8, 16, 32, 64, 128 },
-	{ -1, 3, 6, 12, 24, 48, 96, 192 }
-};
-
-static uint8_t armv6_cpuid_cache_linesz[] = {
-	8,
-	16,
-	32,
-	64
-};
-
-static void
-cpuid_fill_onecache(arm_cpuid_cache_t *accp, uint32_t val)
-{
-	int mbit, index, assoc;
-
-	mbit = (val & CACHE_M_BIT) != 0 ? 1 : 0;
-	index = (val & CACHE_ASSOC_MASK) >> CACHE_ASSOC_SHIFT;
-	assoc = armv6_cpuid_cache_assoc[mbit][index];
-	if (assoc == -1) {
-		accp->acc_exists = B_FALSE;
-		return;
-	}
-	ASSERT(assoc > 0);
-	accp->acc_assoc = assoc;
-	accp->acc_rcolor = (val & CACHE_COLOR_BIT) == 0 ?
-	    B_FALSE : B_TRUE;
-	index = val & CACHE_LEN_MASK;
-	accp->acc_linesz = armv6_cpuid_cache_linesz[index];
-	index = (val & CACHE_SIZE_MASK) >> CACHE_SIZE_SHIFT;
-	accp->acc_size = armv6_cpuid_cache_sizes[mbit][index];
-	accp->acc_exists = B_TRUE;
 }
 
 static void
 cpuid_fill_caches(arm_cpuid_t *cpd)
 {
-	uint32_t val, icache, dcache;
+	uint32_t erg, cwg;
+	uint32_t l1ip;
+	uint32_t ctr;
+	uint32_t clidr;
+	int level;
+
+	clidr = arm_cpuid_clidr();
+	cpd->ac_clidr = clidr;
+
+	/* default all caches to not existing, and not unified */
+	for (level = 0; level < 7; level++) {
+		cpd->ac_caches[B_TRUE][level].acc_exists = B_FALSE;
+		cpd->ac_caches[B_FALSE][level].acc_exists = B_FALSE;
+		cpd->ac_caches[B_TRUE][level].acc_unified = B_FALSE;
+		cpd->ac_caches[B_FALSE][level].acc_unified = B_FALSE;
+	}
+
+	/* retrieve cache info for each level */
+	for (level = 0; level < 7; level++) {
+		arm_cpuid_cache_t *icache = &cpd->ac_caches[B_TRUE][level];
+		arm_cpuid_cache_t *dcache = &cpd->ac_caches[B_FALSE][level];
+		uint32_t ctype = (cpd->ac_clidr >> (3 * level)) & 0x7;
+
+		/* stop looking we find the first non-existent level */
+		if (!ctype)
+			break;
+
+		switch (ctype) {
+		case 1:
+			cpuid_fill_onecache(cpd, level, B_TRUE);
+			break;
+		case 2:
+			cpuid_fill_onecache(cpd, level, B_FALSE);
+			break;
+		case 3:
+			cpuid_fill_onecache(cpd, level, B_TRUE);
+			cpuid_fill_onecache(cpd, level, B_FALSE);
+			break;
+		case 4:
+			cpuid_fill_onecache(cpd, level, B_FALSE);
+			dcache->acc_unified = B_TRUE;
+			break;
+		default:
+			bop_panic("unsupported cache type");
+		}
+	}
+
+	/*
+	 * We require L1-I/D & L2-D.  Unified caches are OK as well.
+	 */
+	if (!cpd->ac_caches[B_TRUE][0].acc_exists &&
+	    (!cpd->ac_caches[B_FALSE][0].acc_exists ||
+	    !cpd->ac_caches[B_FALSE][0].acc_unified))
+		bop_panic("no L1 instructian cache detected");
 
-	val = arm_cpuid_ctr();
-	icache = val & CACHE_MASK;
-	cpuid_fill_onecache(&cpd->ac_icache, icache);
-	dcache = (val >> CACHE_DCACHE_SHIFT) & CACHE_MASK;
-	cpuid_fill_onecache(&cpd->ac_dcache, dcache);
-
-	if (val & CACHE_SEPARATE) {
-		cpd->ac_unifiedl1 = B_FALSE;
-	} else {
-		cpd->ac_unifiedl1 = B_TRUE;
-	}
-
-	armv7_bsmdep_l2cacheinfo();
-	armv6_cachesz = cpd->ac_dcache.acc_size;
-	armv6_cache_assoc = cpd->ac_dcache.acc_assoc;
+	if (!cpd->ac_caches[B_FALSE][1].acc_exists)
+		bop_panic("no L2 data cache detected");
+
+	/*
+	 * set globals with cache size info
+	 */
+	l2cache_sz = cpd->ac_caches[B_FALSE][1].acc_size;
+	l2cache_linesz = cpd->ac_caches[B_FALSE][1].acc_linesz;
+	l2cache_assoc = cpd->ac_caches[B_FALSE][1].acc_assoc;
 }
 
 
 /*
- * There isn't a specific way to indicate that we're on ARMv6k. Instead what we
- * need to do is go through and check for a few features that we know we're
- * going to need.
- *
- * TODO This will have to be revisited with ARMv7 support
+ * We need to do is go through and check for a few features that we know
+ * we're going to need.
  */
 static void
 cpuid_verify(void)
@@ -159,44 +185,33 @@
 
 	arm_cpuid_t *cpd = &cpuid_data0;
 
-	/* v6 vmsa */
-	cpuid_parse_stage(cpd->ac_mmfr[0], ARM_CPUID_MMFR0_STATE0_MASK,
-	    ARM_CPUID_MMFR0_STATE0_SHIFT, (int *)&vmsa);
-	/* TODO We might be able to support v6, but bcm2835+qvpb are this */
+	/* v7 vmsa */
+	vmsa = extract(cpd->ac_mmfr[0], ARM_CPUID_MMFR0_STATE0_MASK,
+	    ARM_CPUID_MMFR0_STATE0_SHIFT);
 	if (vmsa != ARM_CPUID_MEM_VMSA_V7) {
 		bop_printf(NULL, "invalid vmsa setting, found 0x%x\n", vmsa);
 		bop_panic("unsupported cpu");
 	}
 
 	/* check for ISB, DSB, etc. in cp15 */
-	cpuid_parse_stage(cpd->ac_mmfr[2], ARM_CPUID_MMFR2_STATE5_MASK,
-	    ARM_CPUID_MMFR2_STATE5_SHIFT, (int *)&barrier);
-	if (barrier != ARM_CPUID_MEM_BARRIER_CP15 &&
-	    barrier != ARM_CPUID_MEM_BARRIER_INSTR) {
-		bop_printf(NULL, "missing support for CP15 memory barriers\n");
+	barrier = extract(cpd->ac_mmfr[2], ARM_CPUID_MMFR2_STATE5_MASK,
+	    ARM_CPUID_MMFR2_STATE5_SHIFT);
+	if (barrier != ARM_CPUID_MEM_BARRIER_INSTR) {
+		bop_printf(NULL, "missing support for memory barrier "
+		    "instructions\n");
 		bop_panic("unsupported CPU");
 	}
 
 	/* synch prims */
-	cpuid_parse_stage(cpd->ac_isar[4], ARM_CPUID_ISAR3_STATE3_SHIFT,
-	    ARM_CPUID_ISAR4_STATE5_SHIFT, (int *)&sync);
-	cpuid_parse_stage(cpd->ac_isar[4], ARM_CPUID_ISAR4_STATE3_SHIFT,
-	    ARM_CPUID_ISAR4_STATE5_SHIFT, (int *)&syncf);
+	sync = extract(cpd->ac_isar[3], ARM_CPUID_ISAR3_STATE3_SHIFT,
+	    ARM_CPUID_ISAR3_STATE3_SHIFT);
+	syncf = extract(cpd->ac_isar[4], ARM_CPUID_ISAR4_STATE5_SHIFT,
+	    ARM_CPUID_ISAR4_STATE5_SHIFT);
 	if (sync != 0x2 && syncf != 0x0) {
 		bop_printf(NULL, "unsupported synch primitives: sync,frac: "
 		    "%x,%x\n", sync, syncf);
 		bop_panic("unsupported CPU");
 	}
-
-	if (cpd->ac_icache.acc_exists == B_FALSE) {
-		bop_printf(NULL, "icache not defined to exist\n");
-		bop_panic("unsupported CPU");
-	}
-
-	if (cpd->ac_dcache.acc_exists == B_FALSE) {
-		bop_printf(NULL, "dcache not defined to exist\n");
-		bop_panic("unsupported CPU");
-	}
 }
 
 static void
@@ -205,7 +220,7 @@
 	arm_cpuid_ident_arch_t arch;
 
 	/*
-	 * We don't support stock ARMv6 or older.
+	 * We don't support anything older than ARMv7.
 	 */
 	arch = (ident & ARM_CPUID_IDENT_ARCH_MASK) >>
 	    ARM_CPUID_IDENT_ARCH_SHIFT;
@@ -221,8 +236,8 @@
 {
 	arm_cpuid_vfp_arch_t vfp;
 
-	cpuid_parse_stage(ident, ARM_CPUID_VFP_ARCH_MASK,
-	    ARM_CPUID_VFP_ARCH_SHIFT, (int *)&vfp);
+	vfp = extract(ident, ARM_CPUID_VFP_ARCH_MASK, ARM_CPUID_VFP_ARCH_SHIFT);
+	// XXX: _V3_V2BASE? _V3_NOBASE? _V3_V3BASE?
 	if (vfp != ARM_CPUID_VFP_ARCH_V2) {
 		bop_printf(NULL, "unsupported vfp version: %x\n", vfp);
 		bop_panic("unsupported CPU");
@@ -238,13 +253,14 @@
 {
 	arm_cpuid_t *cpd = &cpuid_data0;
 
-	cpd->ac_ident = arm_cpuid_idreg();
+	cpd->ac_ident = arm_cpuid_midr();
 	cpuid_valid_ident(cpd->ac_ident);
 	cpuid_fill_main(cpd);
 
 	cpd->ac_fpident = arm_cpuid_vfpidreg();
 	cpuid_valid_fpident(cpd->ac_fpident);
 	cpuid_fill_fpu(cpd);
+
 	cpuid_fill_caches(cpd);
 
 	cpuid_verify();
