Index: kernel/arch/arm32/src/cpu/cpu.c
===================================================================
--- kernel/arch/arm32/src/cpu/cpu.c	(revision 65871bbac3619bcfef8e6721cd8ed346b6151b36)
+++ kernel/arch/arm32/src/cpu/cpu.c	(revision c8a5c8ccc4d51228eb2b54620daac293e42c66b3)
@@ -34,8 +34,26 @@
  */
 
+#include <arch/cache.h>
 #include <arch/cpu.h>
+#include <arch/cp15.h>
 #include <cpu.h>
 #include <arch.h>
 #include <print.h>
+
+static inline unsigned log2(unsigned val)
+{
+	unsigned log = 0;
+	--val;
+	while (val) {
+		++log;
+		val >>= 1;
+	}
+	return log;
+}
+
+static unsigned dcache_ways(unsigned level);
+static unsigned dcache_sets(unsigned level);
+static unsigned dcache_linesize_log(unsigned level);
+
 
 /** Implementers (vendor) names */
@@ -81,16 +99,26 @@
 static void arch_cpu_identify(cpu_arch_t *cpu)
 {
-	uint32_t ident;
-	asm volatile (
-		"mrc p15, 0, %[ident], c0, c0, 0\n"
-		: [ident] "=r" (ident)
-	);
-	
-	cpu->imp_num = ident >> 24;
-	cpu->variant_num = (ident << 8) >> 28;
-	cpu->arch_num = (ident << 12) >> 28;
-	cpu->prim_part_num = (ident << 16) >> 20;
-	cpu->rev_num = (ident << 28) >> 28;
+	const uint32_t ident = MIDR_read();
+
+	cpu->imp_num = (ident >> MIDR_IMPLEMENTER_SHIFT) & MIDR_IMPLEMENTER_MASK;
+	cpu->variant_num = (ident >> MIDR_VARIANT_SHIFT) & MIDR_VARIANT_MASK;
+	cpu->arch_num = (ident >> MIDR_ARCHITECTURE_SHIFT) & MIDR_ARCHITECTURE_MASK;
+	cpu->prim_part_num = (ident >> MIDR_PART_NUMBER_SHIFT) & MIDR_PART_NUMBER_MASK;
+	cpu->rev_num = (ident >> MIDR_REVISION_SHIFT) & MIDR_REVISION_MASK;
+
 	// TODO CPUs with arch_num == 0xf use CPUID scheme for identification
+	cpu->dcache_levels = dcache_levels();
+
+	for (unsigned i = 0; i < cpu->dcache_levels; ++i) {
+		cpu->dcache[i].ways = dcache_ways(i);
+		cpu->dcache[i].sets = dcache_sets(i);
+		cpu->dcache[i].way_shift = 31 - log2(cpu->dcache[i].ways);
+		cpu->dcache[i].set_shift = dcache_linesize_log(i);
+		cpu->dcache[i].line_size = 1 << dcache_linesize_log(i);
+		printf("Found DCache L%u: %u-way, %u sets, %u byte lines "
+		    "(shifts: w%u, s%u)\n", i + 1, cpu->dcache[i].ways,
+		    cpu->dcache[i].sets, cpu->dcache[i].line_size,
+		    cpu->dcache[i].way_shift, cpu->dcache[i].set_shift);
+	}
 }
 
