1
/*
2
 * Copyright (c) 2018 naehrwert
3
 * Copyright (c) 2018-2024 CTCaer
4
 *
5
 * This program is free software; you can redistribute it and/or modify it
6
 * under the terms and conditions of the GNU General Public License,
7
 * version 2, as published by the Free Software Foundation.
8
 *
9
 * This program is distributed in the hope it will be useful, but WITHOUT
10
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
12
 * more details.
13
 *
14
 * You should have received a copy of the GNU General Public License
15
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
 */
17

18
#include <soc/bpmp.h>
19
#include <soc/clock.h>
20
#include <soc/hw_init.h>
21
#include <soc/pmc.h>
22
#include <soc/timer.h>
23
#include <soc/t210.h>
24
#include <storage/sdmmc.h>
25

26
typedef struct _clock_osc_t
27
{
28
	u32 freq;
29
	u16 min;
30
	u16 max;
31
} clock_osc_t;
32

33
static const clock_osc_t _clock_osc_cnt[] = {
34
	{ 12000, 706,  757 },
35
	{ 13000, 766,  820 },
36
	{ 16800, 991,  1059 },
37
	{ 19200, 1133, 1210 },
38
	{ 26000, 1535, 1638 },
39
	{ 38400, 2268, 2418 },
40
	{ 48000, 2836, 3023 }
41
};
42

43
/* clk_rst_t: reset, enable, source, index, clk_src, clk_div */
44

45
static const clk_rst_t _clock_uart[] = {
46
	{ CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTA,   CLK_L_UARTA,   0, CLK_SRC_DIV(2) },
47
	{ CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_UARTB,   CLK_L_UARTB,   0, CLK_SRC_DIV(2) },
48
	{ CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_UARTC,   CLK_H_UARTC,   0, CLK_SRC_DIV(2) },
49
	{ CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_UARTD,   CLK_U_UARTD,   0, CLK_SRC_DIV(2) },
50
	{ CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_UARTAPE, CLK_Y_UARTAPE, 0, CLK_SRC_DIV(2) }
51
};
52

53
//I2C default parameters - TLOW: 4, THIGH: 2, DEBOUNCE: 0, FM_DIV: 26.
54
static const clk_rst_t _clock_i2c[] = {
55
	{ CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_I2C1, CLK_L_I2C1, 0, CLK_SRC_DIV(10.5) }, //20.4MHz -> 100KHz
56
	{ CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C2, CLK_H_I2C2, 0, CLK_SRC_DIV(3)    }, //81.6MHz -> 400KHz
57
	{ CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_I2C3, CLK_U_I2C3, 0, CLK_SRC_DIV(3)    }, //81.6MHz -> 400KHz
58
	{ CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_I2C4, CLK_V_I2C4, 0, CLK_SRC_DIV(10.5) }, //20.4MHz -> 100KHz
59
	{ CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_RST_CONTROLLER_CLK_SOURCE_I2C5, CLK_H_I2C5, 0, CLK_SRC_DIV(3)    }, //81.6MHz -> 400KHz
60
	{ CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_I2C6, CLK_X_I2C6, 0, CLK_SRC_DIV(10.5) }  //20.4MHz -> 100KHz
61
};
62

