forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-11 6778948f9de86c3cfaf36725a7c87dcff9ba247f 
 kernel/drivers/clk/rockchip/clk-pll.c
....@@ -332,6 +332,64 @@
332332 	return rate_table;
333333 }
334334 
335
+static u32
336
+rockchip_rk3588_pll_frac_get(u32 m, u32 p, u32 s, u64 fin_hz, u64 fvco)
337
+{
338
+	u64 fref, fout, ffrac;
339
+	u32 k = 0;
340
+
341
+	fref = fin_hz / p;
342
+	ffrac = fvco - (m * fref);
343
+	fout = ffrac * 65536;
344
+	k = fout / fref;
345
+	if (k > 32767) {
346
+		fref = fin_hz / p;
347
+		ffrac = ((m + 1) * fref) - fvco;
348
+		fout = ffrac * 65536;
349
+		k = ((fout * 10 / fref) + 7) / 10;
350
+		if (k > 32767)
351
+			k = 0;
352
+		else
353
+			k = ~k + 1;
354
+	}
355
+	return k;
356
+}
357
+
358
+static struct rockchip_pll_rate_table *
359
+rockchip_rk3588_pll_frac_by_auto(unsigned long fin_hz,  unsigned long fout_hz)
360
+{
361
+	struct rockchip_pll_rate_table *rate_table = rk_pll_rate_table_get();
362
+	u64 fvco_min = 2250 * MHZ, fvco_max = 4500 * MHZ;
363
+	u32 p, m, s, k;
364
+	u64 fvco;
365
+
366
+	for (s = 0; s <= 6; s++) {
367
+		fvco = (u64)fout_hz << s;
368
+		if (fvco < fvco_min || fvco > fvco_max)
369
+			continue;
370
+		for (p = 1; p <= 4; p++) {
371
+			for (m = 64; m <= 1023; m++) {
372
+				if ((fvco >= m * fin_hz / p) && (fvco < (m + 1) * fin_hz / p)) {
373
+					k = rockchip_rk3588_pll_frac_get(m, p, s,
374
+									 (u64)fin_hz,
375
+									 fvco);
376
+					if (!k)
377
+						continue;
378
+					rate_table->p = p;
379
+					rate_table->s = s;
380
+					rate_table->k = k;
381
+					if (k > 32767)
382
+						rate_table->m = m + 1;
383
+					else
384
+						rate_table->m = m;
385
+					return rate_table;
386
+				}
387
+			}
388
+		}
389
+	}
390
+	return NULL;
391
+}
392
+
335393 static struct rockchip_pll_rate_table *
336394 rockchip_rk3588_pll_clk_set_by_auto(struct rockchip_clk_pll *pll,
337395 				    unsigned long fin_hz,
....@@ -341,7 +399,7 @@
341399 	u64 fvco_min = 2250 * MHZ, fvco_max = 4500 * MHZ;
342400 	u64 fout_min = 37 * MHZ, fout_max = 4500 * MHZ;
343401 	u32 p, m, s;
344
-	u64 fvco, fref, fout, ffrac;
402
+	u64 fvco;
345403 
346404 	if (fin_hz == 0 || fout_hz == 0 || fout_hz == fin_hz)
347405 		return NULL;
....@@ -368,26 +426,11 @@
368426 		}
369427 		pr_err("CANNOT FIND Fout by auto,fout = %lu\n", fout_hz);
370428 	} else {
371
-		for (s = 0; s <= 6; s++) {
372
-			fvco = (u64)fout_hz << s;
373
-			if (fvco < fvco_min || fvco > fvco_max)
374
-				continue;
375
-			for (p = 1; p <= 4; p++) {
376
-				for (m = 64; m <= 1023; m++) {
377
-					if ((fvco >= m * fin_hz / p) && (fvco < (m + 1) * fin_hz / p)) {
378
-						rate_table->p = p;
379
-						rate_table->m = m;
380
-						rate_table->s = s;
381
-						fref = fin_hz / p;
382
-						ffrac = fvco - (m * fref);
383
-						fout = ffrac * 65536;
384
-						rate_table->k = fout / fref;
385
-						return rate_table;
386
-					}
387
-				}
388
-			}
389
-		}
390
-		pr_err("CANNOT FIND Fout by auto,fout = %lu\n", fout_hz);
429
+		rate_table = rockchip_rk3588_pll_frac_by_auto(fin_hz, fout_hz);
430
+		if (!rate_table)
431
+			pr_err("CANNOT FIND Fout by auto,fout = %lu\n", fout_hz);
432
+		else
433
+			return rate_table;
391434 	}
392435 	return NULL;
393436 }
....@@ -1345,7 +1388,17 @@
13451388 	rate64 *= cur.m;
13461389 	do_div(rate64, cur.p);
13471390 
1348
-	if (cur.k) {
1391
+	if (cur.k & BIT(15)) {
1392
+		/* fractional mode */
1393
+		u64 frac_rate64;
1394
+
1395
+		cur.k = (~(cur.k - 1)) & RK3588_PLLCON2_K_MASK;
1396
+		frac_rate64 = prate * cur.k;
1397
+		postdiv = cur.p;
1398
+		postdiv *= 65536;
1399
+		do_div(frac_rate64, postdiv);
1400
+		rate64 -= frac_rate64;
1401
+	} else {
13491402 		/* fractional mode */
13501403 		u64 frac_rate64 = prate * cur.k;
13511404 
....@@ -1516,7 +1569,7 @@
15161569 {
15171570 	struct clk *parent = clk_get_parent(clk);
15181571 	struct rockchip_clk_pll *pll;
1519
-	static u32 frac, fbdiv;
1572
+	static u32 frac, fbdiv, s, p;
15201573 	bool negative;
15211574 	u32 pllcon, pllcon0, pllcon2, fbdiv_mask, frac_mask, frac_shift;
15221575 	u64 fracdiv, m, n;
....@@ -1567,11 +1620,6 @@
15671620 	negative = !!(ppm & BIT(31));
15681621 	ppm = negative ? ~ppm + 1 : ppm;
15691622 
1570
-	if (!frac) {
1571
-		frac = readl_relaxed(pll->reg_base + pllcon2) & frac_mask;
1572
-		fbdiv = readl_relaxed(pll->reg_base + pllcon0) & fbdiv_mask;
1573
-	}
1574
-
15751623 	switch (pll->type) {
15761624 	case pll_rk3036:
15771625 	case pll_rk3328:
....@@ -1583,6 +1631,10 @@
15831631 		 *    1 << 24                 1 << 24       1000000
15841632 		 *
15851633 		 */
1634
+		if (!frac) {
1635
+			frac = readl_relaxed(pll->reg_base + pllcon2) & frac_mask;
1636
+			fbdiv = readl_relaxed(pll->reg_base + pllcon0) & fbdiv_mask;
1637
+		}
15861638 		m = div64_u64((uint64_t)frac * ppm, 1000000);
15871639 		n = div64_u64((uint64_t)ppm << 24, 1000000) * fbdiv;
15881640 
....@@ -1597,13 +1649,65 @@
15971649 		writel_relaxed(pllcon, pll->reg_base + pllcon2);
15981650 		break;
15991651 	case pll_rk3588:
1600
-		m = div64_u64((uint64_t)frac * ppm, 100000);
1601
-		n = div64_u64((uint64_t)ppm * 65535 * fbdiv, 100000);
1652
+		if (!fbdiv) {
1653
+			frac = readl_relaxed(pll->reg_base + pllcon2) & frac_mask;
1654
+			fbdiv = readl_relaxed(pll->reg_base + pllcon0) & fbdiv_mask;
1655
+		}
1656
+		if (!frac) {
1657
+			pllcon = readl_relaxed(pll->reg_base + RK3588_PLLCON(1));
1658
+			s = ((pllcon >> RK3588_PLLCON1_S_SHIFT)
1659
+				& RK3588_PLLCON1_S_MASK);
1660
+			p = ((pllcon >> RK3588_PLLCON1_P_SHIFT)
1661
+				& RK3588_PLLCON1_P_MASK);
1662
+			m = div64_u64((uint64_t)clk_get_rate(clk) * ppm, 24000000);
1663
+			n = div64_u64((uint64_t)m * 65536 * p * (1 << s), 1000000);
16021664 
1603
-		fracdiv = negative ? frac - (div64_u64(m + n, 10)) : frac + (div64_u64(m + n, 10));
1604
-
1605
-		if (!frac || fracdiv > frac_mask)
1606
-			return -EINVAL;
1665
+			if (n > 32767)
1666
+				return -EINVAL;
1667
+			fracdiv = negative ? ~n + 1 : n;
1668
+		} else if (frac & BIT(15)) {
1669
+			frac = (~(frac - 1)) & RK3588_PLLCON2_K_MASK;
1670
+			m = div64_u64((uint64_t)frac * ppm, 100000);
1671
+			n = div64_u64((uint64_t)ppm * 65536 * fbdiv, 100000);
1672
+			if (negative) {
1673
+				fracdiv = frac + (div64_u64(m + n, 10));
1674
+				if (fracdiv > 32767)
1675
+					return -EINVAL;
1676
+				fracdiv = ~fracdiv + 1;
1677
+			} else {
1678
+				s = div64_u64(m + n, 10);
1679
+				if (frac >= s) {
1680
+					fracdiv = frac - s;
1681
+					if (fracdiv > 32767)
1682
+						return -EINVAL;
1683
+					fracdiv = ~fracdiv + 1;
1684
+				} else {
1685
+					fracdiv = s - frac;
1686
+					if (fracdiv > 32767)
1687
+						return -EINVAL;
1688
+				}
1689
+			}
1690
+		} else {
1691
+			m = div64_u64((uint64_t)frac * ppm, 100000);
1692
+			n = div64_u64((uint64_t)ppm * 65536 * fbdiv, 100000);
1693
+			if (!negative) {
1694
+				fracdiv = frac + (div64_u64(m + n, 10));
1695
+				if (fracdiv > 32767)
1696
+					return -EINVAL;
1697
+			} else {
1698
+				s = div64_u64(m + n, 10);
1699
+				if (frac >= s) {
1700
+					fracdiv = frac - s;
1701
+					if (fracdiv > 32767)
1702
+						return -EINVAL;
1703
+				} else {
1704
+					fracdiv = s - frac;
1705
+					if (fracdiv > 32767)
1706
+						return -EINVAL;
1707
+					fracdiv = ~fracdiv + 1;
1708
+				}
1709
+			}
1710
+		}
16071711 
16081712 		writel_relaxed(HIWORD_UPDATE(fracdiv, frac_mask, frac_shift),
16091713 			       pll->reg_base + pllcon2);