@@ -98,23 +126,21 @@
 void cpu_arch_init(void)
 {
-#if defined(PROCESSOR_armv7_a) | defined(PROCESSOR_armv6)
-	uint32_t control_reg = 0;
-	asm volatile (
-		"mrc p15, 0, %[control_reg], c1, c0"
-		: [control_reg] "=r" (control_reg)
-	);
+	uint32_t control_reg = SCTLR_read();
 	
-	/* Turn off tex remap, RAZ ignores writes prior to armv7 */
-	control_reg &= ~CP15_R1_TEX_REMAP_EN;
-	/* Turn off accessed flag, RAZ ignores writes prior to armv7 */
-	control_reg &= ~(CP15_R1_ACCESS_FLAG_EN | CP15_R1_HW_ACCESS_FLAG_EN);
-	/* Enable unaligned access, RAZ ignores writes prior to armv6
-	 * switchable on armv6, RAO ignores writes on armv7,
+	/* Turn off tex remap, RAZ/WI prior to armv7 */
+	control_reg &= ~SCTLR_TEX_REMAP_EN_FLAG;
+	/* Turn off accessed flag, RAZ/WI prior to armv7 */
+	control_reg &= ~(SCTLR_ACCESS_FLAG_EN_FLAG | SCTLR_HW_ACCESS_FLAG_EN_FLAG);
+
+	/* Unaligned access is supported on armv6+ */
+#if defined(PROCESSOR_ARCH_armv7_a) | defined(PROCESSOR_ARCH_armv6)
+	/* Enable unaligned access, RAZ/WI prior to armv6
+	 * switchable on armv6, RAO/WI writes on armv7,
 	 * see ARM Architecture Reference Manual ARMv7-A and ARMv7-R edition
 	 * L.3.1 (p. 2456) */
-	control_reg |= CP15_R1_UNALIGNED_EN;
+	control_reg |= SCTLR_UNALIGNED_EN_FLAG;
 	/* Disable alignment checks, this turns unaligned access to undefined,
 	 * unless U bit is set. */
-	control_reg &= ~CP15_R1_ALIGN_CHECK_EN;
+	control_reg &= ~SCTLR_ALIGN_CHECK_EN_FLAG;
 	/* Enable caching, On arm prior to armv7 there is only one level
 	 * of caches. Data cache is coherent.
@@ -124,16 +150,34 @@
 	 *    ARM Architecture Reference Manual ARMv7-A and ARMv7-R Edition
 	 *    B3.11.1 (p. 1383)
-	 * ICache coherency is elaborate on in barrier.h.
-	 * We are safe to turn these on.
+	 * We are safe to turn this on. For arm v6 see ch L.6.2 (p. 2469)
+	 * L2 Cache for armv7 is enabled by default (i.e. controlled by
+	 * this flag).
 	 */
-	control_reg |= CP15_R1_CACHE_EN | CP15_R1_INST_CACHE_EN;
-	
-	asm volatile (
-		"mcr p15, 0, %[control_reg], c1, c0"
-		:: [control_reg] "r" (control_reg)
-	);
-#endif
+	control_reg |= SCTLR_CACHE_EN_FLAG;
+#endif
+#ifdef PROCESSOR_ARCH_armv7_a
+	 /* ICache coherency is elaborate on in barrier.h.
+	  * VIPT and PIPT caches need maintenance only on code modify,
+	  * so it should be safe for general use.
+	  * Enable branch predictors too as they follow the same rules
+	  * as ICache and they can be flushed together
+	  */
+	if ((CTR_read() & CTR_L1I_POLICY_MASK) != CTR_L1I_POLICY_AIVIVT) {
+		control_reg |=
+		    SCTLR_INST_CACHE_EN_FLAG | SCTLR_BRANCH_PREDICT_EN_FLAG;
+	}
+#endif
+	SCTLR_write(control_reg);
+
 #ifdef CONFIG_FPU
 	fpu_setup();
+#endif
+
+#ifdef PROCESSOR_ARCH_armv7_a
+	if ((ID_PFR1_read() & ID_PFR1_GEN_TIMER_EXT_MASK) !=
+	    ID_PFR1_GEN_TIMER_EXT) {
+		PMCR_write(PMCR_read() | PMCR_E_FLAG | PMCR_D_FLAG);
+		PMCNTENSET_write(PMCNTENSET_CYCLE_COUNTER_EN_FLAG);
+	}
 #endif
 }
@@ -155,4 +199,127 @@
 }
 