63
static clk_rst_t _clock_se = {
64
	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_SE,          CLK_V_SE,          0, CLK_SRC_DIV(1)   // 408MHz. Default: 408MHz. Max: 627.2 MHz.
65
};
66
static clk_rst_t _clock_tzram = {
67
	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_NO_SOURCE,                             CLK_V_TZRAM,       0, CLK_SRC_DIV(1)
68
};
69
static clk_rst_t _clock_host1x = {
70
	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_HOST1X,      CLK_L_HOST1X,      4, CLK_SRC_DIV(2.5) // 163.2MHz. Max: 408MHz.
71
};
72
static clk_rst_t _clock_tsec = {
73
	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_TSEC,        CLK_U_TSEC,        0, CLK_SRC_DIV(2)   // 204MHz. Max: 408MHz.
74
};
75
static clk_rst_t _clock_nvdec = {
76
	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_NVDEC,       CLK_Y_NVDEC,       4, CLK_SRC_DIV(1)   // 408 MHz. Max: 716.8/979.2MHz.
77
};
78
static clk_rst_t _clock_nvjpg = {
79
	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_NVJPG,       CLK_Y_NVJPG,       4, CLK_SRC_DIV(1)   // 408 MHz. Max: 627.2/652.8MHz.
80
};
81
static clk_rst_t _clock_vic = {
82
	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_VIC,         CLK_X_VIC,         2, CLK_SRC_DIV(1)   // 408 MHz. Max: 627.2/652.8MHz.
83
};
84
static clk_rst_t _clock_sor_safe = {
85
	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_NO_SOURCE,                             CLK_Y_SOR_SAFE,    0, CLK_SRC_DIV(1)
86
};
87
static clk_rst_t _clock_sor0 = {
88
	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_NOT_USED,                              CLK_X_SOR0,        0, CLK_SRC_DIV(1)
89
};
90
static clk_rst_t _clock_sor1 = {
91
	CLK_RST_CONTROLLER_RST_DEVICES_X, CLK_RST_CONTROLLER_CLK_OUT_ENB_X, CLK_RST_CONTROLLER_CLK_SOURCE_SOR1,        CLK_X_SOR1,        0, CLK_SRC_DIV(2)   // 204MHz.
92
};
93
static clk_rst_t _clock_kfuse = {
94
	CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_NO_SOURCE,                             CLK_H_KFUSE,       0, CLK_SRC_DIV(1)
95
};
96
static clk_rst_t _clock_cl_dvfs =	{
97
	CLK_RST_CONTROLLER_RST_DEVICES_W, CLK_RST_CONTROLLER_CLK_OUT_ENB_W, CLK_NO_SOURCE,                             CLK_W_DVFS,        0, CLK_SRC_DIV(1)
98
};
99
static clk_rst_t _clock_coresight = {
100
	CLK_RST_CONTROLLER_RST_DEVICES_U, CLK_RST_CONTROLLER_CLK_OUT_ENB_U, CLK_RST_CONTROLLER_CLK_SOURCE_CSITE,       CLK_U_CSITE,       0, CLK_SRC_DIV(3)   // 136MHz.
101
};
102
static clk_rst_t _clock_pwm = {
103
	CLK_RST_CONTROLLER_RST_DEVICES_L, CLK_RST_CONTROLLER_CLK_OUT_ENB_L, CLK_RST_CONTROLLER_CLK_SOURCE_PWM,         CLK_L_PWM,         6, CLK_SRC_DIV(3)   // Fref: 6.4MHz. HOS: PLLP / 54 = 7.55MHz.
104
};
105
static clk_rst_t _clock_sdmmc_legacy_tm = {
106
	CLK_RST_CONTROLLER_RST_DEVICES_Y, CLK_RST_CONTROLLER_CLK_OUT_ENB_Y, CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC_LEGACY_TM, CLK_Y_SDMMC_LEGACY_TM, 4, CLK_SRC_DIV(34) // 12MHz.
107
};
108
static clk_rst_t _clock_apbdma = {
109
	CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_NO_SOURCE,                             CLK_H_APBDMA,      0, CLK_SRC_DIV(1)   // Max: 204MHz.
110
};
111
static clk_rst_t _clock_ahbdma = {
112
	CLK_RST_CONTROLLER_RST_DEVICES_H, CLK_RST_CONTROLLER_CLK_OUT_ENB_H, CLK_NO_SOURCE,                             CLK_H_AHBDMA,      0, CLK_SRC_DIV(1)
113
};
114
static clk_rst_t _clock_actmon = {
115
	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_ACTMON,      CLK_V_ACTMON,      6, CLK_SRC_DIV(1)   // 19.2MHz.
116
};
117
static clk_rst_t _clock_extperiph1 = {
118
	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH1,  CLK_V_EXTPERIPH1,  0, CLK_SRC_DIV(1)
119
};
120
static clk_rst_t _clock_extperiph2 = {
121
	CLK_RST_CONTROLLER_RST_DEVICES_V, CLK_RST_CONTROLLER_CLK_OUT_ENB_V, CLK_RST_CONTROLLER_CLK_SOURCE_EXTPERIPH2,  CLK_V_EXTPERIPH2,  2, CLK_SRC_DIV(102) // 4.0MHz
122
};
123

124
void clock_enable(const clk_rst_t *clk)
125
{
126
	// Put clock into reset.
127
	CLOCK(clk->reset) = (CLOCK(clk->reset) & ~BIT(clk->index)) | BIT(clk->index);
128
	// Disable.
129
	CLOCK(clk->enable) &= ~BIT(clk->index);
130
	// Configure clock source if required.
131
	if (clk->source)
132
		CLOCK(clk->source) = clk->clk_div | (clk->clk_src << 29u);
133
	// Enable.
134
	CLOCK(clk->enable) = (CLOCK(clk->enable) & ~BIT(clk->index)) | BIT(clk->index);
135
	usleep(2);
136

137
	// Take clock off reset.
138
	CLOCK(clk->reset) &= ~BIT(clk->index);
139
}
140

141
void clock_disable(const clk_rst_t *clk)
142
{
143
	// Put clock into reset.
144
	CLOCK(clk->reset) = (CLOCK(clk->reset) & ~BIT(clk->index)) | BIT(clk->index);
145
	// Disable.
146
	CLOCK(clk->enable) &= ~BIT(clk->index);
147
}
148

149
void clock_enable_fuse(bool enable)
150
{
151
	// Enable Fuse registers visibility.
152
	CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) = (CLOCK(CLK_RST_CONTROLLER_MISC_CLK_ENB) & 0xEFFFFFFF) | ((enable & 1) << 28);
153
}
154

155
void clock_enable_uart(u32 idx)
156
{
157
	// Ease the stress to APB.
158
	bpmp_clk_rate_relaxed(true);
159

160
	clock_enable(&_clock_uart[idx]);
161

162
	// Restore OC.
163
	bpmp_clk_rate_relaxed(false);
164
}
165

166
void clock_disable_uart(u32 idx)
167
{
168
	clock_disable(&_clock_uart[idx]);
169
}
170

171
#define UART_SRC_CLK_DIV_EN BIT(24)
172

