 @@ -1,14 +1,14 @@
-/*	$NetBSD: a9_mpsubr.S,v 1.1 2012/09/01 00:03:14 matt Exp $	*/
+/*	$NetBSD: a9_mpsubr.S,v 1.2 2012/09/02 05:01:54 matt Exp $	*/
 /*-
  * Copyright (c) 2012 The NetBSD Foundation, Inc.
  * All rights reserved.
+ *
  * This code is derived from software contributed to The NetBSD Foundation
  * by Matt Thomas of 3am Software Foundry.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
 @@ -24,34 +24,43 @@
  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include "opt_cpuoptions.h"
 #include "opt_cputypes.h"
 #include "opt_multiprocessor.h"
 #include <arm/asm.h>
 #include <arm/armreg.h>
 #include <arm/cortex/scu_reg.h>
 #include "assym.h"
 /* We'll modify va and pa at run time so we can use relocatable addresses. */
 #define MMU_INIT(va,pa,n_sec,attr) \
 	.word	va					    ; \
 	.word	pa					    ; \
 	.word	n_sec					    ; \
 	.word	attr					    ;
 /*
  * Set up a preliminary mapping in the MMU to allow us to run
  * at KERNEL_BASE with caches on.
  */
 arm_boot_l1pt_init:
-	mv	ip, r1			@ save mmu table addr
+	mov	ip, r1			@ save mmu table addr
 	/* Build page table from scratch */
 	mov	r1, r0			/* Start address to clear memory. */
 	/* Zero the entire table so all virtual addresses are invalid. */
 	mov	r2, #L1_TABLE_SIZE	/* in bytes */
 	mov	r3, #0
 	mov	r4, r3
 	mov	r5, r3
 	mov	r6, r3
 	mov	r7, r3
 	mov	r8, r3
 	mov	r10, r3
 	mov	r11, r3
 :	stmia	r1!, {r3-r8,r10-r11}
 @@ -62,58 +71,54 @@ arm_boot_l1pt_init:
 	bne	1b
 	/* Now create our entries per the mmu_init_table. */
 	l1table	.req r0
 	va	.req r1
 	pa	.req r2
 	n_sec	.req r3
 	attr	.req r4
 	itable	.req r5
 	mov	itable, ip		@ reclaim table address
 	b	3f
