--- a/drivers/vlynq/vlynq.c
+++ b/drivers/vlynq/vlynq.c
-@@ -514,9 +514,14 @@ static int __vlynq_enable_device(struct
- !__vlynq_try_external(dev))
- return 0;
+@@ -119,20 +119,40 @@ static int vlynq_linked(struct vlynq_dev
+ return 0;
+ }
+
++static volatile int vlynq_delay_value_new = 0;
++
++static void vlynq_delay_wait(u32 count)
++{
++ /* Code adopted from original vlynq driver */
++ int i = 0;
++ volatile int *ptr = &vlynq_delay_value_new;
++ *ptr = 0;
++
++ /* We are assuming that the each cycle takes about
++ * 23 assembly instructions. */
++ for(i = 0; i < (count + 23)/23; i++)
++ *ptr = *ptr + 1;
++}
++
+ static void vlynq_reset(struct vlynq_device *dev)
+ {
++ u32 rtm = readl(&dev->local->revision);
++
++ rtm = rtm < 0x00010205 || readl(&dev->local->status) & 0x800 == 0 ?
++ 0 : 0x600000;
++
+ writel(readl(&dev->local->control) | VLYNQ_CTRL_RESET,
+ &dev->local->control);
+
+ /* Wait for the devices to finish resetting */
+- msleep(5);
++ vlynq_delay_wait(0xffffff);
+
+ /* Remove reset bit */
+- writel(readl(&dev->local->control) & ~VLYNQ_CTRL_RESET,
++ writel(readl(&dev->local->control) & ~VLYNQ_CTRL_RESET | rtm,
+ &dev->local->control);
+
+ /* Give some time for the devices to settle */
+- msleep(5);
++ vlynq_delay_wait(0xffffff);
+ }
+
+ static void vlynq_irq_unmask(struct irq_data *d)
+@@ -379,6 +399,61 @@ void vlynq_unregister_driver(struct vlyn
+ }
+ EXPORT_SYMBOL(vlynq_unregister_driver);
+
++enum vlynq_clk_src {
++ vlynq_clk_external,
++ vlynq_clk_local,
++ vlynq_clk_remote,
++ vlynq_clk_invalid,
++};
++
++static int __vlynq_set_clocks(struct vlynq_device *dev,
++ enum vlynq_clk_src clk_dir,
++ int lclk_div, int rclk_div)
++{
++ u32 reg;
++
++ if (clk_dir == vlynq_clk_invalid) {
++ printk(KERN_ERR "%s: attempt to set invalid clocking\n",
++ dev_name(&dev->dev));
++ return -EINVAL;
++ }
++
++ reg = readl(&dev->local->control);
++ if (readl(&dev->local->revision) < 0x00010205) {
++ if (clk_dir & vlynq_clk_local)
++ reg |= VLYNQ_CTRL_CLOCK_INT;
++ else
++ reg &= ~VLYNQ_CTRL_CLOCK_INT;
++ }
++ reg &= ~VLYNQ_CTRL_CLOCK_MASK;
++ reg |= VLYNQ_CTRL_CLOCK_DIV(lclk_div);
++ writel(reg, &dev->local->control);
++
++ if (!vlynq_linked(dev))
++ return -ENODEV;
++
++ printk(KERN_INFO "%s: local VLYNQ protocol rev. is 0x%08x\n",
++ dev_name(&dev->dev), readl(&dev->local->revision));
++ printk(KERN_INFO "%s: remote VLYNQ protocol rev. is 0x%08x\n",
++ dev_name(&dev->dev), readl(&dev->remote->revision));
++
++ reg = readl(&dev->remote->control);
++ if (readl(&dev->remote->revision) < 0x00010205) {
++ if (clk_dir & vlynq_clk_remote)
++ reg |= VLYNQ_CTRL_CLOCK_INT;
++ else
++ reg &= ~VLYNQ_CTRL_CLOCK_INT;
++ }
++ reg &= ~VLYNQ_CTRL_CLOCK_MASK;
++ reg |= VLYNQ_CTRL_CLOCK_DIV(rclk_div);
++ writel(reg, &dev->remote->control);
++
++ if (!vlynq_linked(dev))
++ return -ENODEV;
++
++ return 0;
++}
++
+ /*
+ * A VLYNQ remote device can clock the VLYNQ bus master
+ * using a dedicated clock line. In that case, both the
+@@ -392,29 +467,16 @@ static int __vlynq_try_remote(struct vly
+ int i;
+
+ vlynq_reset(dev);
+- for (i = dev->dev_id ? vlynq_rdiv2 : vlynq_rdiv8; dev->dev_id ?
+- i <= vlynq_rdiv8 : i >= vlynq_rdiv2;
+- dev->dev_id ? i++ : i--) {
++ for (i = 0; i <= 7; i++) {
+
+ if (!vlynq_linked(dev))
+ break;
+
+- writel((readl(&dev->remote->control) &
+- ~VLYNQ_CTRL_CLOCK_MASK) |
+- VLYNQ_CTRL_CLOCK_INT |
+- VLYNQ_CTRL_CLOCK_DIV(i - vlynq_rdiv1),
+- &dev->remote->control);
+- writel((readl(&dev->local->control)
+- & ~(VLYNQ_CTRL_CLOCK_INT |
+- VLYNQ_CTRL_CLOCK_MASK)) |
+- VLYNQ_CTRL_CLOCK_DIV(i - vlynq_rdiv1),
+- &dev->local->control);
+-
+- if (vlynq_linked(dev)) {
+- printk(KERN_DEBUG
+- "%s: using remote clock divisor %d\n",
+- dev_name(&dev->dev), i - vlynq_rdiv1 + 1);
+- dev->divisor = i;
++ if (!__vlynq_set_clocks(dev, vlynq_clk_remote, i, i)) {
++ printk(KERN_INFO
++ "%s: using remote clock divisor %d\n",
++ dev_name(&dev->dev), i + 1);
++ dev->divisor = i + vlynq_rdiv1;
+ return 0;
} else {
+ vlynq_reset(dev);
+@@ -433,25 +495,17 @@ static int __vlynq_try_remote(struct vly
+ */
+ static int __vlynq_try_local(struct vlynq_device *dev)
+ {
+- int i;
++ int i, dir = !dev->dev_id;
+
+ vlynq_reset(dev);
+
+- for (i = dev->dev_id ? vlynq_ldiv2 : vlynq_ldiv8; dev->dev_id ?
+- i <= vlynq_ldiv8 : i >= vlynq_ldiv2;
+- dev->dev_id ? i++ : i--) {
+-
+- writel((readl(&dev->local->control) &
+- ~VLYNQ_CTRL_CLOCK_MASK) |
+- VLYNQ_CTRL_CLOCK_INT |
+- VLYNQ_CTRL_CLOCK_DIV(i - vlynq_ldiv1),
+- &dev->local->control);
+-
+- if (vlynq_linked(dev)) {
+- printk(KERN_DEBUG
+- "%s: using local clock divisor %d\n",
+- dev_name(&dev->dev), i - vlynq_ldiv1 + 1);
+- dev->divisor = i;
++ for (i = dir ? 7 : 0; dir ? i >= 0 : i <= 7; dir ? i-- : i++) {
++
++ if (!__vlynq_set_clocks(dev, vlynq_clk_local, i, 0)) {
++ printk(KERN_INFO
++ "%s: using local clock divisor %d\n",
++ dev_name(&dev->dev), i + 1);
++ dev->divisor = i + vlynq_ldiv1;
+ return 0;
+ } else {
+ vlynq_reset(dev);
+@@ -473,18 +527,10 @@ static int __vlynq_try_external(struct v
+ if (!vlynq_linked(dev))
+ return -ENODEV;
+
+- writel((readl(&dev->remote->control) &
+- ~VLYNQ_CTRL_CLOCK_INT),
+- &dev->remote->control);
+-
+- writel((readl(&dev->local->control) &
+- ~VLYNQ_CTRL_CLOCK_INT),
+- &dev->local->control);
+-
+- if (vlynq_linked(dev)) {
+- printk(KERN_DEBUG "%s: using external clock\n",
+- dev_name(&dev->dev));
+- dev->divisor = vlynq_div_external;
++ if (!__vlynq_set_clocks(dev, vlynq_clk_external, 0, 0)) {
++ printk(KERN_INFO "%s: using external clock\n",
++ dev_name(&dev->dev));
++ dev->divisor = vlynq_div_external;
+ return 0;
+ }
+
+@@ -501,24 +547,16 @@ static int __vlynq_enable_device(struct
+ return result;
+
+ switch (dev->divisor) {
+- case vlynq_div_external:
+ case vlynq_div_auto:
+ /* When the device is brought from reset it should have clock
+ * generation negotiated by hardware.
+ * Check which device is generating clocks and perform setup
+ * accordingly */
+- if (vlynq_linked(dev) && readl(&dev->remote->control) &
+- VLYNQ_CTRL_CLOCK_INT) {
+- if (!__vlynq_try_remote(dev) ||
+- !__vlynq_try_local(dev) ||
+- !__vlynq_try_external(dev))
+- return 0;
+- } else {
- if (!__vlynq_try_external(dev) ||
- !__vlynq_try_local(dev) ||
- !__vlynq_try_remote(dev))
-+ /* XXX: I don't really know what difference it makes, if the order
-+ * of the following calls is changed, but at least in this order
-+ * my fritzbox doesn't hang at startup as in
-+ * https://dev.openwrt.org/ticket/7324
-+ */
-+ if (!__vlynq_try_remote(dev) ||
-+ !__vlynq_try_local(dev) ||
-+ !__vlynq_try_external(dev))
- return 0;
+- return 0;
+- }
++ if (!__vlynq_try_remote(dev) || !__vlynq_try_local(dev))
++ return 0;
++ case vlynq_div_external:
++ if (!__vlynq_try_external(dev))
++ return 0;
+ break;
+ case vlynq_ldiv1:
+ case vlynq_ldiv2:
+@@ -528,15 +566,12 @@ static int __vlynq_enable_device(struct
+ case vlynq_ldiv6:
+ case vlynq_ldiv7:
+ case vlynq_ldiv8:
+- writel(VLYNQ_CTRL_CLOCK_INT |
+- VLYNQ_CTRL_CLOCK_DIV(dev->divisor -
+- vlynq_ldiv1), &dev->local->control);
+- writel(0, &dev->remote->control);
+- if (vlynq_linked(dev)) {
+- printk(KERN_DEBUG
+- "%s: using local clock divisor %d\n",
+- dev_name(&dev->dev),
+- dev->divisor - vlynq_ldiv1 + 1);
++ if (!__vlynq_set_clocks(dev, vlynq_clk_local, dev->divisor -
++ vlynq_ldiv1, 0)) {
++ printk(KERN_INFO
++ "%s: using local clock divisor %d\n",
++ dev_name(&dev->dev),
++ dev->divisor - vlynq_ldiv1 + 1);
+ return 0;
+ }
+ break;
+@@ -548,20 +583,17 @@ static int __vlynq_enable_device(struct
+ case vlynq_rdiv6:
+ case vlynq_rdiv7:
+ case vlynq_rdiv8:
+- writel(0, &dev->local->control);
+- writel(VLYNQ_CTRL_CLOCK_INT |
+- VLYNQ_CTRL_CLOCK_DIV(dev->divisor -
+- vlynq_rdiv1), &dev->remote->control);
+- if (vlynq_linked(dev)) {
+- printk(KERN_DEBUG
+- "%s: using remote clock divisor %d\n",
+- dev_name(&dev->dev),
+- dev->divisor - vlynq_rdiv1 + 1);
++ if (!__vlynq_set_clocks(dev, vlynq_clk_remote, 0,
++ dev->divisor - vlynq_rdiv1)) {
++ printk(KERN_INFO
++ "%s: using remote clock divisor %d\n",
++ dev_name(&dev->dev),
++ dev->divisor - vlynq_rdiv1 + 1);
+ return 0;
}
break;
+ }
+-
++ vlynq_reset(dev);
+ ops->off(dev);
+ return -ENODEV;
+ }
+@@ -732,14 +764,14 @@ static int vlynq_probe(struct platform_d
+ platform_set_drvdata(pdev, dev);
+
+ printk(KERN_INFO "%s: regs 0x%p, irq %d, mem 0x%p\n",
+- dev_name(&dev->dev), (void *)dev->regs_start, dev->irq,
+- (void *)dev->mem_start);
++ dev_name(&dev->dev), (void *)dev->regs_start,
++ dev->irq, (void *)dev->mem_start);
+
+ dev->dev_id = 0;
+ dev->divisor = vlynq_div_auto;
+- result = __vlynq_enable_device(dev);
+- if (result == 0) {
++ if (!__vlynq_enable_device(dev)) {
+ dev->dev_id = readl(&dev->remote->chip);
++ vlynq_reset(dev);
+ ((struct plat_vlynq_ops *)(dev->dev.platform_data))->off(dev);
+ }
+ if (dev->dev_id)
projects / openwrt / openwrt.git / commitdiff