173
int clock_uart_use_src_div(u32 idx, u32 baud)
174
{
175
	u32 clk_src_div = CLOCK(_clock_uart[idx].source) & 0xE0000000;
176

177
	if (baud == 3000000)
178
		CLOCK(_clock_uart[idx].source) = clk_src_div | UART_SRC_CLK_DIV_EN | CLK_SRC_DIV(8.5);
179
	else if (baud == 1000000)
180
		CLOCK(_clock_uart[idx].source) = clk_src_div | UART_SRC_CLK_DIV_EN | CLK_SRC_DIV(25.5);
181
	else
182
	{
183
		CLOCK(_clock_uart[idx].source) = clk_src_div | CLK_SRC_DIV(2);
184

185
		return 1;
186
	}
187

188
	return 0;
189
}
190

191
void clock_enable_i2c(u32 idx)
192
{
193
	clock_enable(&_clock_i2c[idx]);
194
}
195

196
void clock_disable_i2c(u32 idx)
197
{
198
	clock_disable(&_clock_i2c[idx]);
199
}
200

201
void clock_enable_se()
202
{
203
	clock_enable(&_clock_se);
204

205
	// Lock clock to always enabled if T210B01.
206
	if (hw_get_chip_id() == GP_HIDREV_MAJOR_T210B01)
207
		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SE) |= 0x100;
208
}
209

210
void clock_enable_tzram()
211
{
212
	clock_enable(&_clock_tzram);
213
}
214

215
void clock_enable_host1x()
216
{
217
	clock_enable(&_clock_host1x);
218
}
219

220
void clock_disable_host1x()
221
{
222
	clock_disable(&_clock_host1x);
223
}
224

225
void clock_enable_tsec()
226
{
227
	clock_enable(&_clock_tsec);
228
}
229

230
void clock_disable_tsec()
231
{
232
	clock_disable(&_clock_tsec);
233
}
234

235
void clock_enable_nvdec()
236
{
237
	clock_enable(&_clock_nvdec);
238
}
239

240
void clock_disable_nvdec()
241
{
242
	clock_disable(&_clock_nvdec);
243
}
244

245
void clock_enable_nvjpg()
246
{
247
	clock_enable(&_clock_nvjpg);
248
}
249

250
void clock_disable_nvjpg()
251
{
252
	clock_disable(&_clock_nvjpg);
253
}
254

255
void clock_enable_vic()
256
{
257
	// Ease the stress to APB.
258
	bpmp_clk_rate_relaxed(true);
259

260
	clock_enable(&_clock_vic);
261

262
	// Restore sys clock.
263
	bpmp_clk_rate_relaxed(false);
264
}
265

266
void clock_disable_vic()
267
{
268
	clock_disable(&_clock_vic);
269
}
270

271
void clock_enable_sor_safe()
272
{
273
	clock_enable(&_clock_sor_safe);
274
}
275

276
void clock_disable_sor_safe()
277
{
278
	clock_disable(&_clock_sor_safe);
279
}
280

281
void clock_enable_sor0()
282
{
283
	clock_enable(&_clock_sor0);
284
}
285

286
void clock_disable_sor0()
287
{
288
	clock_disable(&_clock_sor0);
289
}
290

291
void clock_enable_sor1()
292
{
293
	clock_enable(&_clock_sor1);
294
}
295

296
void clock_disable_sor1()
297
{
298
	clock_disable(&_clock_sor1);
299
}
300

301
void clock_enable_kfuse()
302
{
303
	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_SET) = BIT(CLK_H_KFUSE);
304
	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_CLR) = BIT(CLK_H_KFUSE);
305
	CLOCK(CLK_RST_CONTROLLER_CLK_ENB_H_SET) = BIT(CLK_H_KFUSE);
306
	usleep(10); // Wait 10s to prevent glitching.
307

308
	CLOCK(CLK_RST_CONTROLLER_RST_DEV_H_CLR) = BIT(CLK_H_KFUSE);
309
	usleep(20); // Wait 20s fo kfuse hw to init.
310
}
311

312
void clock_disable_kfuse()
313
{
314
	clock_disable(&_clock_kfuse);
315
}
316

317
void clock_enable_cl_dvfs()
318
{
319
	clock_enable(&_clock_cl_dvfs);
320
}
321

322
void clock_disable_cl_dvfs()
323
{
324
	clock_disable(&_clock_cl_dvfs);
325
}
326

327
void clock_enable_coresight()
328
{
329
	clock_enable(&_clock_coresight);
330
}
331

332
void clock_disable_coresight()
333
{
334
	clock_disable(&_clock_coresight);
335
}
336

337
void clock_enable_pwm()
338
{
339
	// Ease the stress to APB.
340
	bpmp_clk_rate_relaxed(true);
341

342
	clock_enable(&_clock_pwm);
343

344
	// Restore OC.
345
	bpmp_clk_rate_relaxed(false);
346
}
347

348
void clock_disable_pwm()
349
{
350
	clock_disable(&_clock_pwm);
351
}
352

353
void clock_enable_apbdma()
354
{
355
	clock_enable(&_clock_apbdma);
356
}
357

358
void clock_disable_apbdma()
359
{
360
	clock_disable(&_clock_apbdma);
361
}
362

363
void clock_enable_ahbdma()
364
{
365
	clock_enable(&_clock_ahbdma);
366
}
367