-:	str	pa, [l1table, va]
+:	str	pa, [l1table, va, lsl #2]
-	add	va, va, #4
+	add	va, va, #1
 	add	pa, pa, #(L1_S_SIZE)
 	subs	n_sec, n_sec, #1
 	bhi	2b
 :	ldmia	itable!, {va,pa,n_sec,attr}
 	/* Convert va to l1 offset:	va = 4 * (va >> L1_S_SHIFT)	*/
 	lsr	va, va, #L1_S_SHIFT
 	lsl	va, va, #2
 	/* Convert pa to l1 entry:	pa = (pa & L1_S_FRAME) | attr	*/
 #ifdef _ARM_ARCH_7
 	bfc	pa, #0, #L1_S_SHIFT
 #else
 	lsr	pa, pa, #L1_S_SHIFT
 	lsl	pa, pa, #L1_S_SHIFT
-#ndif
+#endif
 	orr	pa, pa, attr
 	cmp	n_sec, #0
 	bne	2b
 	bx	lr			@ return
 	.unreq	va
 	.unreq	pa
 	.unreq	n_sec
 	.unreq	attr
 	.unreq	itable
 	.unreq	l1table
 .Lctl_ID:
 	.word	CPU_CONTROL_IC_ENABLE|CPU_CONTROL_DC_ENABLE
 a9_cpuinit:
 	/*
 	 * In theory, because the MMU is off, we shouldn't need all of this,
 	 * but let's not take any chances and do a typical sequence to set
 	 * the Translation Table Base.
 	 */
 	mov	ip, lr
 	mov	r10, r0
 	mrc	p15, 0, r2, c1, c0, 0	/*  "       "   "     */
 	bic	r2, r2, #CPU_CONTROL_DC_ENABLE	@ clear data cache enable
 	bic	r2, r2, #CPU_CONTROL_IC_ENABLE	@ clear instruction cache enable
 	mcr	p15, 0, r2, c1, c0, 0	/*  "       "   "     */
 @@ -149,68 +154,90 @@ a9_cpuinit:
 	.align 5
 	@ turn mmu on!
 	mov	r0, r0
 	mcr	p15, 0, r0, c1, c0, 0
 	/*
 	 * Ensure that the coprocessor has finished turning on the MMU.
 	 */
 	mrc	p15, 0, r0, c0, c0, 0	/* Read an arbitrary value. */
 	mov	r0, r0			/* Stall until read completes. */
 	bx	ip			/* return */
-#if defined(VERBOSE_INIT_ARM) && XPUTC
+/*
  * Coprocessor register initialization values
  */
 	.p2align 2
 	/* bits to clear in the Control Register */
 .Lcontrol_clr:
 	.word	0
 	/* bits to set in the Control Register */
 .Lcontrol_set:
 	.word CPU_CONTROL_MMU_ENABLE	|			\
 	      CPU_CONTROL_AFLT_ENABLE	|			\
 	      CPU_CONTROL_DC_ENABLE	|			\
 	      CPU_CONTROL_SYST_ENABLE	|			\
 	      CPU_CONTROL_SWP_ENABLE	|			\
 	      CPU_CONTROL_IC_ENABLE
 #if defined(VERBOSE_INIT_ARM) && XPUTC_COM
 #define	TIMO	0x25000
 #ifndef COM_MULT
 #define	COM_MULT	1
 #endif
 xputc:
 #ifdef MULTIPROCESSOR
 	mov	r2, #1
 	ldr	r3, .Lcomlock
 :
 	ldrex	r1, [r3]
 	cmp	r1, #0
 	bne	10b
 	strex	r1, r2, [r3]
 	cmp	r1, #0
 	bne	10b
 	dsb
 #endif
 	mov	r2, #TIMO
 	ldr	r3, .Luart0
-:	ldrb	r1, [r3, #COM_LSR]
+:	ldrb	r1, [r3, #(COM_LSR*COM_MULT)]
 	tst	r1, #LSR_TXRDY
 	bne	2f
 	subs	r2, r2, #1
 	bne	1b
 :
 	strb	r0, [r3, #COM_DATA]
 	mov	r2, #TIMO
-:	ldrb	r1, [r3, #COM_LSR]
+:	ldrb	r1, [r3, #(COM_LSR*COM_MULT)]
 	tst	r1, #LSR_TSRE
 	bne	4f
 	subs	r2, r2, #1
 	bne	3b
 :
 #ifdef MULTIPROCESSOR
 	ldr	r3, .Lcomlock
 	mov	r0, #0
 	str	r0, [r3]
 	dsb
 #endif
 	bx	lr
 .Luart0:
-	.word	CONSADDR
+	.word	CONADDR
 #ifdef MULTIPROCESSOR
 .Lcomlock:
 	.word	comlock
 	.pushsection .data
 comlock:
 	.p2align 2
 	.word	0		@ not in bss
 	.popsection
 #endif /* MULTIPROCESSOR */
 #endif /* VERBOSE_INIT_ARM */
 @@ -219,103 +246,107 @@ comlock:
 a9_start:
 	mov	r10, lr				@ save lr
 	cpsid	if, #PSR_SVC32_MODE
 	XPUTC(#64)
 	bl	_C_LABEL(armv7_icache_inv_all)	@ invalidate i-cache
 	/*
 	 * Step 1a, invalidate the all cache tags in all ways on the SCU.
 	 */
 	XPUTC(#65)
 	mrc	p15, 4, r3, c15, c0, 0		@ read cbar
-	ldr	r0, [r3, #4]			@ read scu config
+	ldr	r0, [r3, #SCU_CFG]		@ read scu config
 	and	r0, r0, #7			@ get cpu max
-	add	r0, r0, #1			@ adjust to cpu num
+	add	r0, r0, #2			@ adjust to cpu num
-	lsl	r0, r0, #4			@ multiply by 16
+	mov	r1, #0xf			@ select all ways
-	sub	r0, r0, #1			@ make it into a mask
+	lsl	r1, r1, r0			@ shift into place
-	str	r0, [r3, #12]			@ write scu invalidate all
+	str	r1, [r3, #SCU_INV_ALL_REG]	@ write scu invalidate all
 	dsb
 	isb
 	/*
 	 * Step 1b, invalidate the data cache
 	 */
 	XPUTC(#66)
 	bl	_C_LABEL(armv7_dcache_wbinv_all)	@ writeback/invalidate d-cache
 	/*
 	 * Step 2, disable the data cache
 	 */
 	mrc	p15, 0, r2, c1, c0, 0		@ get system ctl register (save)
 	bic	r1, r2, #CPU_CONTROL_DC_ENABLE	@ clear data cache enable
 	mcr	p15, 0, r1, c1, c0, 0		@ set system ctl register
 	isb
 	/*
 	 * Step 3, enable the SCU (and set SMP mode)
 	 */
-	ldr	r1, [r3, #4]			@ read scu config
+	ldr	r1, [r3, #SCU_CTL]		@ read scu control
-	orr	r1, r1, #0xf0			@ set smp mode
+	orr	r1, r1, #SCU_CTL_SCU_ENA	@ set scu enable flag
-	str	r1, [r3, #4]			@ write scu config
+	str	r1, [r3, #SCU_CTL]		@ write scu control
 	ldr	r1, [r3, #0]			@ read scu control
 	orr	r1, r1, #1			@ set scu enable flag
 	str	r1, [r3, #4]			@ write scu control
 	dsb
 	isb
 	/*
 	 * Step 4a, enable the data cache
 	 */
 	mcr	p15, 0, r2, c1, c0, 0		@ reenable caches
 	isb
 	/*
 	 * Step 4b, set ACTLR.SMP=1 (and ACTRL.FX=1)
 	 */
 	mrc	p15, 0, r0, c1, c0, 1		@ read aux ctl
-	orr	r0, #0x41			@ enable cache/tlb/coherency
+	orr	r0, #CORTEXA9_AUXCTL_SMP	@ enable SMP
 	mcr	p15, 0, r0, c1, c0, 1		@ write aux ctl
 	isb
 	orr	r0, #CORTEXA9_AUXCTL_FW		@ enable cache/tlb/coherency
 	mcr	p15, 0, r0, c1, c0, 1		@ write aux ctl
 	isb
 	bx	r10
-ASEND(a9_startup)
+ASEND(a9_start)
 /*
  * Secondary processors come here after exiting the SKU ROM.
  */
 a9_mpstart:
 #ifdef MULTIPROCESSOR
 	/*
 	 * Step 1, invalidate the caches
 	 */
 	bl	_C_LABEL(armv7_icache_inv_all)	@ toss i-cache
 	bl	_C_LABEL(armv7_dcache_inv_all)	@ toss d-cache
 	/*
 	 * Step 2, wait for the SCU to be enabled
 	 */
 	mrc	p15, 4, r3, c15, c0, 0		@ read cbar
-:	ldr	r0, [r3, #0]			@ read scu control
+:	ldr	r0, [r3, #SCU_CTL]		@ read scu control
-	tst	r0, #1				@ enable bit set yet?
+	tst	r0, #SCU_CTL_SCU_ENA		@ enable bit set yet?
 	bne	1b				@ try again
 	/*
 	 * Step 3, set ACTLR.SMP=1 (and ACTRL.FX=1)
 	 */
 	mrc	p15, 0, r0, c1, c0, 1		@ read aux ctl
-	orr	r0, #0x41			@ enable cache/tlb/coherency
+	orr	r0, #CORTEXA9_AUXCTL_SMP	@ enable SMP
 	mcr	p15, 0, r0, c1, c0, 1		@ write aux ctl
 	mov	r0, r0
 	orr	r0, #CORTEXA9_AUXCTL_FW		@ enable cache/tlb/coherency
 	mcr	p15, 0, r0, c1, c0, 1		@ write aux ctl
 	mov	r0, r0
 	/*
 	 * We should be in SMP mode now.
 	 */
 	mrc	p15, 0, r4, c0, c0, 5		@ get MPIDR
 	and	r4, r4, #7			@ get our cpu numder
 #if defined(VERBOSE_INIT_ARM)
 	add	r0, r4, #48
 	bl	xputc
 #endif
 	ldr	r0, .Lcpu_hatched		@ now show we've hatched
 @@ -370,27 +401,27 @@ a9_mpstart:
 #else
 #error either TPIDRPRW_IS_CURCPU or TPIDRPRW_IS_CURLWP must be defined
 #endif
 	str	r6, [r5, #CI_CURLWP]		/* and note we are running on it */
 	ldr	r0, .Lcpu_marker
 	str	pc, [r0]
 	mov	r0, r5				/* pass cpu_info */
 	mov	r1, r4				/* pass cpu_id */
 	ldr	r2, .Lbcm53xx_cpu_hatch		/* pass md_cpu_hatch */
 	bl	_C_LABEL(cpu_hatch)
 	b	_C_LABEL(idle_loop)
 ASEND(a9_mpstart)
 	/* NOT REACHED */
 .Lkernel_l1pt:
 	.word	_C_LABEL(kernel_l1pt)
 .Lcpu_info:
 	.word	_C_LABEL(cpu_info)
 .Lcpu_max:
 	.word	_C_LABEL(arm_cpu_max)
 .Lcpu_hatched:
 	.word	_C_LABEL(arm_cpu_hatched)
 .Lcpu_mbox:
 	.word	_C_LABEL(arm_cpu_mbox)
 .Lcpu_marker: