1 --- a/drivers/vlynq/vlynq.c
2 +++ b/drivers/vlynq/vlynq.c
3 @@ -119,20 +119,40 @@ static int vlynq_linked(struct vlynq_dev
7 +static volatile int vlynq_delay_value_new = 0;
9 +static void vlynq_delay_wait(u32 count)
11 + /* Code adopted from original vlynq driver */
13 + volatile int *ptr = &vlynq_delay_value_new;
16 + /* We are assuming that the each cycle takes about
17 + * 23 assembly instructions. */
18 + for(i = 0; i < (count + 23)/23; i++)
22 static void vlynq_reset(struct vlynq_device *dev)
24 + u32 rtm = readl(&dev->local->revision);
26 + rtm = rtm < 0x00010205 || readl(&dev->local->status) & 0x800 == 0 ?
29 writel(readl(&dev->local->control) | VLYNQ_CTRL_RESET,
30 &dev->local->control);
32 /* Wait for the devices to finish resetting */
34 + vlynq_delay_wait(0xffffff);
36 /* Remove reset bit */
37 - writel(readl(&dev->local->control) & ~VLYNQ_CTRL_RESET,
38 + writel(readl(&dev->local->control) & ~VLYNQ_CTRL_RESET | rtm,
39 &dev->local->control);
41 /* Give some time for the devices to settle */
43 + vlynq_delay_wait(0xffffff);
46 static void vlynq_irq_unmask(struct irq_data *d)
47 @@ -379,6 +399,61 @@ void vlynq_unregister_driver(struct vlyn
49 EXPORT_SYMBOL(vlynq_unregister_driver);
58 +static int __vlynq_set_clocks(struct vlynq_device *dev,
59 + enum vlynq_clk_src clk_dir,
60 + int lclk_div, int rclk_div)
64 + if (clk_dir == vlynq_clk_invalid) {
65 + printk(KERN_ERR "%s: attempt to set invalid clocking\n",
66 + dev_name(&dev->dev));
70 + reg = readl(&dev->local->control);
71 + if (readl(&dev->local->revision) < 0x00010205) {
72 + if (clk_dir & vlynq_clk_local)
73 + reg |= VLYNQ_CTRL_CLOCK_INT;
75 + reg &= ~VLYNQ_CTRL_CLOCK_INT;
77 + reg &= ~VLYNQ_CTRL_CLOCK_MASK;
78 + reg |= VLYNQ_CTRL_CLOCK_DIV(lclk_div);
79 + writel(reg, &dev->local->control);
81 + if (!vlynq_linked(dev))
84 + printk(KERN_INFO "%s: local VLYNQ protocol rev. is 0x%08x\n",
85 + dev_name(&dev->dev), readl(&dev->local->revision));
86 + printk(KERN_INFO "%s: remote VLYNQ protocol rev. is 0x%08x\n",
87 + dev_name(&dev->dev), readl(&dev->remote->revision));
89 + reg = readl(&dev->remote->control);
90 + if (readl(&dev->remote->revision) < 0x00010205) {
91 + if (clk_dir & vlynq_clk_remote)
92 + reg |= VLYNQ_CTRL_CLOCK_INT;
94 + reg &= ~VLYNQ_CTRL_CLOCK_INT;
96 + reg &= ~VLYNQ_CTRL_CLOCK_MASK;
97 + reg |= VLYNQ_CTRL_CLOCK_DIV(rclk_div);
98 + writel(reg, &dev->remote->control);
100 + if (!vlynq_linked(dev))
107 * A VLYNQ remote device can clock the VLYNQ bus master
108 * using a dedicated clock line. In that case, both the
109 @@ -392,29 +467,16 @@ static int __vlynq_try_remote(struct vly
113 - for (i = dev->dev_id ? vlynq_rdiv2 : vlynq_rdiv8; dev->dev_id ?
114 - i <= vlynq_rdiv8 : i >= vlynq_rdiv2;
115 - dev->dev_id ? i++ : i--) {
116 + for (i = 0; i <= 7; i++) {
118 if (!vlynq_linked(dev))
121 - writel((readl(&dev->remote->control) &
122 - ~VLYNQ_CTRL_CLOCK_MASK) |
123 - VLYNQ_CTRL_CLOCK_INT |
124 - VLYNQ_CTRL_CLOCK_DIV(i - vlynq_rdiv1),
125 - &dev->remote->control);
126 - writel((readl(&dev->local->control)
127 - & ~(VLYNQ_CTRL_CLOCK_INT |
128 - VLYNQ_CTRL_CLOCK_MASK)) |
129 - VLYNQ_CTRL_CLOCK_DIV(i - vlynq_rdiv1),
130 - &dev->local->control);
132 - if (vlynq_linked(dev)) {
134 - "%s: using remote clock divisor %d\n",
135 - dev_name(&dev->dev), i - vlynq_rdiv1 + 1);
137 + if (!__vlynq_set_clocks(dev, vlynq_clk_remote, i, i)) {
139 + "%s: using remote clock divisor %d\n",
140 + dev_name(&dev->dev), i + 1);
141 + dev->divisor = i + vlynq_rdiv1;
145 @@ -433,25 +495,17 @@ static int __vlynq_try_remote(struct vly
147 static int __vlynq_try_local(struct vlynq_device *dev)
150 + int i, dir = !dev->dev_id;
154 - for (i = dev->dev_id ? vlynq_ldiv2 : vlynq_ldiv8; dev->dev_id ?
155 - i <= vlynq_ldiv8 : i >= vlynq_ldiv2;
156 - dev->dev_id ? i++ : i--) {
158 - writel((readl(&dev->local->control) &
159 - ~VLYNQ_CTRL_CLOCK_MASK) |
160 - VLYNQ_CTRL_CLOCK_INT |
161 - VLYNQ_CTRL_CLOCK_DIV(i - vlynq_ldiv1),
162 - &dev->local->control);
164 - if (vlynq_linked(dev)) {
166 - "%s: using local clock divisor %d\n",
167 - dev_name(&dev->dev), i - vlynq_ldiv1 + 1);
169 + for (i = dir ? 7 : 0; dir ? i >= 0 : i <= 7; dir ? i-- : i++) {
171 + if (!__vlynq_set_clocks(dev, vlynq_clk_local, i, 0)) {
173 + "%s: using local clock divisor %d\n",
174 + dev_name(&dev->dev), i + 1);
175 + dev->divisor = i + vlynq_ldiv1;
179 @@ -473,18 +527,10 @@ static int __vlynq_try_external(struct v
180 if (!vlynq_linked(dev))
183 - writel((readl(&dev->remote->control) &
184 - ~VLYNQ_CTRL_CLOCK_INT),
185 - &dev->remote->control);
187 - writel((readl(&dev->local->control) &
188 - ~VLYNQ_CTRL_CLOCK_INT),
189 - &dev->local->control);
191 - if (vlynq_linked(dev)) {
192 - printk(KERN_DEBUG "%s: using external clock\n",
193 - dev_name(&dev->dev));
194 - dev->divisor = vlynq_div_external;
195 + if (!__vlynq_set_clocks(dev, vlynq_clk_external, 0, 0)) {
196 + printk(KERN_INFO "%s: using external clock\n",
197 + dev_name(&dev->dev));
198 + dev->divisor = vlynq_div_external;
202 @@ -501,24 +547,16 @@ static int __vlynq_enable_device(struct
205 switch (dev->divisor) {
206 - case vlynq_div_external:
208 /* When the device is brought from reset it should have clock
209 * generation negotiated by hardware.
210 * Check which device is generating clocks and perform setup
212 - if (vlynq_linked(dev) && readl(&dev->remote->control) &
213 - VLYNQ_CTRL_CLOCK_INT) {
214 - if (!__vlynq_try_remote(dev) ||
215 - !__vlynq_try_local(dev) ||
216 - !__vlynq_try_external(dev))
219 - if (!__vlynq_try_external(dev) ||
220 - !__vlynq_try_local(dev) ||
221 - !__vlynq_try_remote(dev))
224 + if (!__vlynq_try_remote(dev) || !__vlynq_try_local(dev))
226 + case vlynq_div_external:
227 + if (!__vlynq_try_external(dev))
232 @@ -528,15 +566,12 @@ static int __vlynq_enable_device(struct
236 - writel(VLYNQ_CTRL_CLOCK_INT |
237 - VLYNQ_CTRL_CLOCK_DIV(dev->divisor -
238 - vlynq_ldiv1), &dev->local->control);
239 - writel(0, &dev->remote->control);
240 - if (vlynq_linked(dev)) {
242 - "%s: using local clock divisor %d\n",
243 - dev_name(&dev->dev),
244 - dev->divisor - vlynq_ldiv1 + 1);
245 + if (!__vlynq_set_clocks(dev, vlynq_clk_local, dev->divisor -
248 + "%s: using local clock divisor %d\n",
249 + dev_name(&dev->dev),
250 + dev->divisor - vlynq_ldiv1 + 1);
254 @@ -548,20 +583,17 @@ static int __vlynq_enable_device(struct
258 - writel(0, &dev->local->control);
259 - writel(VLYNQ_CTRL_CLOCK_INT |
260 - VLYNQ_CTRL_CLOCK_DIV(dev->divisor -
261 - vlynq_rdiv1), &dev->remote->control);
262 - if (vlynq_linked(dev)) {
264 - "%s: using remote clock divisor %d\n",
265 - dev_name(&dev->dev),
266 - dev->divisor - vlynq_rdiv1 + 1);
267 + if (!__vlynq_set_clocks(dev, vlynq_clk_remote, 0,
268 + dev->divisor - vlynq_rdiv1)) {
270 + "%s: using remote clock divisor %d\n",
271 + dev_name(&dev->dev),
272 + dev->divisor - vlynq_rdiv1 + 1);
282 @@ -732,14 +764,14 @@ static int vlynq_probe(struct platform_d
283 platform_set_drvdata(pdev, dev);
285 printk(KERN_INFO "%s: regs 0x%p, irq %d, mem 0x%p\n",
286 - dev_name(&dev->dev), (void *)dev->regs_start, dev->irq,
287 - (void *)dev->mem_start);
288 + dev_name(&dev->dev), (void *)dev->regs_start,
289 + dev->irq, (void *)dev->mem_start);
292 dev->divisor = vlynq_div_auto;
293 - result = __vlynq_enable_device(dev);
295 + if (!__vlynq_enable_device(dev)) {
296 dev->dev_id = readl(&dev->remote->chip);
298 ((struct plat_vlynq_ops *)(dev->dev.platform_data))->off(dev);