368
void clock_disable_ahbdma()
369
{
370
	clock_disable(&_clock_ahbdma);
371
}
372

373
void clock_enable_actmon()
374
{
375
	clock_enable(&_clock_actmon);
376
}
377

378
void clock_disable_actmon()
379
{
380
	clock_disable(&_clock_actmon);
381
}
382

383
void clock_enable_extperiph1()
384
{
385
	clock_enable(&_clock_extperiph1);
386

387
	PMC(APBDEV_PMC_CLK_OUT_CNTRL) |= PMC_CLK_OUT_CNTRL_CLK1_SRC_SEL(OSC_CAR) | PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN;
388
	usleep(5);
389
}
390

391
void clock_disable_extperiph1()
392
{
393
	PMC(APBDEV_PMC_CLK_OUT_CNTRL) &= ~((PMC_CLK_OUT_CNTRL_CLK1_SRC_SEL(OSC_CAR)) | PMC_CLK_OUT_CNTRL_CLK1_FORCE_EN);
394
	clock_disable(&_clock_extperiph1);
395
}
396

397
void clock_enable_extperiph2()
398
{
399
	clock_enable(&_clock_extperiph2);
400

401
	PMC(APBDEV_PMC_CLK_OUT_CNTRL) |= PMC_CLK_OUT_CNTRL_CLK2_SRC_SEL(OSC_CAR) | PMC_CLK_OUT_CNTRL_CLK2_FORCE_EN;
402
	usleep(5);
403
}
404

405
void clock_disable_extperiph2()
406
{
407
	PMC(APBDEV_PMC_CLK_OUT_CNTRL) &= ~((PMC_CLK_OUT_CNTRL_CLK2_SRC_SEL(OSC_CAR)) | PMC_CLK_OUT_CNTRL_CLK2_FORCE_EN);
408
	clock_disable(&_clock_extperiph2);
409
}
410

411
void clock_enable_plld(u32 divp, u32 divn, bool lowpower, bool tegra_t210)
412
{
413
	u32 plld_div = (divp << 20) | (divn << 11) | 1;
414

415
	// N divider is fractional, so N = DIVN + 1/2 + PLLD_SDM_DIN/8192.
416
	u32 misc = 0x2D0000 | 0xFC00; // Clock enable and PLLD_SDM_DIN: -1024 -> DIVN + 0.375.
417
	if (lowpower && tegra_t210)
418
		misc = 0x2D0000 | 0x0AAA; // Clock enable and PLLD_SDM_DIN:  2730 -> DIVN + 0.833.
419

420
	// Set DISP1 clock source.
421
	CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_DISP1) = 2 << 29u; // PLLD_OUT0.
422

423
	// Set dividers and enable PLLD.
424
	CLOCK(CLK_RST_CONTROLLER_PLLD_BASE)  = PLLCX_BASE_ENABLE | PLLCX_BASE_LOCK | plld_div;
425
	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC1) = tegra_t210 ? 0x20 : 0; // Keep default PLLD_SETUP.
426

427
	// Set PLLD_SDM_DIN and enable PLLD to DSI pads.
428
	CLOCK(CLK_RST_CONTROLLER_PLLD_MISC) = misc;
429
}
430

431
void clock_enable_pllx()
432
{
433
	// Configure and enable PLLX if disabled.
434
	if (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & PLLX_BASE_ENABLE)) // PLLX_ENABLE.
435
	{
436
		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC_3) &= ~PLLX_MISC3_IDDQ; // Disable IDDQ.
437
		usleep(2);
438

439
		// Set div configuration.
440
		const u32 pllx_div_cfg = (2 << 20) | (156 << 8) | 2; // P div: 2 (3), N div: 156, M div: 2. 998.4 MHz.
441

442
		// Bypass dividers.
443
		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = PLLX_BASE_BYPASS | pllx_div_cfg;
444
		// Disable bypass
445
		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = pllx_div_cfg;
446
		// Set PLLX_LOCK_ENABLE.
447
		CLOCK(CLK_RST_CONTROLLER_PLLX_MISC) |= PLLX_MISC_LOCK_EN;
448
		// Enable PLLX.
449
		CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) = PLLX_BASE_ENABLE | pllx_div_cfg;
450
	}
451

452
	// Wait for PLL to stabilize.
453
	while (!(CLOCK(CLK_RST_CONTROLLER_PLLX_BASE) & PLLX_BASE_LOCK))
454
		;
455
}
456

457
void clock_enable_pllc(u32 divn)
458
{
459
	u8 pll_divn_curr = (CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) >> 10) & 0xFF;
460

461
	// Check if already enabled and configured.
462
	if ((CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) & PLLCX_BASE_ENABLE) && (pll_divn_curr == divn))
463
		return;
464

465
	// Take PLLC out of reset and set basic misc parameters.
466
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC) =
467
		((CLOCK(CLK_RST_CONTROLLER_PLLC_MISC) & 0xFFF0000F) & ~PLLC_MISC_RESET) | (0x8000 << 4); // PLLC_EXT_FRU.
468
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_2) |= 0xF0 << 8; // PLLC_FLL_LD_MEM.
469

470
	// Disable PLL and IDDQ in case they are on.
