1 files changed, 63 insertions, 50 deletions
diff --git a/firmware/target/arm/crt0-pp-bl.S b/firmware/target/arm/crt0-pp-bl.S
index 9ab33a78d3..7aabd2b06a 100644
--- a/firmware/target/arm/crt0-pp-bl.S
+++ b/firmware/target/arm/crt0-pp-bl.S
@@ -32,23 +32,31 @@ start:
 *
 */
 #if CONFIG_CPU == PP5002
-    .equ    PROC_ID,    0xc4000000
+    .equ    PROC_ID,     0xc4000000
-    .equ    COP_CTRL,   0xcf004058
+    .equ    CPU_CTRL,    0xcf004054
-    .equ    COP_STATUS, 0xcf004050
+    .equ    CPU_STATUS,  0xcf004050
-    .equ    IIS_CONFIG, 0xc0002500
+    .equ    COP_CTRL,    0xcf004058
-    .equ    SLEEP,      0xca
+    .equ    COP_STATUS,  0xcf004050
-    .equ    WAKE,       0xce
+    .equ    IIS_CONFIG,  0xc0002500
-    .equ    SLEEPING,   0x4000
+    .equ    SLEEP,       0xca
+    .equ    WAKE,        0xce
+    .equ    CPUSLEEPING, 0x8000
+    .equ    COPSLEEPING, 0x4000
+    .equ    CACHE_CTRL,  0xcf004024
+    .equ    CACHE_ENAB,  0x2 /* Actually the CACHE_INIT flag */
 #else
-    .equ    PROC_ID,    0x60000000
+    .equ    PROC_ID,     0x60000000
-    .equ    COP_CTRL,   0x60007004
+    .equ    CPU_CTRL,    0x60007000
-    .equ    COP_STATUS, 0x60007004
+    .equ    CPU_STATUS,  0x60007000
-    .equ    IIS_CONFIG, 0x70002800
+    .equ    COP_CTRL,    0x60007004
-    .equ    SLEEP,      0x80000000
+    .equ    COP_STATUS,  0x60007004
-    .equ    WAKE,       0x0
+    .equ    IIS_CONFIG,  0x70002800
-    .equ    SLEEPING,   0x80000000
+    .equ    SLEEP,       0x80000000
-    .equ    CACHE_CTRL, 0x6000c000
+    .equ    WAKE,        0x0
-    .equ    CACHE_ENAB, 0x1
+    .equ    CPUSLEEPING, 0x80000000
+    .equ    COPSLEEPING, 0x80000000
+    .equ    CACHE_CTRL,  0x6000c000
+    .equ    CACHE_ENAB,  0x1
 #endif
    msr    cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */
@@ -94,23 +102,12 @@ cop:
    ldr    r0, =COP_CTRL
    mov    r1, #SLEEP
    str    r1, [r0]
+    nop
-#ifdef CPU_PP502x
+    nop
-     /* COP: Invalidate cache if enabled */
+    
-    ldr    r2, =CACHE_CTRL
+    /* Invalidate cache */
-    ldr    r1, [r2]
+    mov    r0, #1
-    tst    r1, #CACHE_ENAB
+    bl     cache_op
-    beq    2f
-    ldr    r0, =0xf000f044
-    ldr    r1, [r0]
-    orr    r1, r1, #0x6
-    str    r1, [r0]
-1:
-    ldr    r1, [r2]
-    tst    r1, #0x8000
-    bne    1b
-2:
-#endif /* CPU_PP502x */
    ldr    r0, =startup_loc
    ldr    pc, [r0]
@@ -120,7 +117,7 @@ cpu:
    ldr    r0, =COP_STATUS
 1:
    ldr    r1, [r0]
-    tst    r1, #SLEEPING
+    tst    r1, #COPSLEEPING
    beq    1b
    
    /* Initialise bss section to zero */
@@ -148,22 +145,9 @@ cpu:
    ldr    r1, =startup_loc
    str    r0, [r1]
