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

waterHeterogeneousDataSynchronization/externalConnectionMonitoring/commandProcessing.c

@@ -254,3 +254,137 @@ static long pll1_set_rate(struct clk *clk, unsigned long rate, int apply)
 	if (apply) {
 		if (actual_rate != rate)
 			return -EINVAL;
+		if (clk->users > 0)
+			return -EBUSY;
+		pr_debug(KERN_INFO "clk %s: new rate %lu (actual rate %lu)\n",
+				clk->name, rate, actual_rate);
+		pm_writel(PLL1, ctrl);
+	}
+
+	return actual_rate;
+}
+
+static int pll1_set_parent(struct clk *clk, struct clk *parent)
+{
+	u32 ctrl;
+
+	if (clk->users > 0)
+		return -EBUSY;
+
+	ctrl = pm_readl(PLL1);
+	WARN_ON(ctrl & PM_BIT(PLLEN));
+
+	if (parent == &osc0)
+		ctrl &= ~PM_BIT(PLLOSC);
+	else if (parent == &osc1)
+		ctrl |= PM_BIT(PLLOSC);
+	else
+		return -EINVAL;
+
+	pm_writel(PLL1, ctrl);
+	clk->parent = parent;
+
+	return 0;
+}
+
+/*
+ * The AT32AP7000 has five primary clock sources: One 32kHz
+ * oscillator, two crystal oscillators and two PLLs.
+ */
+static struct clk osc32k = {
+	.name		= "osc32k",
+	.get_rate	= osc_get_rate,
+	.users		= 1,
+	.index		= 0,
+};
+static struct clk osc0 = {
+	.name		= "osc0",
+	.get_rate	= osc_get_rate,
+	.users		= 1,
+	.index		= 1,
+};
+static struct clk osc1 = {
+	.name		= "osc1",
+	.get_rate	= osc_get_rate,
+	.index		= 2,
+};
+static struct clk pll0 = {
+	.name		= "pll0",
+	.get_rate	= pll0_get_rate,
+	.parent		= &osc0,
+};
+static struct clk pll1 = {
+	.name		= "pll1",
+	.mode		= pll1_mode,
+	.get_rate	= pll1_get_rate,
+	.set_rate	= pll1_set_rate,
+	.set_parent	= pll1_set_parent,
+	.parent		= &osc0,
+};
+
+/*
+ * The main clock can be either osc0 or pll0.  The boot loader may
+ * have chosen one for us, so we don't really know which one until we
+ * have a look at the SM.
+ */
+static struct clk *main_clock;
+
+/*
+ * Synchronous clocks are generated from the main clock. The clocks
+ * must satisfy the constraint
+ *   fCPU >= fHSB >= fPB
+ * i.e. each clock must not be faster than its parent.
+ */
+static unsigned long bus_clk_get_rate(struct clk *clk, unsigned int shift)
+{
+	return main_clock->get_rate(main_clock) >> shift;
+};
+
+static void cpu_clk_mode(struct clk *clk, int enabled)
+{
+	unsigned long flags;
+	u32 mask;
+
+	spin_lock_irqsave(&pm_lock, flags);
+	mask = pm_readl(CPU_MASK);
+	if (enabled)
+		mask |= 1 << clk->index;
+	else
+		mask &= ~(1 << clk->index);
+	pm_writel(CPU_MASK, mask);
+	spin_unlock_irqrestore(&pm_lock, flags);
+}
+
+static unsigned long cpu_clk_get_rate(struct clk *clk)
+{
+	unsigned long cksel, shift = 0;
+
+	cksel = pm_readl(CKSEL);
+	if (cksel & PM_BIT(CPUDIV))
+		shift = PM_BFEXT(CPUSEL, cksel) + 1;
+
+	return bus_clk_get_rate(clk, shift);
+}
+
+static long cpu_clk_set_rate(struct clk *clk, unsigned long rate, int apply)
+{
+	u32 control;
+	unsigned long parent_rate, child_div, actual_rate, div;
+
+	parent_rate = clk->parent->get_rate(clk->parent);
+	control = pm_readl(CKSEL);
+
+	if (control & PM_BIT(HSBDIV))
+		child_div = 1 << (PM_BFEXT(HSBSEL, control) + 1);
+	else
+		child_div = 1;
+
+	if (rate > 3 * (parent_rate / 4) || child_div == 1) {
+		actual_rate = parent_rate;
+		control &= ~PM_BIT(CPUDIV);
+	} else {
+		unsigned int cpusel;
+		div = (parent_rate + rate / 2) / rate;
+		if (div > child_div)
+			div = child_div;
+		cpusel = (div > 1) ? (fls(div) - 2) : 0;