471
	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE)   &= ~PLLCX_BASE_ENABLE;
472
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_1) &= ~PLLC_MISC1_IDDQ;
473
	usleep(10);
474

475
	// Set PLLC dividers.
476
	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) = (divn << 10) | 4; // DIVM: 4, DIVP: 1.
477

478
	// Enable PLLC and wait for Phase and Frequency lock.
479
	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) |= PLLCX_BASE_ENABLE;
480
	while (!(CLOCK(CLK_RST_CONTROLLER_PLLC_BASE) & PLLCX_BASE_LOCK))
481
		;
482

483
	// Disable PLLC_OUT1, enable reset and set div to 1.5.
484
	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT) = 1 << 8;
485

486
	// Enable PLLC_OUT1 and bring it out of reset.
487
	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT) |= PLLC_OUT1_CLKEN | PLLC_OUT1_RSTN_CLR;
488
	msleep(1); // Wait a bit for PLL to stabilize.
489
}
490

491
void clock_disable_pllc()
492
{
493
	// Disable PLLC and PLLC_OUT1.
494
	CLOCK(CLK_RST_CONTROLLER_PLLC_OUT)    &= ~PLLC_OUT1_RSTN_CLR;
495
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC)    = PLLC_MISC_RESET;
496
	CLOCK(CLK_RST_CONTROLLER_PLLC_BASE)   &= ~PLLCX_BASE_ENABLE;
497
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_1) |= PLLC_MISC1_IDDQ;
498
	CLOCK(CLK_RST_CONTROLLER_PLLC_MISC_2) &= ~(0xFF << 8); // PLLC_FLL_LD_MEM.
499
	usleep(10);
500
}
501

502
#define PLLC4_ENABLED BIT(31)
503
#define PLLC4_IN_USE  (~PLLC4_ENABLED)
504

505
u32 pllc4_enabled = 0;
506

507
static void _clock_enable_pllc4(u32 mask)
508
{
509
	pllc4_enabled |= mask;
510

511
	if (pllc4_enabled & PLLC4_ENABLED)
512
		return;
513

514
	// Enable Phase and Frequency lock detection.
515
	//CLOCK(CLK_RST_CONTROLLER_PLLC4_MISC) = PLLC4_MISC_EN_LCKDET;
516

517
	// Disable PLL and IDDQ in case they are on.
518
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) &= ~PLLCX_BASE_ENABLE;
519
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) &= ~PLLC4_BASE_IDDQ;
520
	usleep(10);
521

522
	// Set PLLC4 dividers.
523
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) = (0 << 19) | (104 << 8) | 4; // DIVP: 1, DIVN: 104, DIVM: 4. 998MHz OUT0, 199MHz OUT2.
524

525
	// Enable PLLC4 and wait for Phase and Frequency lock.
526
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) |= PLLCX_BASE_ENABLE;
527
	while (!(CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) & PLLCX_BASE_LOCK))
528
		;
529

530
	msleep(1); // Wait a bit for PLL to stabilize.
531

532
	pllc4_enabled |= PLLC4_ENABLED;
533
}
534

535
static void _clock_disable_pllc4(u32 mask)
536
{
537
	pllc4_enabled &= ~mask;
538

539
	// Check if currently in use or disabled.
540
	if ((pllc4_enabled & PLLC4_IN_USE) || !(pllc4_enabled & PLLC4_ENABLED))
541
		return;
542

543
	// Disable PLLC4.
544
	msleep(1); // Wait at least 1ms to prevent glitching.
545
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) &= ~PLLCX_BASE_ENABLE;
546
	CLOCK(CLK_RST_CONTROLLER_PLLC4_BASE) |= PLLC4_BASE_IDDQ;
547
	usleep(10);
548

549
	pllc4_enabled = 0;
550
}
551

552
void clock_enable_pllu()
553
{
554
	// Configure PLLU.
555
	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) |= BIT(29); // Disable reference clock.
556
	u32 pllu_cfg = (CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) & 0xFFE00000) | BIT(24) | (1 << 16) | (0x19 << 8) | 2;
557
	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg;
558
	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) = pllu_cfg | PLLCX_BASE_ENABLE; // Enable.
559

560
	// Wait for PLL to stabilize.
561
	u32 timeout = get_tmr_us() + 1300;
562
	while (!(CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) & PLLCX_BASE_LOCK)) // PLL_LOCK.
563
		if (get_tmr_us() > timeout)
564
			break;
565
	usleep(10);
566

567
	// Enable PLLU USB/HSIC/ICUSB/48M.
568
	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) |= 0x2E00000;
569
}
570

571
void clock_disable_pllu()
572
{
573
	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~0x2E00000; // Disable PLLU USB/HSIC/ICUSB/48M.
574
	CLOCK(CLK_RST_CONTROLLER_PLLU_BASE) &= ~BIT(30);   // Disable PLLU.
575
	CLOCK(CLK_RST_CONTROLLER_PLLU_MISC) &= ~BIT(29);   // Enable reference clock.
576
}
577

578
void clock_enable_utmipll()
579
{
580
	// Set UTMIPLL dividers and config based on OSC and enable it to 960 MHz.
581
	CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG0) = (CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG0) & 0xFF0000FF) | (25 << 16) | (1 << 8); // 38.4Mhz * (25 / 1) = 960 MHz.
