1 /*
2  * Copyright (C) 2016 Atmel Corporation
3  *               Wenyou.Yang <wenyou.yang@atmel.com>
4  *
5  * SPDX-License-Identifier:	GPL-2.0+
6  */
7 
8 #include <common.h>
9 #include <clk-uclass.h>
10 #include <dm/device.h>
11 #include <linux/io.h>
12 #include <mach/at91_pmc.h>
13 #include "pmc.h"
14 
15 DECLARE_GLOBAL_DATA_PTR;
16 
17 #define GENERATED_SOURCE_MAX	6
18 #define GENERATED_MAX_DIV	255
19 
20 struct generated_clk_priv {
21 	u32 num_parents;
22 };
23 
24 static ulong generated_clk_get_rate(struct clk *clk)
25 {
26 	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
27 	struct at91_pmc *pmc = plat->reg_base;
28 	struct clk parent;
29 	u32 tmp, gckdiv;
30 	u8 parent_id;
31 	int ret;
32 
33 	writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
34 	tmp = readl(&pmc->pcr);
35 	parent_id = (tmp >> AT91_PMC_PCR_GCKCSS_OFFSET) &
36 		    AT91_PMC_PCR_GCKCSS_MASK;
37 	gckdiv = (tmp >> AT91_PMC_PCR_GCKDIV_OFFSET) & AT91_PMC_PCR_GCKDIV_MASK;
38 
39 	ret = clk_get_by_index(clk->dev, parent_id, &parent);
40 	if (ret)
41 		return 0;
42 
43 	return clk_get_rate(&parent) / (gckdiv + 1);
44 }
45 
46 static ulong generated_clk_set_rate(struct clk *clk, ulong rate)
47 {
48 	struct pmc_platdata *plat = dev_get_platdata(clk->dev);
49 	struct at91_pmc *pmc = plat->reg_base;
50 	struct generated_clk_priv *priv = dev_get_priv(clk->dev);
51 	struct clk parent, best_parent;
52 	ulong tmp_rate, best_rate = rate, parent_rate;
53 	int tmp_diff, best_diff = -1;
54 	u32 div, best_div = 0;
55 	u8 best_parent_id = 0;
56 	u8 i;
57 	u32 tmp;
58 	int ret;
59 
60 	for (i = 0; i < priv->num_parents; i++) {
61 		ret = clk_get_by_index(clk->dev, i, &parent);
62 		if (ret)
63 			return ret;
64 
65 		parent_rate = clk_get_rate(&parent);
66 		if (IS_ERR_VALUE(parent_rate))
67 			return parent_rate;
68 
69 		for (div = 1; div < GENERATED_MAX_DIV + 2; div++) {
70 			tmp_rate = DIV_ROUND_CLOSEST(parent_rate, div);
71 			if (rate < tmp_rate)
72 				continue;
73 			tmp_diff = rate - tmp_rate;
74 
75 			if (best_diff < 0 || best_diff > tmp_diff) {
76 				best_rate = tmp_rate;
77 				best_diff = tmp_diff;
78 
79 				best_div = div - 1;
80 				best_parent = parent;
81 				best_parent_id = i;
82 			}
83 
84 			if (!best_diff || tmp_rate < rate)
85 				break;
86 		}
87 
88 		if (!best_diff)
89 			break;
90 	}
91 
92 	debug("GCK: best parent: %s, best_rate = %ld, best_div = %d\n",
93 	      best_parent.dev->name, best_rate, best_div);
94 
95 	ret = clk_enable(&best_parent);
96 	if (ret)
97 		return ret;
98 
99 	writel(clk->id & AT91_PMC_PCR_PID_MASK, &pmc->pcr);
100 	tmp = readl(&pmc->pcr);
101 	tmp &= ~(AT91_PMC_PCR_GCKDIV | AT91_PMC_PCR_GCKCSS);
102 	tmp |= AT91_PMC_PCR_GCKCSS_(best_parent_id) |
103 	       AT91_PMC_PCR_CMD_WRITE |
104 	       AT91_PMC_PCR_GCKDIV_(best_div) |
105 	       AT91_PMC_PCR_GCKEN;
106 	writel(tmp, &pmc->pcr);
107 
108 	while (!(readl(&pmc->sr) & AT91_PMC_GCKRDY))
109 		;
110 
111 	return 0;
112 }
113 
114 static struct clk_ops generated_clk_ops = {
115 	.get_rate = generated_clk_get_rate,
116 	.set_rate = generated_clk_set_rate,
117 };
118 
119 static int generated_clk_ofdata_to_platdata(struct udevice *dev)
120 {
121 	struct generated_clk_priv *priv = dev_get_priv(dev);
122 	u32 cells[GENERATED_SOURCE_MAX];
123 	u32 num_parents;
124 
125 	num_parents = fdtdec_get_int_array_count(gd->fdt_blob, dev->of_offset,
126 						 "clocks", cells,
127 						 GENERATED_SOURCE_MAX);
128 
129 	if (!num_parents)
130 		return -1;
131 
132 	priv->num_parents = num_parents;
133 
134 	return 0;
135 }
136 
137 static int generated_clk_bind(struct udevice *dev)
138 {
139 	return at91_pmc_clk_node_bind(dev);
140 }
141 
142 static int generated_clk_probe(struct udevice *dev)
143 {
144 	return at91_pmc_core_probe(dev);
145 }
146 
147 static const struct udevice_id generated_clk_match[] = {
148 	{ .compatible = "atmel,sama5d2-clk-generated" },
149 	{}
150 };
151 
152 U_BOOT_DRIVER(generated_clk) = {
153 	.name = "generated-clk",
154 	.id = UCLASS_CLK,
155 	.of_match = generated_clk_match,
156 	.bind = generated_clk_bind,
157 	.probe = generated_clk_probe,
158 	.ofdata_to_platdata = generated_clk_ofdata_to_platdata,
159 	.priv_auto_alloc_size = sizeof(struct generated_clk_priv),
160 	.platdata_auto_alloc_size = sizeof(struct pmc_platdata),
161 	.ops = &generated_clk_ops,
162 };
163