waterHeterogeneousDataSynchronization commandProcessing.c 李欣儒 commit at 2020-09-27

waterHeterogeneousDataSynchronization/externalConnectionMonitoring/commandProcessing.c

@@ -388,3 +388,106 @@ static long cpu_clk_set_rate(struct clk *clk, unsigned long rate, int apply)
 		if (div > child_div)
 			div = child_div;
 		cpusel = (div > 1) ? (fls(div) - 2) : 0;
+		control = PM_BIT(CPUDIV) | PM_BFINS(CPUSEL, cpusel, control);
+		actual_rate = parent_rate / (1 << (cpusel + 1));
+	}
+
+	pr_debug("clk %s: new rate %lu (actual rate %lu)\n",
+			clk->name, rate, actual_rate);
+
+	if (apply)
+		pm_writel(CKSEL, control);
+
+	return actual_rate;
+}
+
+static void hsb_clk_mode(struct clk *clk, int enabled)
+{
+	unsigned long flags;
+	u32 mask;
+
+	spin_lock_irqsave(&pm_lock, flags);
+	mask = pm_readl(HSB_MASK);
+	if (enabled)
+		mask |= 1 << clk->index;
+	else
+		mask &= ~(1 << clk->index);
+	pm_writel(HSB_MASK, mask);
+	spin_unlock_irqrestore(&pm_lock, flags);
+}
+
+static unsigned long hsb_clk_get_rate(struct clk *clk)
+{
+	unsigned long cksel, shift = 0;
+
+	cksel = pm_readl(CKSEL);
+	if (cksel & PM_BIT(HSBDIV))
+		shift = PM_BFEXT(HSBSEL, cksel) + 1;
+
+	return bus_clk_get_rate(clk, shift);
+}
+
+void pba_clk_mode(struct clk *clk, int enabled)
+{
+	unsigned long flags;
+	u32 mask;
+
+	spin_lock_irqsave(&pm_lock, flags);
+	mask = pm_readl(PBA_MASK);
+	if (enabled)
+		mask |= 1 << clk->index;
+	else
+		mask &= ~(1 << clk->index);
+	pm_writel(PBA_MASK, mask);
+	spin_unlock_irqrestore(&pm_lock, flags);
+}
+
+unsigned long pba_clk_get_rate(struct clk *clk)
+{
+	unsigned long cksel, shift = 0;
+
+	cksel = pm_readl(CKSEL);
+	if (cksel & PM_BIT(PBADIV))
+		shift = PM_BFEXT(PBASEL, cksel) + 1;
+
+	return bus_clk_get_rate(clk, shift);
+}
+
+static void pbb_clk_mode(struct clk *clk, int enabled)
+{
+	unsigned long flags;
+	u32 mask;
+
+	spin_lock_irqsave(&pm_lock, flags);
+	mask = pm_readl(PBB_MASK);
+	if (enabled)
+		mask |= 1 << clk->index;
+	else
+		mask &= ~(1 << clk->index);
+	pm_writel(PBB_MASK, mask);
+	spin_unlock_irqrestore(&pm_lock, flags);
+}
+
+static unsigned long pbb_clk_get_rate(struct clk *clk)
+{
+	unsigned long cksel, shift = 0;
+
+	cksel = pm_readl(CKSEL);
+	if (cksel & PM_BIT(PBBDIV))
+		shift = PM_BFEXT(PBBSEL, cksel) + 1;
+
+	return bus_clk_get_rate(clk, shift);
+}
+
+static struct clk cpu_clk = {
+	.name		= "cpu",
+	.get_rate	= cpu_clk_get_rate,
+	.set_rate	= cpu_clk_set_rate,
+	.users		= 1,
+};
+static struct clk hsb_clk = {
+	.name		= "hsb",
+	.parent		= &cpu_clk,
+	.get_rate	= hsb_clk_get_rate,
+};
+static struct clk pba_clk = {