582
	CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG2) = (CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG2) & 0xFF00003F) | (24 << 18); // Set delay count for 38.4Mhz osc crystal.
583
	CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG1) = (CLOCK(CLK_RST_CONTROLLER_UTMIP_PLL_CFG1) &  0x7FFA000) | (1 << 15) | 375;
584

585
	// Wait for UTMIPLL to stabilize.
586
	u32 retries = 10; // Wait 20us
587
	while (!(CLOCK(CLK_RST_CONTROLLER_UTMIPLL_HW_PWRDN_CFG0) & UTMIPLL_LOCK) && retries)
588
	{
589
		usleep(1);
590
		retries--;
591
	}
592
}
593

594
static int _clock_sdmmc_is_reset(u32 id)
595
{
596
	switch (id)
597
	{
598
	case SDMMC_1:
599
		return CLOCK(CLK_RST_CONTROLLER_RST_DEVICES_L) & BIT(CLK_L_SDMMC1);
600
	case SDMMC_2:
601
		return CLOCK(CLK_RST_CONTROLLER_RST_DEVICES_L) & BIT(CLK_L_SDMMC2);
602
	case SDMMC_3:
603
		return CLOCK(CLK_RST_CONTROLLER_RST_DEVICES_U) & BIT(CLK_U_SDMMC3);
604
	case SDMMC_4:
605
		return CLOCK(CLK_RST_CONTROLLER_RST_DEVICES_L) & BIT(CLK_L_SDMMC4);
606
	}
607
	return 0;
608
}
609

610
static void _clock_sdmmc_set_reset(u32 id)
611
{
612
	switch (id)
613
	{
614
	case SDMMC_1:
615
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = BIT(CLK_L_SDMMC1);
616
		break;
617
	case SDMMC_2:
618
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = BIT(CLK_L_SDMMC2);
619
		break;
620
	case SDMMC_3:
621
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_U_SET) = BIT(CLK_U_SDMMC3);
622
		break;
623
	case SDMMC_4:
624
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_SET) = BIT(CLK_L_SDMMC4);
625
		break;
626
	}
627
}
628

629
static void _clock_sdmmc_clear_reset(u32 id)
630
{
631
	switch (id)
632
	{
633
	case SDMMC_1:
634
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = BIT(CLK_L_SDMMC1);
635
		break;
636
	case SDMMC_2:
637
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = BIT(CLK_L_SDMMC2);
638
		break;
639
	case SDMMC_3:
640
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_U_CLR) = BIT(CLK_U_SDMMC3);
641
		break;
642
	case SDMMC_4:
643
		CLOCK(CLK_RST_CONTROLLER_RST_DEV_L_CLR) = BIT(CLK_L_SDMMC4);
644
		break;
645
	}
646
}
647

648
static int _clock_sdmmc_is_enabled(u32 id)
649
{
650
	switch (id)
651
	{
652
	case SDMMC_1:
653
		return CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) & BIT(CLK_L_SDMMC1);
654
	case SDMMC_2:
655
		return CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) & BIT(CLK_L_SDMMC2);
656
	case SDMMC_3:
657
		return CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_U) & BIT(CLK_U_SDMMC3);
658
	case SDMMC_4:
659
		return CLOCK(CLK_RST_CONTROLLER_CLK_OUT_ENB_L) & BIT(CLK_L_SDMMC4);
660
	}
661
	return 0;
662
}
663

664
static void _clock_sdmmc_set_enable(u32 id)
665
{
666
	switch (id)
667
	{
668
	case SDMMC_1:
669
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = BIT(CLK_L_SDMMC1);
670
		break;
671
	case SDMMC_2:
672
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = BIT(CLK_L_SDMMC2);
673
		break;
674
	case SDMMC_3:
675
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_U_SET) = BIT(CLK_U_SDMMC3);
676
		break;
677
	case SDMMC_4:
678
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_SET) = BIT(CLK_L_SDMMC4);
679
		break;
680
	}
681
}
682

683
static void _clock_sdmmc_clear_enable(u32 id)
684
{
685
	switch (id)
686
	{
687
	case SDMMC_1:
688
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = BIT(CLK_L_SDMMC1);
689
		break;
690
	case SDMMC_2:
691
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = BIT(CLK_L_SDMMC2);
692
		break;
693
	case SDMMC_3:
694
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_U_CLR) = BIT(CLK_U_SDMMC3);
695
		break;
696
	case SDMMC_4:
697
		CLOCK(CLK_RST_CONTROLLER_CLK_ENB_L_CLR) = BIT(CLK_L_SDMMC4);
698
		break;
699
	}
700
}
701

702
static void _clock_sdmmc_config_legacy_tm()
703
{
704
	const clk_rst_t *clk = &_clock_sdmmc_legacy_tm;
705
	if (!(CLOCK(clk->enable) & BIT(clk->index)))
706
		clock_enable(clk);
707
}
708

709
typedef struct _clock_sdmmc_t
710
{
711
	u32 clock;
712
	u32 real_clock;
713
} clock_sdmmc_t;
714

715
static clock_sdmmc_t _clock_sdmmc_table[4] = { 0 };
716