+/** See chapter B4.1.19 of ARM Architecture Reference Manual */
+static unsigned dcache_linesize_log(unsigned level)
+{
+#ifdef PROCESSOR_ARCH_armv7_a
+	CSSELR_write((level & CCSELR_LEVEL_MASK) << CCSELR_LEVEL_SHIFT);
+	const unsigned ls_log = 2 +
+	    ((CCSIDR_read() >> CCSIDR_LINESIZE_SHIFT) & CCSIDR_LINESIZE_MASK);
+	return ls_log + 2; //return log2(bytes)
+#endif
+	return 0;
+
+}
+
+/** See chapter B4.1.19 of ARM Architecture Reference Manual */
+static unsigned dcache_ways(unsigned level)
+{
+#ifdef PROCESSOR_ARCH_armv7_a
+	CSSELR_write((level & CCSELR_LEVEL_MASK) << CCSELR_LEVEL_SHIFT);
+	const unsigned ways = 1 +
+	    ((CCSIDR_read() >> CCSIDR_ASSOC_SHIFT) & CCSIDR_ASSOC_MASK);
+	return ways;
+#endif
+	return 0;
+}
+
+/** See chapter B4.1.19 of ARM Architecture Reference Manual */
+static unsigned dcache_sets(unsigned level)
+{
+#ifdef PROCESSOR_ARCH_armv7_a
+	CSSELR_write((level & CCSELR_LEVEL_MASK) << CCSELR_LEVEL_SHIFT);
+	const unsigned sets = 1 +
+	    ((CCSIDR_read() >> CCSIDR_NUMSETS_SHIFT) & CCSIDR_NUMSETS_MASK);
+	return sets;
+#endif
+	return 0;
+}
+
+unsigned dcache_levels(void)
+{
+	unsigned levels = 0;
+#ifdef PROCESSOR_ARCH_armv7_a
+	const uint32_t val = CLIDR_read();
+	for (unsigned i = 1; i <= 7; ++i) {
+		const unsigned ctype = CLIDR_CACHE(i, val);
+		switch (ctype) {
+		case CLIDR_DCACHE_ONLY:
+		case CLIDR_SEP_CACHE:
+		case CLIDR_UNI_CACHE:
+			++levels;
+		default:
+			(void)0;
+		}
+	}
+#endif
+	return levels;
+}
+
+static void dcache_clean_manual(unsigned level, bool invalidate,
+    unsigned ways, unsigned sets, unsigned way_shift, unsigned set_shift)
+{
+
+	for (unsigned i = 0; i < ways; ++i) {
+		for (unsigned j = 0; j < sets; ++j) {
+			const uint32_t val =
+			    ((level & 0x7) << 1) |
+			    (j << set_shift) | (i << way_shift);
+			if (invalidate)
+				DCCISW_write(val);
+			else
+				DCCSW_write(val);
+		}
+	}
+}
+
+void dcache_flush(void)
+{
+	/* See ARM Architecture Reference Manual ch. B4.2.1 p. B4-1724 */
+	const unsigned levels = dcache_levels();
+	for (unsigned i = 0; i < levels; ++i) {
+		const unsigned ways = dcache_ways(i);
+		const unsigned sets = dcache_sets(i);
+		const unsigned way_shift =  31 - log2(ways);
+		const unsigned set_shift = dcache_linesize_log(i);
+		dcache_clean_manual(i, false, ways, sets, way_shift, set_shift);
+	}
+}
+
+void dcache_flush_invalidate(void)
+{
+	/* See ARM Architecture Reference Manual ch. B4.2.1 p. B4-1724 */
+	const unsigned levels = dcache_levels();
+	for (unsigned i = 0; i < levels; ++i) {
+		const unsigned ways = dcache_ways(i);
+		const unsigned sets = dcache_sets(i);
+		const unsigned way_shift =  31 - log2(ways);
+		const unsigned set_shift = dcache_linesize_log(i);
+		dcache_clean_manual(i, true, ways, sets, way_shift, set_shift);
+	}
+}
+
+
+void cpu_dcache_flush(void)
+{
+	for (unsigned i = 0; i < CPU->arch.dcache_levels; ++i)
+		dcache_clean_manual(i, false,
+		    CPU->arch.dcache[i].ways, CPU->arch.dcache[i].sets,
+		    CPU->arch.dcache[i].way_shift, CPU->arch.dcache[i].set_shift);
+}
+
+void cpu_dcache_flush_invalidate(void)
+{
+	const unsigned levels =  dcache_levels();
+	for (unsigned i = 0; i < levels; ++i)
+		dcache_clean_manual(i, true,
+		    CPU->arch.dcache[i].ways, CPU->arch.dcache[i].sets,
+		    CPU->arch.dcache[i].way_shift, CPU->arch.dcache[i].set_shift);
+}
+
+void icache_invalidate(void)
+{
+	ICIALLU_write(0);
+}
+
 /** @}
  */
