vc4boot/clkgetf.S at master · ptesarik/vc4boot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
;;;
;;; Get clock frequency
;;;

.include "config.inc"
.include "mmio.inc"

.if RPI_MODEL < 4
;;; Raspberry Pi 1, 2 & 3 use a 19.2 MHz base crystal
	OSCFREQ = 0x4b927c00	; 19.2 * 10^6
.else
;;; Raspberry Pi 4 has a 54.0 MHz base crystal
	OSCFREQ = 0x4c4dfe60	; 54.0 * 10^6
.endif

	.text

;;; r0 ... offset to the Clock Manager control register
;;;        (e.g. CM_VPUCTL)
;;; Returns: clock frequency in Hz
;;;
	.global clk_get_freq
clk_get_freq:
	mov  r1, A2W_BASE
	add  r0, CM_BASE
	ld   r2, (r0 + 4)	; xxx_DIV register
	beq  r2, 0, .L_gnd
	fltu r2, r2, sasr 12

	;; floating point divisor is in r2, now determine base rate:
	ld  r3, (r0)		; xxx_CTL register
	bmask r3, 3
	bgt  r3, 9, .L_unk_source
	switch r3
1:
	.case .L_gnd - 1b	; GND
	.case .L_xosc - 1b	; XOSC
	.case .L_unk_source - 1b ; TESTDEBUG0
	.case .L_unk_source - 1b ; TESTDEBUG1
	.case .L_plla - 1b	; PLLA_CORE
	.case .L_pllc0 - 1b	; PLLC_CORE0
	.case .L_plld - 1b	; PLLD_CORE
	.case .L_pllh - 1b	; PLLH_AUX
	.case .L_pllc1 - 1b	; PLLC_CORE1
	.case .L_pllc2 - 1b	; PLLC_CORE2
	.endswitch

.L_xosc:
	mov   r0, OSCFREQ
	fdiv  r0, r0, r2
	floor r0, r0, sasl0
	rts

.L_plla:
	ld  r0, (r1 + A2W_PLLA_CORE)
	ld  r3, (r1 + A2W_PLLA_ANA1)
	ld  r4, (r1 + A2W_PLLA_CTRL)
	ld  r5, (r1 + A2W_PLLA_FRAC)
	btest r3, 14
	b   .L_pll

.L_plld:
	ld  r0, (r1 + A2W_PLLD_CORE)
	ld  r3, (r1 + A2W_PLLD_ANA1)
	ld  r4, (r1 + A2W_PLLD_CTRL)
	ld  r5, (r1 + A2W_PLLD_FRAC)
	btest r3, 14
	b   .L_pll

.L_pllh:
	ld  r0, (r1 + A2W_PLLH_AUX)
	ld  r3, (r1 + A2W_PLLH_ANA1)
	ld  r4, (r1 + A2W_PLLH_CTRL)
	ld  r5, (r1 + A2W_PLLH_FRAC)
	btest r3, 11
	bic r4, 0x300		; PLLH ndiv is only 8 bits
	b   .L_pll

.L_pllc2:
	ld  r0, (r1 + A2W_PLLC_CORE2)
	b   .L_pllc
.L_pllc1:
	ld  r0, (r1 + A2W_PLLC_CORE1)
	b   .L_pllc
.L_pllc0:
	ld  r0, (r1 + A2W_PLLC_CORE0)
.L_pllc:
	ld  r3, (r1 + A2W_PLLC_ANA1)
	ld  r4, (r1 + A2W_PLLC_CTRL)
	ld  r5, (r1 + A2W_PLLC_FRAC)
	btest r3, 14
.L_pll:
	;; combine PLL integer and fractional parts
	bmask r3, r4, 10	; integer part (ndiv)
	shl.ne r3, r3, 1	; prediv (ANA1)
	bmask r5, 20
	shl.ne r5, r5, 1	; prediv (ANA1)
	fltu  r5, r5, sasr 20	; fractional part (fdiv)
	fltu  r3, r3, sasr 0
	fadd  r5, r5, r3

	;; apply pre-divider
	lsr   r4, 12		; pre-divider (pdiv)
	bmask r4, 3
	beq   r4, 0, .L_gnd
	fltu  r4, r4, sasr 0
	fdiv  r5, r5, r4

	;; adjust accumulated divider by PLL divider
	bmask r0, 8
	fltu r0, r0, sasr 0
	fmul r2, r2, r0

	;; calculate base VCO rate
	mov   r0, OSCFREQ
	fmul  r5, r0, r5

	;; apply cumulated clock divider
	fdiv  r0, r5, r2

	;; convert to integer
	floor r0, r0, sasl0
	rts

.L_unk_source:
	;; unknown clock source, nothing we can handle
.L_gnd:
	mov  r0, 0
	rts