717
#define SDMMC_CLOCK_SRC_PLLP_OUT0      0x0
718
#define SDMMC_CLOCK_SRC_PLLC4_OUT2     0x3
719
#define SDMMC_CLOCK_SRC_PLLC4_OUT0     0x7
720
#define SDMMC4_CLOCK_SRC_PLLC4_OUT2_LJ 0x1
721

722
static int _clock_sdmmc_config_clock_host(u32 *pclock, u32 id, u32 val)
723
{
724
	u32 divisor = 0;
725
	u32 source = SDMMC_CLOCK_SRC_PLLP_OUT0;
726

727
	if (id > SDMMC_4)
728
		return 0;
729

730
	// Get IO clock divisor.
731
	switch (val)
732
	{
733
	case 25000:
734
		*pclock = 24728;
735
		divisor = CLK_SRC_DIV(16.5);
736
		break;
737

738
	case 26000:
739
		*pclock = 25500;
740
		divisor = CLK_SRC_DIV(16);
741
		break;
742

743
	case 50000:
744
		*pclock = 48000;
745
		divisor = CLK_SRC_DIV(8.5);
746
		break;
747

748
	case 52000:
749
		*pclock = 51000;
750
		divisor = CLK_SRC_DIV(8);
751
		break;
752

753
	case 82000:
754
		*pclock = 81600;
755
		divisor = CLK_SRC_DIV(5);
756
		break;
757

758
	case 100000:
759
		source = SDMMC_CLOCK_SRC_PLLC4_OUT2;
760
		*pclock = 99840;
761
		divisor = CLK_SRC_DIV(2);
762
		break;
763

764
	case 164000:
765
		*pclock = 163200;
766
		divisor = CLK_SRC_DIV(2.5);
767
		break;
768

769
	case 200000:
770
		switch (id)
771
		{
772
		case SDMMC_1:
773
		case SDMMC_3:
774
			source = SDMMC_CLOCK_SRC_PLLC4_OUT2;
775
			break;
776
		case SDMMC_2:
777
		case SDMMC_4:
778
			source = SDMMC4_CLOCK_SRC_PLLC4_OUT2_LJ; // div is ignored.
779
			break;
780
		}
781
		*pclock = 199680;
782
		divisor = CLK_SRC_DIV(1);
783
		break;
784

785
#ifdef BDK_SDMMC_UHS_DDR200_SUPPORT
786
	case 400000:
787
		source = SDMMC_CLOCK_SRC_PLLC4_OUT0;
788
		*pclock = 399360;
789
		divisor = CLK_SRC_DIV(2.5);
790
		break;
791
#endif
792
	}
793

794
	_clock_sdmmc_table[id].clock = val;
795
	_clock_sdmmc_table[id].real_clock = *pclock;
796

797
	// Enable PLLC4 if in use by any SDMMC.
798
	if (source != SDMMC_CLOCK_SRC_PLLP_OUT0)
799
		_clock_enable_pllc4(BIT(id));
800

801
	// Set SDMMC legacy timeout clock.
802
	_clock_sdmmc_config_legacy_tm();
803

804
	// Set SDMMC clock.
805
	u32 src_div = (source << 29u) | divisor;
806
	switch (id)
807
	{
808
	case SDMMC_1:
809
		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC1) = src_div;
810
		break;
811
	case SDMMC_2:
812
		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC2) = src_div;
813
		break;
814
	case SDMMC_3:
815
		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC3) = src_div;
816
		break;
817
	case SDMMC_4:
818
		CLOCK(CLK_RST_CONTROLLER_CLK_SOURCE_SDMMC4) = src_div;
819
		break;
820
	}
821

822
	return 1;
823
}
824

825
void clock_sdmmc_config_clock_source(u32 *pclock, u32 id, u32 val)
826
{
827
	if (_clock_sdmmc_table[id].clock == val)
828
	{
829
		*pclock = _clock_sdmmc_table[id].real_clock;
830
	}
831
	else
832
	{
833
		int is_enabled = _clock_sdmmc_is_enabled(id);
834
		if (is_enabled)
835
			_clock_sdmmc_clear_enable(id);
836
		_clock_sdmmc_config_clock_host(pclock, id, val);
837
		if (is_enabled)
838
			_clock_sdmmc_set_enable(id);
839
		_clock_sdmmc_is_reset(id);
840
	}
841
}
842

