vsm: PMU handling.

PMU related functions moved to pmu_trap_handler.c
Changed assembly for save/restore of the PMU context across a cluster switch

1 files changed, 58 insertions, 47 deletions

diff --git a/acsr/v7.s b/acsr/v7.s
index c94b6ce..b9ae637 100644
--- a/acsr/v7.s
+++ b/acsr/v7.s
@@ -69,70 +69,71 @@ save_performance_monitors	FUNCTION
 
 	push	{r4, r8, r9, r10}
 
-	; Ignore:
-	;        Count Enable Clear Register
-	;        Software Increment Register
-	;        Interrupt Enable Clear Register
-
 	mrc	p15,0,r8,c9,c12,0	; PMon: Control Register
 	bic	r1,r8,#1
 	mcr	p15,0,r1,c9,c12,0	; disable counter updates from here
 	isb				; 0b0 => PMCR<0>
-	mrc	p15,0,r9,c9,c12,3	; PMon: Overflow Flag Status Reg
-	mrc	p15,0,r10,c9,c12,5	; PMon: Event Counter Selection Reg
+	mrc	p15,0,r9,c9,c12,5 	; PMon: Event Counter Selection Reg
+	mrc	p15,0,r10,c9,c12,1	; PMon: Count Enable Set Reg
+	stm	r0!, {r8-r10}
+	mrc	p15,0,r8,c9,c12,2 	; PMon: Count Enable Clear Reg
+	mrc	p15,0,r9,c9,c13,0	; PMon: Cycle Counter Register
+	mrc	p15,0,r10,c9,c12,3	; PMon: Overflow flag Status Reg
 	stm	r0!, {r8-r10}
+	mrc	p15,0,r8,c9,c14,1 	; PMon: Interrupt Enable Set Reg
+	mrc	p15,0,r9,c9,c14,2	; PMon: Interrupt Enable Clear Reg
+	stm	r0!, {r8-r9}
+	mrc	p15,0,r8,c9,c12,0	; Read PMon Control Register	
 	ubfx	r9,r8,#11,#5		; extract # of event counters, N
 	tst	r9, r9
 	beq	%f1
 
-0	subs	r9,r9,#1		; decrement N
-	mcr	p15,0,r9,c9,c12,5	; PMon: select CounterN
+	mov	r8,#0
+0	mcr	p15,0,r8,c9,c12,5	; PMon: select CounterN
 	isb
 	mrc	p15,0,r3,c9,c13,1	; PMon: save Event Type register
 	mrc	p15,0,r4,c9,c13,2	; PMon: save Event Counter register
 	stm	r0!, {r3,r4}
+	add	r8,r8,#1	  	; increment index
+	cmps	r8,r9
 	bne	%b0
 
-1	mrc	p15,0,r1,c9,c13,0	; PMon: Cycle Count Register
-	mrc	p15,0,r2,c9,c14,0	; PMon: User Enable Register
-	mrc	p15,0,r3,c9,c14,1	; PMon: Interrupt Enable Set Reg
-	mrc	p15,0,r4,c9,c12,1	; PMon: Count Enable Set Register
-	stm	r0!, {r1-r4}
-
-	pop	{r4, r8, r9, r10}
+1	pop	{r4, r8, r9, r10}
 	bx	lr
 	ENDFUNC
 
 restore_performance_monitors	FUNCTION
 
-	push	{r4-r5, r8-r10, lr}
+	push	{r4-r6, r8-r10, lr}
 	; NOTE: all counters disabled by PMCR<0> == 0 on reset
 
-	; Restore performance counters
-	ldm	r0!,{r8-r10}	; recover first block of PMon context
-				; (PMCR, PMOVSR, PMSELR)
-	mov	r1, #0		; generate register of all 0's
-	mvn	r2, #0		; generate register of all 1's
+	ldr	r8,[r0]			; r8 = PMCR
+	add	r1,r0,#20		; r1 now points to saved PMOVSR
+	ldr	r9,[r1]			; r9 = PMOVSR
+	
+	mvn	r2,#0			; generate register of all 1's
 	mcr	p15,0,r2,c9,c14,2	; disable all counter related interrupts
 	mcr	p15,0,r2,c9,c12,3	; clear all overflow flags
 	isb
 
-	ubfx	r12,r8,#11,#5	; extract # of event counters, N (0-31)
+	ubfx	r12,r8,#11,#5		; extract # of event counters, N (0-31)
 	tst	r12, r12
 	beq	%f20
-	mov	r3, r12		; for N >0, generate a 2nd copy of N
+	mov	r3, r12
 	mov	r4, #1
 	lsl	r4, r4, r3
-	sub	r4, r4, #1	; set bits<N-1:0> to all 1's   
+	sub	r4, r4, #1		; set bits<N-1:0> to all 1's   
 