-#ifdef CPU_PP502x
+    /* flush cache */    
-    /* Flush cache if enabled */
+    mov    r0, #0
-    ldr    r2, =CACHE_CTRL
+    bl     cache_op
-    ldr    r1, [r2]
-    tst    r1, #CACHE_ENAB
-    beq    2f
-    ldr    r0, =0xf000f044
-    ldr    r1, [r0]
-    orr    r1, r1, #0x2
-    str    r1, [r0]
-1:
-    ldr    r1, [r2]
-    tst    r1, #0x8000
-    bne    1b
-2:
-#endif /* CPU_PP502x */
    /* Wake up the coprocessor before executing the firmware */
    ldr    r0, =COP_CTRL
@@ -192,3 +176,32 @@ boot_table:
       code+data must stay <= 256 bytes */
    .space 400
 #endif
+cache_op:
+    ldr    r2, =CACHE_CTRL
+    ldr    r1, [r2]
+    tst    r1, #CACHE_ENAB
+    bxeq   lr
+    cmp    r0, #0
+#ifdef CPU_PP502x
+    ldr    r0, =0xf000f044
+    ldr    r1, [r0]
+    orrne  r1, r1, #0x6
+    orreq  r1, r1, #0x2
+    str    r1, [r0]
+1:
+    ldr    r1, [r2]
+    tst    r1, #0x8000
+    bne    1b
+#elif CONFIG_CPU == PP5002
+    ldrne  r0, =0xf0004000
+    ldreq  r0, =0xf000c000
+    add    r1, r0, #0x2000
+    mov    r2, #0
+1:
+    cmp    r1, r0
+    strhi  r2, [r0], #16
+    bhi    1b
+#endif /* CPU type */
+    bx     lr

diff --git a/firmware/target/arm/crt0-pp-bl.S b/firmware/target/arm/crt0-pp-bl.S index 9ab33a78d3..7aabd2b06a 100644 --- a/firmware/target/arm/crt0-pp-bl.S +++ b/firmware/target/arm/crt0-pp-bl.S
@@ -32,23 +32,31 @@ start:
32	*	32	*
33	*/	33	*/
34	#if CONFIG_CPU == PP5002	34	#if CONFIG_CPU == PP5002
35	.equ PROC_ID, 0xc4000000	35	.equ PROC_ID, 0xc4000000
36	.equ COP_CTRL, 0xcf004058	36	.equ CPU_CTRL, 0xcf004054
37	.equ COP_STATUS, 0xcf004050	37	.equ CPU_STATUS, 0xcf004050
38	.equ IIS_CONFIG, 0xc0002500	38	.equ COP_CTRL, 0xcf004058
39	.equ SLEEP, 0xca	39	.equ COP_STATUS, 0xcf004050
40	.equ WAKE, 0xce	40	.equ IIS_CONFIG, 0xc0002500
41	.equ SLEEPING, 0x4000	41	.equ SLEEP, 0xca
		42	.equ WAKE, 0xce
		43	.equ CPUSLEEPING, 0x8000
		44	.equ COPSLEEPING, 0x4000
		45	.equ CACHE_CTRL, 0xcf004024
		46	.equ CACHE_ENAB, 0x2 /* Actually the CACHE_INIT flag */
42	#else	47	#else
43	.equ PROC_ID, 0x60000000	48	.equ PROC_ID, 0x60000000
44	.equ COP_CTRL, 0x60007004	49	.equ CPU_CTRL, 0x60007000
45	.equ COP_STATUS, 0x60007004	50	.equ CPU_STATUS, 0x60007000
46	.equ IIS_CONFIG, 0x70002800	51	.equ COP_CTRL, 0x60007004
47	.equ SLEEP, 0x80000000	52	.equ COP_STATUS, 0x60007004
48	.equ WAKE, 0x0	53	.equ IIS_CONFIG, 0x70002800
49	.equ SLEEPING, 0x80000000	54	.equ SLEEP, 0x80000000
50	.equ CACHE_CTRL, 0x6000c000	55	.equ WAKE, 0x0
51	.equ CACHE_ENAB, 0x1	56	.equ CPUSLEEPING, 0x80000000
		57	.equ COPSLEEPING, 0x80000000
		58	.equ CACHE_CTRL, 0x6000c000
		59	.equ CACHE_ENAB, 0x1
