target/linux/ar7/patches-4.1/160-vlynq_try_remote_first.patch

   1 --- a/drivers/vlynq/vlynq.c
   2 +++ b/drivers/vlynq/vlynq.c
   3 @@ -119,20 +119,40 @@ static int vlynq_linked(struct vlynq_dev
   4         return 0;
   5  }
   6
   7 +static volatile int vlynq_delay_value_new = 0;
   8 +
   9 +static void vlynq_delay_wait(u32 count)
  10 +{
  11 +       /* Code adopted from original vlynq driver */
  12 +       int i = 0;
  13 +       volatile int *ptr = &vlynq_delay_value_new;
  14 +       *ptr = 0;
  15 +
  16 +       /* We are assuming that the each cycle takes about
  17 +        * 23 assembly instructions. */
  18 +       for(i = 0; i < (count + 23)/23; i++)
  19 +               *ptr = *ptr + 1;
  20 +}
  21 +
  22  static void vlynq_reset(struct vlynq_device *dev)
  23  {
  24 +       u32 rtm = readl(&dev->local->revision);
  25 +
  26 +       rtm = rtm < 0x00010205 || readl(&dev->local->status) & 0x800 == 0 ?
  27 +                       0 : 0x600000;
  28 +
  29         writel(readl(&dev->local->control) | VLYNQ_CTRL_RESET,
  30                         &dev->local->control);
  31
  32         /* Wait for the devices to finish resetting */
  33 -       msleep(5);
  34 +       vlynq_delay_wait(0xffffff);
  35
  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);
  40
  41         /* Give some time for the devices to settle */
  42 -       msleep(5);
  43 +       vlynq_delay_wait(0xffffff);
  44  }
  45
  46  static void vlynq_irq_unmask(struct irq_data *d)
  47 @@ -379,6 +399,61 @@ void vlynq_unregister_driver(struct vlyn
  48  }
  49  EXPORT_SYMBOL(vlynq_unregister_driver);
  50
  51 +enum vlynq_clk_src {
  52 +       vlynq_clk_external,
  53 +       vlynq_clk_local,
  54 +       vlynq_clk_remote,
  55 +       vlynq_clk_invalid,
  56 +};
  57 +
  58 +static int __vlynq_set_clocks(struct vlynq_device *dev,
  59 +                               enum vlynq_clk_src clk_dir,
  60 +                               int lclk_div, int rclk_div)
  61 +{
  62 +       u32 reg;
  63 +
  64 +       if (clk_dir == vlynq_clk_invalid) {
  65 +               printk(KERN_ERR "%s: attempt to set invalid clocking\n",
  66 +                               dev_name(&dev->dev));
  67 +               return -EINVAL;
  68 +       }
  69 +
  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;
  74 +               else
  75 +                       reg &= ~VLYNQ_CTRL_CLOCK_INT;
  76 +       }
  77 +       reg &= ~VLYNQ_CTRL_CLOCK_MASK;
  78 +       reg |= VLYNQ_CTRL_CLOCK_DIV(lclk_div);
  79 +       writel(reg, &dev->local->control);
  80 +
  81 +       if (!vlynq_linked(dev))
  82 +               return -ENODEV;
  83 +
  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));
  88 +
  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;
  93 +               else
  94 +                       reg &= ~VLYNQ_CTRL_CLOCK_INT;
  95 +       }
  96 +       reg &= ~VLYNQ_CTRL_CLOCK_MASK;
  97 +       reg |= VLYNQ_CTRL_CLOCK_DIV(rclk_div);
  98 +       writel(reg, &dev->remote->control);
  99 +
 100 +       if (!vlynq_linked(dev))
 101 +               return -ENODEV;
 102 +
 103 +       return 0;
 104 +}
 105 +
 106  /*
 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
 110         int i;
 111
 112         vlynq_reset(dev);
 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++) {
 117
 118                 if (!vlynq_linked(dev))
 119                         break;
 120
 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);
 131 -
 132 -               if (vlynq_linked(dev)) {
 133 -                       printk(KERN_DEBUG
 134 -                               "%s: using remote clock divisor %d\n",
 135 -                               dev_name(&dev->dev), i - vlynq_rdiv1 + 1);
 136 -                       dev->divisor = i;
 137 +               if (!__vlynq_set_clocks(dev, vlynq_clk_remote, i, i)) {
 138 +                       printk(KERN_INFO
 139 +                                       "%s: using remote clock divisor %d\n",
 140 +                                       dev_name(&dev->dev), i + 1);
 141 +                       dev->divisor = i + vlynq_rdiv1;
 142                         return 0;
 143                 } else {
 144                         vlynq_reset(dev);
 145 @@ -433,25 +495,17 @@ static int __vlynq_try_remote(struct vly
 146   */
 147  static int __vlynq_try_local(struct vlynq_device *dev)
 148  {
 149 -       int i;
 150 +       int i, dir = !dev->dev_id;
 151
 152         vlynq_reset(dev);
 153
 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--) {
 157 -
 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);
 163 -
 164 -               if (vlynq_linked(dev)) {
 165 -                       printk(KERN_DEBUG
 166 -                               "%s: using local clock divisor %d\n",
 167 -                               dev_name(&dev->dev), i - vlynq_ldiv1 + 1);
 168 -                       dev->divisor = i;
 169 +       for (i = dir ? 7 : 0; dir ? i >= 0 : i <= 7; dir ? i-- : i++) {
 170 +
 171 +               if (!__vlynq_set_clocks(dev, vlynq_clk_local, i, 0)) {
 172 +                       printk(KERN_INFO
 173 +                                       "%s: using local clock divisor %d\n",
 174 +                                       dev_name(&dev->dev), i + 1);
 175 +                       dev->divisor = i + vlynq_ldiv1;
 176                         return 0;
 177                 } else {
 178                         vlynq_reset(dev);
 179 @@ -473,18 +527,10 @@ static int __vlynq_try_external(struct v
 180         if (!vlynq_linked(dev))
 181                 return -ENODEV;
 182
 183 -       writel((readl(&dev->remote->control) &
 184 -                       ~VLYNQ_CTRL_CLOCK_INT),
 185 -                       &dev->remote->control);
 186 -
 187 -       writel((readl(&dev->local->control) &
 188 -                       ~VLYNQ_CTRL_CLOCK_INT),
 189 -                       &dev->local->control);
 190 -
 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;
 199                 return 0;
 200         }
 201
 202 @@ -501,24 +547,16 @@ static int __vlynq_enable_device(struct
 203                 return result;
 204
 205         switch (dev->divisor) {
 206 -       case vlynq_div_external:
 207         case vlynq_div_auto:
 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
 211                  * accordingly */
 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))
 217 -                               return 0;
 218 -               } else {
 219 -                       if (!__vlynq_try_external(dev) ||
 220 -                               !__vlynq_try_local(dev)    ||
 221 -                               !__vlynq_try_remote(dev))
 222 -                               return 0;
 223 -               }
 224 +               if (!__vlynq_try_remote(dev) || !__vlynq_try_local(dev))
 225 +                       return 0;
 226 +       case vlynq_div_external:
 227 +               if (!__vlynq_try_external(dev))
 228 +                       return 0;
 229                 break;
 230         case vlynq_ldiv1:
 231         case vlynq_ldiv2:
 232 @@ -528,15 +566,12 @@ static int __vlynq_enable_device(struct
 233         case vlynq_ldiv6:
 234         case vlynq_ldiv7:
 235         case vlynq_ldiv8:
 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)) {
 241 -                       printk(KERN_DEBUG
 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 -
 246 +                               vlynq_ldiv1, 0)) {
 247 +                       printk(KERN_INFO
 248 +                                       "%s: using local clock divisor %d\n",
 249 +                                       dev_name(&dev->dev),
 250 +                                       dev->divisor - vlynq_ldiv1 + 1);
 251                         return 0;
 252                 }
 253                 break;
 254 @@ -548,20 +583,17 @@ static int __vlynq_enable_device(struct
 255         case vlynq_rdiv6:
 256         case vlynq_rdiv7:
 257         case vlynq_rdiv8:
 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)) {
 263 -                       printk(KERN_DEBUG
 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)) {
 269 +                       printk(KERN_INFO
 270 +                                       "%s: using remote clock divisor %d\n",
 271 +                                       dev_name(&dev->dev),
 272 +                                       dev->divisor - vlynq_rdiv1 + 1);
 273                         return 0;
 274                 }
 275                 break;
 276         }
 277 -
 278 +       vlynq_reset(dev);
 279         ops->off(dev);
 280         return -ENODEV;
 281  }
 282 @@ -732,14 +764,14 @@ static int vlynq_probe(struct platform_d
 283         platform_set_drvdata(pdev, dev);
 284
 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);
 290
 291         dev->dev_id = 0;
 292         dev->divisor = vlynq_div_auto;
 293 -       result = __vlynq_enable_device(dev);
 294 -       if (result == 0) {
 295 +       if (!__vlynq_enable_device(dev)) {
 296                 dev->dev_id = readl(&dev->remote->chip);
 297 +               vlynq_reset(dev);
 298                 ((struct plat_vlynq_ops *)(dev->dev.platform_data))->off(dev);
 299         }
 300         if (dev->dev_id)