1 files changed, 88 insertions, 0 deletions
diff --git a/src/soc/mediatek/common/pll.c b/src/soc/mediatek/common/pll.c
new file mode 100644
index 0000000000..e087be38a2
--- /dev/null
+++ b/src/soc/mediatek/common/pll.c
@@ -0,0 +1,88 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright 2018 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <arch/io.h>
+#include <assert.h>
+#include <soc/pll.h>
+
+#define GENMASK(h, l) (BIT(h + 1) - BIT(l))
+
+void mux_set_sel(const struct mux *mux, u32 sel)
+{
+	u32 mask = GENMASK(mux->mux_width - 1, 0);
+	u32 val = read32(mux->reg);
+
+	val &= ~(mask << mux->mux_shift);
+	val |= (sel & mask) << mux->mux_shift;
+	write32(mux->reg, val);
+	if (mux->upd_reg)
+		write32(mux->upd_reg, 1 << mux->upd_shift);
+}
+
+static void pll_calc_values(const struct pll *pll, u32 *pcw, u32 *postdiv,
+			    u32 freq)
+{
+	const u32 fin_hz = CLK26M_HZ;
+	const u32 *div_rate = pll->div_rate;
+	u32 val;
+
+	assert(freq <= div_rate[0]);
+	assert(freq >= 1 * GHz / 16);
+
+	for (val = 1; div_rate[val] != 0; val++) {
+		if (freq > div_rate[val])
+			break;
+	}
+	val--;
+	*postdiv = val;
+
+	/* _pcw = freq * 2^postdiv / fin * 2^pcwbits_fractional */
+	val += pll->pcwbits - PCW_INTEGER_BITS;
+
+	*pcw = ((u64)freq << val) / fin_hz;
+}
+
+static void pll_set_rate_regs(const struct pll *pll, u32 pcw, u32 postdiv)
+{
+	u32 val;
+
+	/* set postdiv */
+	val = read32(pll->div_reg);
+	val &= ~(PLL_POSTDIV_MASK << pll->div_shift);
+	val |= postdiv << pll->div_shift;
+
+	/* set postdiv and pcw at the same time if on the same register */
+	if (pll->div_reg != pll->pcw_reg) {
+		write32(pll->div_reg, val);
+		val = read32(pll->pcw_reg);
+	}
+
+	/* set pcw */
+	val &= ~GENMASK(pll->pcw_shift + pll->pcwbits - 1, pll->pcw_shift);
+	val |= pcw << pll->pcw_shift;
+	write32(pll->pcw_reg, val);
+
+	pll_set_pcw_change(pll);
+}
+
+int pll_set_rate(const struct pll *pll, u32 rate)
+{
+	u32 pcw, postdiv;
+
+	pll_calc_values(pll, &pcw, &postdiv, rate);
+	pll_set_rate_regs(pll, pcw, postdiv);
+
+	return 0;
+}