-0	subs    r3,r3,#1            ; decrement N
-	mcr	p15,0,r3,c9,c12,5   ; select Event CounterN
+	mov	r6, #0			; r6 index
+0	mcr	p15,0,r6,c9,c12,5	; PMon: select CounterN
 	isb
-	mrc	p15,0,r5,c9,c13,1   ; read Event Type register
+	mrc	p15,0,r5,c9,c13,1 	; read event typer register
 	bfc	r5,#0,#8
-	mcr	p15,0,r5,c9,c13,1   ; set Event Type to 0x0
-	mcr	p15,0,r2,c9,c13,2   ; set Event Counter to all 1's
+	mcr	p15,0,r5,c9,c13,1   	; set Event Type to 0x0
+	mcr	p15,0,r2,c9,c13,2   	; set Event Counter to all 1's
 	isb
+	add	r6,r6,#1
+	cmps	r6,r3
 	bne	%b0
 
 	mov	r3, #1
@@ -145,20 +146,24 @@ restore_performance_monitors	FUNCTION
 	isb
 	mcr	p15,0,r4,c9,c12,2   ; disable Event Counters
 
-	; restore the event counters
-10	subs	r12,r12,#1          ; decrement N
-	mcr	p15,0,r12,c9,c12,5  ; select Event CounterN
-	isb
-	ldm	r0!,{r3-r4}
-	mcr	p15,0,r3,c9,c13,1   ; restore Event Type
-	mcr	p15,0,r4,c9,c13,2   ; restore Event Counter
+	add	r1,r0,#32	; r1 now points to the 1st saved event counter	
+	;; Restore counters
+	mov	r6,#0
+10	mcr	p15,0,r6,c9,c12,5	; PMon: select CounterN
 	isb
+	ldm	r1!, {r3,r4}	  	; Read saved data
+	mcr	p15,0,r3,c9,c13,1	; PMon: restore Event Type register
+	mcr	p15,0,r4,c9,c13,2	; PMon: restore Event Counter register
+	add	r6,r6,#1	  	; increment index
+	cmps	r6,r12
 	bne	%b10
 
 20	tst	r9, #0x80000000		; check for cycle count overflow flag
 	beq	%f40
 	mcr	p15,0,r2,c9,c13,0	; set Cycle Counter to all 1's
 	isb
+	mov	r3, #1
+	mcr	p15,0,r3,c9,c12,0	; set the PMCR global enable bit
 	mov	r3, #0x80000000
 	mcr	p15,0,r3,c9,c12,1	; enable the Cycle Counter
 	isb
@@ -168,21 +173,27 @@ restore_performance_monitors	FUNCTION
 	bpl	%b30
 	mcr	p15,0,r3,c9,c12,2	; disable the Cycle Counter
 
-40	mcr	p15,0,r1,c9,c12,0	; clear the PMCR global enable bit
+40	mov	r1, #0
+	mcr	p15,0,r1,c9,c12,0	; clear the PMCR global enable bit
 	isb
 
-	; restore the remaining PMon registers
-	ldm	r0!,{r1-r4}
-	mcr	p15,0,r1,c9,c13,0	; restore Cycle Count Register
-	mcr	p15,0,r2,c9,c14,0	; restore User Enable Register
-	mcr	p15,0,r3,c9,c14,1	; restore Interrupt Enable Set Reg
-	mcr	p15,0,r4,c9,c12,1	; restore Count Enable Set Register
-	mcr	p15,0,r10,c9,c12,5	; restore Event Counter Selection
+	;; Restore left regs but PMCR
+	add	r1,r0,#4	; r1 now points to the PMSELR
+	ldm	r1!,{r3,r4}
+	mcr	p15,0,r3,c9,c12,5 	; PMon: Event Counter Selection Reg
+	mcr	p15,0,r4,c9,c12,1	; PMon: Count Enable Set Reg
+	ldm	r1!, {r3,r4}
+	mcr	p15,0,r4,c9,c13,0	; PMon: Cycle Counter Register
+	ldm	r1!,{r3,r4}
+	mcr	p15,0,r3,c9,c14,2	; PMon: Interrupt Enable Clear Reg	
+	mcr	p15,0,r4,c9,c14,1 	; PMon: Interrupt Enable Set Reg
+	ldr	r3,[r1]
 	isb
-	mcr	p15,0,r8,c9,c12,0	; restore the PM Control Register
+	ldr	r0,[r0]
+	mcr	p15,0,r0,c9,c12,0	; restore the PM Control Register
 	isb
 
-	pop	{r4-r5, r8-r10, pc}
+	pop	{r4-r6, r8-r10, pc}
 	ENDFUNC