843
void clock_sdmmc_get_card_clock_div(u32 *pclock, u16 *pdivisor, u32 type)
844
{
845
	// Get Card clock divisor.
846
	switch (type)
847
	{
848
	case SDHCI_TIMING_MMC_ID: // Actual card clock: 386.36 KHz.
849
		*pclock = 26000;
850
		*pdivisor = 66;
851
		break;
852

853
	case SDHCI_TIMING_MMC_LS26:
854
		*pclock = 26000;
855
		*pdivisor = 1;
856
		break;
857

858
	case SDHCI_TIMING_MMC_HS52:
859
		*pclock = 52000;
860
		*pdivisor = 1;
861
		break;
862

863
	case SDHCI_TIMING_MMC_HS200:
864
	case SDHCI_TIMING_MMC_HS400:
865
	case SDHCI_TIMING_UHS_SDR104:
866
		*pclock = 200000;
867
		*pdivisor = 1;
868
		break;
869

870
	case SDHCI_TIMING_SD_ID:      // Actual card clock: 386.38 KHz.
871
		*pclock = 25000;
872
		*pdivisor = 64;
873
		break;
874

875
	case SDHCI_TIMING_SD_DS12:
876
	case SDHCI_TIMING_UHS_SDR12:
877
		*pclock = 25000;
878
		*pdivisor = 1;
879
		break;
880

881
	case SDHCI_TIMING_SD_HS25:
882
	case SDHCI_TIMING_UHS_SDR25:
883
		*pclock = 50000;
884
		*pdivisor = 1;
885
		break;
886

887
	case SDHCI_TIMING_UHS_SDR50:
888
		*pclock = 100000;
889
		*pdivisor = 1;
890
		break;
891

892
	case SDHCI_TIMING_UHS_SDR82:
893
		*pclock = 164000;
894
		*pdivisor = 1;
895
		break;
896

897
	case SDHCI_TIMING_UHS_DDR50: // Actual card clock: 40.80 MHz.
898
		*pclock = 82000;
899
		*pdivisor = 2;
900
		break;
901

902
	case SDHCI_TIMING_MMC_HS100: // Actual card clock: 99.84 MHz.
903
		*pclock = 200000;
904
		*pdivisor = 2;
905
		break;
906

907
#ifdef BDK_SDMMC_UHS_DDR200_SUPPORT
908
	case SDHCI_TIMING_UHS_DDR200: // Actual card clock: 199.68 KHz.
909
		*pclock = 400000;
910
		*pdivisor = 2;
911
		break;
912
#endif
913
	}
914
}
915

916
int clock_sdmmc_is_not_reset_and_enabled(u32 id)
917
{
918
	return !_clock_sdmmc_is_reset(id) && _clock_sdmmc_is_enabled(id);
919
}
920

921
void clock_sdmmc_enable(u32 id, u32 val)
922
{
923
	u32 clock = 0;
924

925
	if (_clock_sdmmc_is_enabled(id))
926
		_clock_sdmmc_clear_enable(id);
927
	_clock_sdmmc_set_reset(id);
928
	_clock_sdmmc_config_clock_host(&clock, id, val);
929
	_clock_sdmmc_set_enable(id);
930
	_clock_sdmmc_is_reset(id);
931
	// Wait 100 cycles for reset and for clocks to stabilize.
932
	usleep((100 * 1000 + clock - 1) / clock);
933
	_clock_sdmmc_clear_reset(id);
934
	_clock_sdmmc_is_reset(id);
935
}
936

937
void clock_sdmmc_disable(u32 id)
938
{
939
	_clock_sdmmc_set_reset(id);
940
	_clock_sdmmc_clear_enable(id);
941
	_clock_sdmmc_is_reset(id);
942
	_clock_disable_pllc4(BIT(id));
943
}
944

945
u32 clock_get_osc_freq()
946
{
947
	CLOCK(CLK_RST_CONTROLLER_OSC_FREQ_DET) = OSC_FREQ_DET_TRIG | (2 - 1); // 2 periods of 32.76KHz window.
948
	while (CLOCK(CLK_RST_CONTROLLER_OSC_FREQ_DET_STATUS) & OSC_FREQ_DET_BUSY)
949
		;
950
	u32 cnt = (CLOCK(CLK_RST_CONTROLLER_OSC_FREQ_DET_STATUS) & OSC_FREQ_DET_CNT);
951
	CLOCK(CLK_RST_CONTROLLER_OSC_FREQ_DET) = 0;
952

953
	// Return frequency in KHz.
954
	for (u32 i = 0; i < ARRAY_SIZE(_clock_osc_cnt); i++)
955
		if (cnt >= _clock_osc_cnt[i].min && cnt <= _clock_osc_cnt[i].max)
956
			return _clock_osc_cnt[i].freq;
957

958
	return 0;
959
}
960

961
u32 clock_get_dev_freq(clock_pto_id_t id)
962
{
963
	const u32 pto_win = 16;
964
	const u32 pto_osc = 32768;
965

966
	u32 val = ((id & PTO_SRC_SEL_MASK) << PTO_SRC_SEL_SHIFT) | PTO_DIV_SEL_DIV1 | PTO_CLK_ENABLE | (pto_win - 1);
967
	CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL) = val;
968
	(void)CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL);
969
	usleep(2);
970

971
	CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL) = val | PTO_CNT_RST;
972
	(void)CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL);
973
	usleep(2);
974

975
	CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL) = val;
976
	(void)CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL);
977
	usleep(2);
978

979
	CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL) = val | PTO_CNT_EN;
980
	(void)CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL);
981
	usleep((1000000 * pto_win / pto_osc) + 12 + 2);
982

983
	while (CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_STATUS) & PTO_CLK_CNT_BUSY)
984
		;
985

986
	u32 cnt = CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_STATUS) & PTO_CLK_CNT;
987

988
	CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL) = 0;
989
	(void)CLOCK(CLK_RST_CONTROLLER_PTO_CLK_CNT_CNTL);
990
	usleep(2);
991

992
	u32 freq_khz = (u64)cnt * pto_osc / pto_win / 1000;
993

994
	return freq_khz;
995
}
996

997

998