52	#endif	60	#endif
53		61
54	msr cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */	62	msr cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */
@@ -94,23 +102,12 @@ cop:
94	ldr r0, =COP_CTRL	102	ldr r0, =COP_CTRL
95	mov r1, #SLEEP	103	mov r1, #SLEEP
96	str r1, [r0]	104	str r1, [r0]
97		105	nop
98	#ifdef CPU_PP502x	106	nop
99	/* COP: Invalidate cache if enabled */	107
100	ldr r2, =CACHE_CTRL	108	/* Invalidate cache */
101	ldr r1, [r2]	109	mov r0, #1
102	tst r1, #CACHE_ENAB	110	bl cache_op
103	beq 2f
104	ldr r0, =0xf000f044
105	ldr r1, [r0]
106	orr r1, r1, #0x6
107	str r1, [r0]
108	1:
109	ldr r1, [r2]
110	tst r1, #0x8000
111	bne 1b
112	2:
113	#endif /* CPU_PP502x */
114		111
115	ldr r0, =startup_loc	112	ldr r0, =startup_loc
116	ldr pc, [r0]	113	ldr pc, [r0]
@@ -120,7 +117,7 @@ cpu:
120	ldr r0, =COP_STATUS	117	ldr r0, =COP_STATUS
121	1:	118	1:
122	ldr r1, [r0]	119	ldr r1, [r0]
123	tst r1, #SLEEPING	120	tst r1, #COPSLEEPING
124	beq 1b	121	beq 1b
125		122
126	/* Initialise bss section to zero */	123	/* Initialise bss section to zero */
@@ -148,22 +145,9 @@ cpu:
148	ldr r1, =startup_loc	145	ldr r1, =startup_loc
149	str r0, [r1]	146	str r0, [r1]
150		147
151	#ifdef CPU_PP502x	148	/* flush cache */
152	/* Flush cache if enabled */	149	mov r0, #0
153	ldr r2, =CACHE_CTRL	150	bl cache_op
154	ldr r1, [r2]
155	tst r1, #CACHE_ENAB
156	beq 2f
157	ldr r0, =0xf000f044
158	ldr r1, [r0]
159	orr r1, r1, #0x2
160	str r1, [r0]
161	1:
162	ldr r1, [r2]
163	tst r1, #0x8000
164	bne 1b
165	2:
166	#endif /* CPU_PP502x */
167		151
168	/* Wake up the coprocessor before executing the firmware */	152	/* Wake up the coprocessor before executing the firmware */
169	ldr r0, =COP_CTRL	153	ldr r0, =COP_CTRL
@@ -192,3 +176,32 @@ boot_table:
192	code+data must stay <= 256 bytes */	176	code+data must stay <= 256 bytes */
193	.space 400	177	.space 400
194	#endif	178	#endif
		179
		180	cache_op:
		181	ldr r2, =CACHE_CTRL
		182	ldr r1, [r2]
		183	tst r1, #CACHE_ENAB
		184	bxeq lr
		185	cmp r0, #0
		186	#ifdef CPU_PP502x
		187	ldr r0, =0xf000f044
		188	ldr r1, [r0]
		189	orrne r1, r1, #0x6
		190	orreq r1, r1, #0x2
		191	str r1, [r0]
		192	1:
		193	ldr r1, [r2]
		194	tst r1, #0x8000
		195	bne 1b
		196	#elif CONFIG_CPU == PP5002
		197	ldrne r0, =0xf0004000
		198	ldreq r0, =0xf000c000
		199	add r1, r0, #0x2000
		200	mov r2, #0
		201	1:
		202	cmp r1, r0
		203	strhi r2, [r0], #16
		204	bhi 1b
		205	#endif /* CPU type */
		206	bx lr
		207