drivers/net/dsa/mv88e6xxx.c

   1 /*
   2  * net/dsa/mv88e6xxx.c - Marvell 88e6xxx switch chip support
   3  * Copyright (c) 2008 Marvell Semiconductor
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  */
  10
  11 #include <linux/delay.h>
  12 #include <linux/etherdevice.h>
  13 #include <linux/if_bridge.h>
  14 #include <linux/jiffies.h>
  15 #include <linux/list.h>
  16 #include <linux/module.h>
  17 #include <linux/netdevice.h>
  18 #include <linux/phy.h>
  19 #include <net/dsa.h>
  20 #include "mv88e6xxx.h"
  21
  22 /* If the switch's ADDR[4:0] strap pins are strapped to zero, it will
  23  * use all 32 SMI bus addresses on its SMI bus, and all switch registers
  24  * will be directly accessible on some {device address,register address}
  25  * pair.  If the ADDR[4:0] pins are not strapped to zero, the switch
  26  * will only respond to SMI transactions to that specific address, and
  27  * an indirect addressing mechanism needs to be used to access its
  28  * registers.
  29  */
  30 static int mv88e6xxx_reg_wait_ready(struct mii_bus *bus, int sw_addr)
  31 {
  32         int ret;
  33         int i;
  34
  35         for (i = 0; i < 16; i++) {
  36                 ret = mdiobus_read(bus, sw_addr, 0);
  37                 if (ret < 0)
  38                         return ret;
  39
  40                 if ((ret & 0x8000) == 0)
  41                         return 0;
  42         }
  43
  44         return -ETIMEDOUT;
  45 }
  46
  47 int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
  48 {
  49         int ret;
  50
  51         if (sw_addr == 0)
  52                 return mdiobus_read(bus, addr, reg);
  53
  54         /* Wait for the bus to become free. */
  55         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
  56         if (ret < 0)
  57                 return ret;
  58
  59         /* Transmit the read command. */
  60         ret = mdiobus_write(bus, sw_addr, 0, 0x9800 | (addr << 5) | reg);
  61         if (ret < 0)
  62                 return ret;
  63
  64         /* Wait for the read command to complete. */
  65         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
  66         if (ret < 0)
  67                 return ret;
  68
  69         /* Read the data. */
  70         ret = mdiobus_read(bus, sw_addr, 1);
  71         if (ret < 0)
  72                 return ret;
  73
  74         return ret & 0xffff;
  75 }
  76
  77 /* Must be called with SMI mutex held */
  78 static int _mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
  79 {
  80         struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
  81         int ret;
  82
  83         if (bus == NULL)
  84                 return -EINVAL;
  85
  86         ret = __mv88e6xxx_reg_read(bus, ds->pd->sw_addr, addr, reg);
  87         if (ret < 0)
  88                 return ret;
  89
  90         dev_dbg(ds->master_dev, "<- addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
  91                 addr, reg, ret);
  92
  93         return ret;
  94 }
  95
  96 int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
  97 {
  98         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
  99         int ret;
 100
 101         mutex_lock(&ps->smi_mutex);
 102         ret = _mv88e6xxx_reg_read(ds, addr, reg);
 103         mutex_unlock(&ps->smi_mutex);
 104
 105         return ret;
 106 }
 107
 108 int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 109                           int reg, u16 val)
 110 {
 111         int ret;
 112
 113         if (sw_addr == 0)
 114                 return mdiobus_write(bus, addr, reg, val);
 115
 116         /* Wait for the bus to become free. */
 117         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 118         if (ret < 0)
 119                 return ret;
 120
 121         /* Transmit the data to write. */
 122         ret = mdiobus_write(bus, sw_addr, 1, val);
 123         if (ret < 0)
 124                 return ret;
 125
 126         /* Transmit the write command. */
 127         ret = mdiobus_write(bus, sw_addr, 0, 0x9400 | (addr << 5) | reg);
 128         if (ret < 0)
 129                 return ret;
 130
 131         /* Wait for the write command to complete. */
 132         ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
 133         if (ret < 0)
 134                 return ret;
 135
 136         return 0;
 137 }
 138
 139 /* Must be called with SMI mutex held */
 140 static int _mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg,
 141                                 u16 val)
 142 {
 143         struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 144
 145         if (bus == NULL)
 146                 return -EINVAL;
 147
 148         dev_dbg(ds->master_dev, "-> addr: 0x%.2x reg: 0x%.2x val: 0x%.4x\n",
 149                 addr, reg, val);
 150
 151         return __mv88e6xxx_reg_write(bus, ds->pd->sw_addr, addr, reg, val);
 152 }
 153
 154 int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 155 {
 156         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 157         int ret;
 158
 159         mutex_lock(&ps->smi_mutex);
 160         ret = _mv88e6xxx_reg_write(ds, addr, reg, val);
 161         mutex_unlock(&ps->smi_mutex);
 162
 163         return ret;
 164 }
 165
 166 int mv88e6xxx_config_prio(struct dsa_switch *ds)
 167 {
 168         /* Configure the IP ToS mapping registers. */
 169         REG_WRITE(REG_GLOBAL, 0x10, 0x0000);
 170         REG_WRITE(REG_GLOBAL, 0x11, 0x0000);
 171         REG_WRITE(REG_GLOBAL, 0x12, 0x5555);
 172         REG_WRITE(REG_GLOBAL, 0x13, 0x5555);
 173         REG_WRITE(REG_GLOBAL, 0x14, 0xaaaa);
 174         REG_WRITE(REG_GLOBAL, 0x15, 0xaaaa);
 175         REG_WRITE(REG_GLOBAL, 0x16, 0xffff);
 176         REG_WRITE(REG_GLOBAL, 0x17, 0xffff);
 177
 178         /* Configure the IEEE 802.1p priority mapping register. */
 179         REG_WRITE(REG_GLOBAL, 0x18, 0xfa41);
 180
 181         return 0;
 182 }
 183
 184 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr)
 185 {
 186         REG_WRITE(REG_GLOBAL, 0x01, (addr[0] << 8) | addr[1]);
 187         REG_WRITE(REG_GLOBAL, 0x02, (addr[2] << 8) | addr[3]);
 188         REG_WRITE(REG_GLOBAL, 0x03, (addr[4] << 8) | addr[5]);
 189
 190         return 0;
 191 }
 192
 193 int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 194 {
 195         int i;
 196         int ret;
 197
 198         for (i = 0; i < 6; i++) {
 199                 int j;
 200
 201                 /* Write the MAC address byte. */
 202                 REG_WRITE(REG_GLOBAL2, 0x0d, 0x8000 | (i << 8) | addr[i]);
 203
 204                 /* Wait for the write to complete. */
 205                 for (j = 0; j < 16; j++) {
 206                         ret = REG_READ(REG_GLOBAL2, 0x0d);
 207                         if ((ret & 0x8000) == 0)
 208                                 break;
 209                 }
 210                 if (j == 16)
 211                         return -ETIMEDOUT;
 212         }
 213
 214         return 0;
 215 }
 216
 217 /* Must be called with phy mutex held */
 218 static int _mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
 219 {
 220         if (addr >= 0)
 221                 return mv88e6xxx_reg_read(ds, addr, regnum);
 222         return 0xffff;
 223 }
 224
 225 /* Must be called with phy mutex held */
 226 static int _mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum,
 227                                 u16 val)
 228 {
 229         if (addr >= 0)
 230                 return mv88e6xxx_reg_write(ds, addr, regnum, val);
 231         return 0;
 232 }
 233
 234 #ifdef CONFIG_NET_DSA_MV88E6XXX_NEED_PPU
 235 static int mv88e6xxx_ppu_disable(struct dsa_switch *ds)
 236 {
 237         int ret;
 238         unsigned long timeout;
 239
 240         ret = REG_READ(REG_GLOBAL, 0x04);
 241         REG_WRITE(REG_GLOBAL, 0x04, ret & ~0x4000);
 242
 243         timeout = jiffies + 1 * HZ;
 244         while (time_before(jiffies, timeout)) {
 245                 ret = REG_READ(REG_GLOBAL, 0x00);
 246                 usleep_range(1000, 2000);
 247                 if ((ret & 0xc000) != 0xc000)
 248                         return 0;
 249         }
 250
 251         return -ETIMEDOUT;
 252 }
 253
 254 static int mv88e6xxx_ppu_enable(struct dsa_switch *ds)
 255 {
 256         int ret;
 257         unsigned long timeout;
 258
 259         ret = REG_READ(REG_GLOBAL, 0x04);
 260         REG_WRITE(REG_GLOBAL, 0x04, ret | 0x4000);
 261
 262         timeout = jiffies + 1 * HZ;
 263         while (time_before(jiffies, timeout)) {
 264                 ret = REG_READ(REG_GLOBAL, 0x00);
 265                 usleep_range(1000, 2000);
 266                 if ((ret & 0xc000) == 0xc000)
 267                         return 0;
 268         }
 269
 270         return -ETIMEDOUT;
 271 }
 272
 273 static void mv88e6xxx_ppu_reenable_work(struct work_struct *ugly)
 274 {
 275         struct mv88e6xxx_priv_state *ps;
 276
 277         ps = container_of(ugly, struct mv88e6xxx_priv_state, ppu_work);
 278         if (mutex_trylock(&ps->ppu_mutex)) {
 279                 struct dsa_switch *ds = ((struct dsa_switch *)ps) - 1;
 280
 281                 if (mv88e6xxx_ppu_enable(ds) == 0)
 282                         ps->ppu_disabled = 0;
 283                 mutex_unlock(&ps->ppu_mutex);
 284         }
 285 }
 286
 287 static void mv88e6xxx_ppu_reenable_timer(unsigned long _ps)
 288 {
 289         struct mv88e6xxx_priv_state *ps = (void *)_ps;
 290
 291         schedule_work(&ps->ppu_work);
 292 }
 293
 294 static int mv88e6xxx_ppu_access_get(struct dsa_switch *ds)
 295 {
 296         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 297         int ret;
 298
 299         mutex_lock(&ps->ppu_mutex);
 300
 301         /* If the PHY polling unit is enabled, disable it so that
 302          * we can access the PHY registers.  If it was already
 303          * disabled, cancel the timer that is going to re-enable
 304          * it.
 305          */
 306         if (!ps->ppu_disabled) {
 307                 ret = mv88e6xxx_ppu_disable(ds);
 308                 if (ret < 0) {
 309                         mutex_unlock(&ps->ppu_mutex);
 310                         return ret;
 311                 }
 312                 ps->ppu_disabled = 1;
 313         } else {
 314                 del_timer(&ps->ppu_timer);
 315                 ret = 0;
 316         }
 317
 318         return ret;
 319 }
 320
 321 static void mv88e6xxx_ppu_access_put(struct dsa_switch *ds)
 322 {
 323         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 324
 325         /* Schedule a timer to re-enable the PHY polling unit. */
 326         mod_timer(&ps->ppu_timer, jiffies + msecs_to_jiffies(10));
 327         mutex_unlock(&ps->ppu_mutex);
 328 }
 329
 330 void mv88e6xxx_ppu_state_init(struct dsa_switch *ds)
 331 {
 332         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 333
 334         mutex_init(&ps->ppu_mutex);
 335         INIT_WORK(&ps->ppu_work, mv88e6xxx_ppu_reenable_work);
 336         init_timer(&ps->ppu_timer);
 337         ps->ppu_timer.data = (unsigned long)ps;
 338         ps->ppu_timer.function = mv88e6xxx_ppu_reenable_timer;
 339 }
 340
 341 int mv88e6xxx_phy_read_ppu(struct dsa_switch *ds, int addr, int regnum)
 342 {
 343         int ret;
 344
 345         ret = mv88e6xxx_ppu_access_get(ds);
 346         if (ret >= 0) {
 347                 ret = mv88e6xxx_reg_read(ds, addr, regnum);
 348                 mv88e6xxx_ppu_access_put(ds);
 349         }
 350
 351         return ret;
 352 }
 353
 354 int mv88e6xxx_phy_write_ppu(struct dsa_switch *ds, int addr,
 355                             int regnum, u16 val)
 356 {
 357         int ret;
 358
 359         ret = mv88e6xxx_ppu_access_get(ds);
 360         if (ret >= 0) {
 361                 ret = mv88e6xxx_reg_write(ds, addr, regnum, val);
 362                 mv88e6xxx_ppu_access_put(ds);
 363         }
 364
 365         return ret;
 366 }
 367 #endif
 368
 369 void mv88e6xxx_poll_link(struct dsa_switch *ds)
 370 {
 371         int i;
 372
 373         for (i = 0; i < DSA_MAX_PORTS; i++) {
 374                 struct net_device *dev;
 375                 int uninitialized_var(port_status);
 376                 int link;
 377                 int speed;
 378                 int duplex;
 379                 int fc;
 380
 381                 dev = ds->ports[i];
 382                 if (dev == NULL)
 383                         continue;
 384
 385                 link = 0;
 386                 if (dev->flags & IFF_UP) {
 387                         port_status = mv88e6xxx_reg_read(ds, REG_PORT(i), 0x00);
 388                         if (port_status < 0)
 389                                 continue;
 390
 391                         link = !!(port_status & 0x0800);
 392                 }
 393
 394                 if (!link) {
 395                         if (netif_carrier_ok(dev)) {
 396                                 netdev_info(dev, "link down\n");
 397                                 netif_carrier_off(dev);
 398                         }
 399                         continue;
 400                 }
 401
 402                 switch (port_status & 0x0300) {
 403                 case 0x0000:
 404                         speed = 10;
 405                         break;
 406                 case 0x0100:
 407                         speed = 100;
 408                         break;
 409                 case 0x0200:
 410                         speed = 1000;
 411                         break;
 412                 default:
 413                         speed = -1;
 414                         break;
 415                 }
 416                 duplex = (port_status & 0x0400) ? 1 : 0;
 417                 fc = (port_status & 0x8000) ? 1 : 0;
 418
 419                 if (!netif_carrier_ok(dev)) {
 420                         netdev_info(dev,
 421                                     "link up, %d Mb/s, %s duplex, flow control %sabled\n",
 422                                     speed,
 423                                     duplex ? "full" : "half",
 424                                     fc ? "en" : "dis");
 425                         netif_carrier_on(dev);
 426                 }
 427         }
 428 }
 429
 430 static int mv88e6xxx_stats_wait(struct dsa_switch *ds)
 431 {
 432         int ret;
 433         int i;
 434
 435         for (i = 0; i < 10; i++) {
 436                 ret = REG_READ(REG_GLOBAL, 0x1d);
 437                 if ((ret & 0x8000) == 0)
 438                         return 0;
 439         }
 440
 441         return -ETIMEDOUT;
 442 }
 443
 444 static int mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
 445 {
 446         int ret;
 447
 448         /* Snapshot the hardware statistics counters for this port. */
 449         REG_WRITE(REG_GLOBAL, 0x1d, 0xdc00 | port);
 450
 451         /* Wait for the snapshotting to complete. */
 452         ret = mv88e6xxx_stats_wait(ds);
 453         if (ret < 0)
 454                 return ret;
 455
 456         return 0;
 457 }
 458
 459 static void mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 460 {
 461         u32 _val;
 462         int ret;
 463
 464         *val = 0;
 465
 466         ret = mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x1d, 0xcc00 | stat);
 467         if (ret < 0)
 468                 return;
 469
 470         ret = mv88e6xxx_stats_wait(ds);
 471         if (ret < 0)
 472                 return;
 473
 474         ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x1e);
 475         if (ret < 0)
 476                 return;
 477
 478         _val = ret << 16;
 479
 480         ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x1f);
 481         if (ret < 0)
 482                 return;
 483
 484         *val = _val | ret;
 485 }
 486
 487 static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
 488         { "in_good_octets", 8, 0x00, },
 489         { "in_bad_octets", 4, 0x02, },
 490         { "in_unicast", 4, 0x04, },
 491         { "in_broadcasts", 4, 0x06, },
 492         { "in_multicasts", 4, 0x07, },
 493         { "in_pause", 4, 0x16, },
 494         { "in_undersize", 4, 0x18, },
 495         { "in_fragments", 4, 0x19, },
 496         { "in_oversize", 4, 0x1a, },
 497         { "in_jabber", 4, 0x1b, },
 498         { "in_rx_error", 4, 0x1c, },
 499         { "in_fcs_error", 4, 0x1d, },
 500         { "out_octets", 8, 0x0e, },
 501         { "out_unicast", 4, 0x10, },
 502         { "out_broadcasts", 4, 0x13, },
 503         { "out_multicasts", 4, 0x12, },
 504         { "out_pause", 4, 0x15, },
 505         { "excessive", 4, 0x11, },
 506         { "collisions", 4, 0x1e, },
 507         { "deferred", 4, 0x05, },
 508         { "single", 4, 0x14, },
 509         { "multiple", 4, 0x17, },
 510         { "out_fcs_error", 4, 0x03, },
 511         { "late", 4, 0x1f, },
 512         { "hist_64bytes", 4, 0x08, },
 513         { "hist_65_127bytes", 4, 0x09, },
 514         { "hist_128_255bytes", 4, 0x0a, },
 515         { "hist_256_511bytes", 4, 0x0b, },
 516         { "hist_512_1023bytes", 4, 0x0c, },
 517         { "hist_1024_max_bytes", 4, 0x0d, },
 518         /* Not all devices have the following counters */
 519         { "sw_in_discards", 4, 0x110, },
 520         { "sw_in_filtered", 2, 0x112, },
 521         { "sw_out_filtered", 2, 0x113, },
 522
 523 };
 524
 525 static bool have_sw_in_discards(struct dsa_switch *ds)
 526 {
 527         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 528
 529         switch (ps->id) {
 530         case ID_6095: case ID_6161: case ID_6165:
 531         case ID_6171: case ID_6172: case ID_6176:
 532         case ID_6182: case ID_6185: case ID_6352:
 533                 return true;
 534         default:
 535                 return false;
 536         }
 537 }
 538
 539 static void _mv88e6xxx_get_strings(struct dsa_switch *ds,
 540                                    int nr_stats,
 541                                    struct mv88e6xxx_hw_stat *stats,
 542                                    int port, uint8_t *data)
 543 {
 544         int i;
 545
 546         for (i = 0; i < nr_stats; i++) {
 547                 memcpy(data + i * ETH_GSTRING_LEN,
 548                        stats[i].string, ETH_GSTRING_LEN);
 549         }
 550 }
 551
 552 static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 553                                          int nr_stats,
 554                                          struct mv88e6xxx_hw_stat *stats,
 555                                          int port, uint64_t *data)
 556 {
 557         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 558         int ret;
 559         int i;
 560
 561         mutex_lock(&ps->stats_mutex);
 562
 563         ret = mv88e6xxx_stats_snapshot(ds, port);
 564         if (ret < 0) {
 565                 mutex_unlock(&ps->stats_mutex);
 566                 return;
 567         }
 568
 569         /* Read each of the counters. */
 570         for (i = 0; i < nr_stats; i++) {
 571                 struct mv88e6xxx_hw_stat *s = stats + i;
 572                 u32 low;
 573                 u32 high = 0;
 574
 575                 if (s->reg >= 0x100) {
 576                         int ret;
 577
 578                         ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
 579                                                  s->reg - 0x100);
 580                         if (ret < 0)
 581                                 goto error;
 582                         low = ret;
 583                         if (s->sizeof_stat == 4) {
 584                                 ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
 585                                                          s->reg - 0x100 + 1);
 586                                 if (ret < 0)
 587                                         goto error;
 588                                 high = ret;
 589                         }
 590                         data[i] = (((u64)high) << 16) | low;
 591                         continue;
 592                 }
 593                 mv88e6xxx_stats_read(ds, s->reg, &low);
 594                 if (s->sizeof_stat == 8)
 595                         mv88e6xxx_stats_read(ds, s->reg + 1, &high);
 596
 597                 data[i] = (((u64)high) << 32) | low;
 598         }
 599 error:
 600         mutex_unlock(&ps->stats_mutex);
 601 }
 602
 603 /* All the statistics in the table */
 604 void
 605 mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
 606 {
 607         if (have_sw_in_discards(ds))
 608                 _mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
 609                                        mv88e6xxx_hw_stats, port, data);
 610         else
 611                 _mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
 612                                        mv88e6xxx_hw_stats, port, data);
 613 }
 614
 615 int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 616 {
 617         if (have_sw_in_discards(ds))
 618                 return ARRAY_SIZE(mv88e6xxx_hw_stats);
 619         return ARRAY_SIZE(mv88e6xxx_hw_stats) - 3;
 620 }
 621
 622 void
 623 mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 624                             int port, uint64_t *data)
 625 {
 626         if (have_sw_in_discards(ds))
 627                 _mv88e6xxx_get_ethtool_stats(
 628                         ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
 629                         mv88e6xxx_hw_stats, port, data);
 630         else
 631                 _mv88e6xxx_get_ethtool_stats(
 632                         ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
 633                         mv88e6xxx_hw_stats, port, data);
 634 }
 635
 636 int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
 637 {
 638         return 32 * sizeof(u16);
 639 }
 640
 641 void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 642                         struct ethtool_regs *regs, void *_p)
 643 {
 644         u16 *p = _p;
 645         int i;
 646
 647         regs->version = 0;
 648
 649         memset(p, 0xff, 32 * sizeof(u16));
 650
 651         for (i = 0; i < 32; i++) {
 652                 int ret;
 653
 654                 ret = mv88e6xxx_reg_read(ds, REG_PORT(port), i);
 655                 if (ret >= 0)
 656                         p[i] = ret;
 657         }
 658 }
 659
 660 #ifdef CONFIG_NET_DSA_HWMON
 661
 662 int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 663 {
 664         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 665         int ret;
 666         int val;
 667
 668         *temp = 0;
 669
 670         mutex_lock(&ps->phy_mutex);
 671
 672         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
 673         if (ret < 0)
 674                 goto error;
 675
 676         /* Enable temperature sensor */
 677         ret = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 678         if (ret < 0)
 679                 goto error;
 680
 681         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
 682         if (ret < 0)
 683                 goto error;
 684
 685         /* Wait for temperature to stabilize */
 686         usleep_range(10000, 12000);
 687
 688         val = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 689         if (val < 0) {
 690                 ret = val;
 691                 goto error;
 692         }
 693
 694         /* Disable temperature sensor */
 695         ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
 696         if (ret < 0)
 697                 goto error;
 698
 699         *temp = ((val & 0x1f) - 5) * 5;
 700
 701 error:
 702         _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
 703         mutex_unlock(&ps->phy_mutex);
 704         return ret;
 705 }
 706 #endif /* CONFIG_NET_DSA_HWMON */
 707
 708 static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
 709 {
 710         unsigned long timeout = jiffies + HZ / 10;
 711
 712         while (time_before(jiffies, timeout)) {
 713                 int ret;
 714
 715                 ret = REG_READ(reg, offset);
 716                 if (!(ret & mask))
 717                         return 0;
 718
 719                 usleep_range(1000, 2000);
 720         }
 721         return -ETIMEDOUT;
 722 }
 723
 724 int mv88e6xxx_phy_wait(struct dsa_switch *ds)
 725 {
 726         return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x18, 0x8000);
 727 }
 728
 729 int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
 730 {
 731         return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x14, 0x0800);
 732 }
 733
 734 int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
 735 {
 736         return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x14, 0x8000);
 737 }
 738
 739 /* Must be called with SMI lock held */
 740 static int _mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
 741 {
 742         unsigned long timeout = jiffies + HZ / 10;
 743
 744         while (time_before(jiffies, timeout)) {
 745                 int ret;
 746
 747                 ret = _mv88e6xxx_reg_read(ds, reg, offset);
 748                 if (ret < 0)
 749                         return ret;
 750                 if (!(ret & mask))
 751                         return 0;
 752
 753                 usleep_range(1000, 2000);
 754         }
 755         return -ETIMEDOUT;
 756 }
 757
 758 /* Must be called with SMI lock held */
 759 static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
 760 {
 761         return _mv88e6xxx_wait(ds, REG_GLOBAL, 0x0b, ATU_BUSY);
 762 }
 763
 764 /* Must be called with phy mutex held */
 765 static int _mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr,
 766                                         int regnum)
 767 {
 768         int ret;
 769
 770         REG_WRITE(REG_GLOBAL2, 0x18, 0x9800 | (addr << 5) | regnum);
 771
 772         ret = mv88e6xxx_phy_wait(ds);
 773         if (ret < 0)
 774                 return ret;
 775
 776         return REG_READ(REG_GLOBAL2, 0x19);
 777 }
 778
 779 /* Must be called with phy mutex held */
 780 static int _mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr,
 781                                          int regnum, u16 val)
 782 {
 783         REG_WRITE(REG_GLOBAL2, 0x19, val);
 784         REG_WRITE(REG_GLOBAL2, 0x18, 0x9400 | (addr << 5) | regnum);
 785
 786         return mv88e6xxx_phy_wait(ds);
 787 }
 788
 789 int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
 790 {
 791         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 792         int reg;
 793
 794         mutex_lock(&ps->phy_mutex);
 795
 796         reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 797         if (reg < 0)
 798                 goto out;
 799
 800         e->eee_enabled = !!(reg & 0x0200);
 801         e->tx_lpi_enabled = !!(reg & 0x0100);
 802
 803         reg = mv88e6xxx_reg_read(ds, REG_PORT(port), 0);
 804         if (reg < 0)
 805                 goto out;
 806
 807         e->eee_active = !!(reg & 0x0040);
 808         reg = 0;
 809
 810 out:
 811         mutex_unlock(&ps->phy_mutex);
 812         return reg;
 813 }
 814
 815 int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
 816                       struct phy_device *phydev, struct ethtool_eee *e)
 817 {
 818         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 819         int reg;
 820         int ret;
 821
 822         mutex_lock(&ps->phy_mutex);
 823
 824         ret = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 825         if (ret < 0)
 826                 goto out;
 827
 828         reg = ret & ~0x0300;
 829         if (e->eee_enabled)
 830                 reg |= 0x0200;
 831         if (e->tx_lpi_enabled)
 832                 reg |= 0x0100;
 833
 834         ret = _mv88e6xxx_phy_write_indirect(ds, port, 16, reg);
 835 out:
 836         mutex_unlock(&ps->phy_mutex);
 837
 838         return ret;
 839 }
 840
 841 static int _mv88e6xxx_atu_cmd(struct dsa_switch *ds, int fid, u16 cmd)
 842 {
 843         int ret;
 844
 845         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x01, fid);
 846         if (ret < 0)
 847                 return ret;
 848
 849         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x0b, cmd);
 850         if (ret < 0)
 851                 return ret;
 852
 853         return _mv88e6xxx_atu_wait(ds);
 854 }
 855
 856 static int _mv88e6xxx_flush_fid(struct dsa_switch *ds, int fid)
 857 {
 858         int ret;
 859
 860         ret = _mv88e6xxx_atu_wait(ds);
 861         if (ret < 0)
 862                 return ret;
 863
 864         return _mv88e6xxx_atu_cmd(ds, fid, ATU_CMD_FLUSH_NONSTATIC_FID);
 865 }
 866
 867 static int mv88e6xxx_set_port_state(struct dsa_switch *ds, int port, u8 state)
 868 {
 869         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 870         int reg, ret;
 871         u8 oldstate;
 872
 873         mutex_lock(&ps->smi_mutex);
 874
 875         reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), 0x04);
 876         if (reg < 0)
 877                 goto abort;
 878
 879         oldstate = reg & PSTATE_MASK;
 880         if (oldstate != state) {
 881                 /* Flush forwarding database if we're moving a port
 882                  * from Learning or Forwarding state to Disabled or
 883                  * Blocking or Listening state.
 884                  */
 885                 if (oldstate >= PSTATE_LEARNING && state <= PSTATE_BLOCKING) {
 886                         ret = _mv88e6xxx_flush_fid(ds, ps->fid[port]);
 887                         if (ret)
 888                                 goto abort;
 889                 }
 890                 reg = (reg & ~PSTATE_MASK) | state;
 891                 ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), 0x04, reg);
 892         }
 893
 894 abort:
 895         mutex_unlock(&ps->smi_mutex);
 896         return ret;
 897 }
 898
 899 /* Must be called with smi lock held */
 900 static int _mv88e6xxx_update_port_config(struct dsa_switch *ds, int port)
 901 {
 902         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 903         u8 fid = ps->fid[port];
 904         u16 reg = fid << 12;
 905
 906         if (dsa_is_cpu_port(ds, port))
 907                 reg |= ds->phys_port_mask;
 908         else
 909                 reg |= (ps->bridge_mask[fid] |
 910                        (1 << dsa_upstream_port(ds))) & ~(1 << port);
 911
 912         return _mv88e6xxx_reg_write(ds, REG_PORT(port), 0x06, reg);
 913 }
 914
 915 /* Must be called with smi lock held */
 916 static int _mv88e6xxx_update_bridge_config(struct dsa_switch *ds, int fid)
 917 {
 918         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 919         int port;
 920         u32 mask;
 921         int ret;
 922
 923         mask = ds->phys_port_mask;
 924         while (mask) {
 925                 port = __ffs(mask);
 926                 mask &= ~(1 << port);
 927                 if (ps->fid[port] != fid)
 928                         continue;
 929
 930                 ret = _mv88e6xxx_update_port_config(ds, port);
 931                 if (ret)
 932                         return ret;
 933         }
 934
 935         return _mv88e6xxx_flush_fid(ds, fid);
 936 }
 937
 938 /* Bridge handling functions */
 939
 940 int mv88e6xxx_join_bridge(struct dsa_switch *ds, int port, u32 br_port_mask)
 941 {
 942         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 943         int ret = 0;
 944         u32 nmask;
 945         int fid;
 946
 947         /* If the bridge group is not empty, join that group.
 948          * Otherwise create a new group.
 949          */
 950         fid = ps->fid[port];
 951         nmask = br_port_mask & ~(1 << port);
 952         if (nmask)
 953                 fid = ps->fid[__ffs(nmask)];
 954
 955         nmask = ps->bridge_mask[fid] | (1 << port);
 956         if (nmask != br_port_mask) {
 957                 netdev_err(ds->ports[port],
 958                            "join: Bridge port mask mismatch fid=%d mask=0x%x expected 0x%x\n",
 959                            fid, br_port_mask, nmask);
 960                 return -EINVAL;
 961         }
 962
 963         mutex_lock(&ps->smi_mutex);
 964
 965         ps->bridge_mask[fid] = br_port_mask;
 966
 967         if (fid != ps->fid[port]) {
 968                 ps->fid_mask |= 1 << ps->fid[port];
 969                 ps->fid[port] = fid;
 970                 ret = _mv88e6xxx_update_bridge_config(ds, fid);
 971         }
 972
 973         mutex_unlock(&ps->smi_mutex);
 974
 975         return ret;
 976 }
 977
 978 int mv88e6xxx_leave_bridge(struct dsa_switch *ds, int port, u32 br_port_mask)
 979 {
 980         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 981         u8 fid, newfid;
 982         int ret;
 983
 984         fid = ps->fid[port];
 985
 986         if (ps->bridge_mask[fid] != br_port_mask) {
 987                 netdev_err(ds->ports[port],
 988                            "leave: Bridge port mask mismatch fid=%d mask=0x%x expected 0x%x\n",
 989                            fid, br_port_mask, ps->bridge_mask[fid]);
 990                 return -EINVAL;
 991         }
 992
 993         /* If the port was the last port of a bridge, we are done.
 994          * Otherwise assign a new fid to the port, and fix up
 995          * the bridge configuration.
 996          */
 997         if (br_port_mask == (1 << port))
 998                 return 0;
 999
1000         mutex_lock(&ps->smi_mutex);
1001
1002         newfid = __ffs(ps->fid_mask);
1003         ps->fid[port] = newfid;
1004         ps->fid_mask &= (1 << newfid);
1005         ps->bridge_mask[fid] &= ~(1 << port);
1006         ps->bridge_mask[newfid] = 1 << port;
1007
1008         ret = _mv88e6xxx_update_bridge_config(ds, fid);
1009         if (!ret)
1010                 ret = _mv88e6xxx_update_bridge_config(ds, newfid);
1011
1012         mutex_unlock(&ps->smi_mutex);
1013
1014         return ret;
1015 }
1016
1017 int mv88e6xxx_port_stp_update(struct dsa_switch *ds, int port, u8 state)
1018 {
1019         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1020         int stp_state;
1021
1022         switch (state) {
1023         case BR_STATE_DISABLED:
1024                 stp_state = PSTATE_DISABLED;
1025                 break;
1026         case BR_STATE_BLOCKING:
1027         case BR_STATE_LISTENING:
1028                 stp_state = PSTATE_BLOCKING;
1029                 break;
1030         case BR_STATE_LEARNING:
1031                 stp_state = PSTATE_LEARNING;
1032                 break;
1033         case BR_STATE_FORWARDING:
1034         default:
1035                 stp_state = PSTATE_FORWARDING;
1036                 break;
1037         }
1038
1039         netdev_dbg(ds->ports[port], "port state %d [%d]\n", state, stp_state);
1040
1041         /* mv88e6xxx_port_stp_update may be called with softirqs disabled,
1042          * so we can not update the port state directly but need to schedule it.
1043          */
1044         ps->port_state[port] = stp_state;
1045         set_bit(port, &ps->port_state_update_mask);
1046         schedule_work(&ps->bridge_work);
1047
1048         return 0;
1049 }
1050
1051 static int __mv88e6xxx_write_addr(struct dsa_switch *ds,
1052                                   const unsigned char *addr)
1053 {
1054         int i, ret;
1055
1056         for (i = 0; i < 3; i++) {
1057                 ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x0d + i,
1058                                         (addr[i * 2] << 8) | addr[i * 2 + 1]);
1059                 if (ret < 0)
1060                         return ret;
1061         }
1062
1063         return 0;
1064 }
1065
1066 static int __mv88e6xxx_read_addr(struct dsa_switch *ds, unsigned char *addr)
1067 {
1068         int i, ret;
1069
1070         for (i = 0; i < 3; i++) {
1071                 ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x0d + i);
1072                 if (ret < 0)
1073                         return ret;
1074                 addr[i * 2] = ret >> 8;
1075                 addr[i * 2 + 1] = ret & 0xff;
1076         }
1077
1078         return 0;
1079 }
1080
1081 static int __mv88e6xxx_port_fdb_cmd(struct dsa_switch *ds, int port,
1082                                     const unsigned char *addr, int state)
1083 {
1084         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1085         u8 fid = ps->fid[port];
1086         int ret;
1087
1088         ret = _mv88e6xxx_atu_wait(ds);
1089         if (ret < 0)
1090                 return ret;
1091
1092         ret = __mv88e6xxx_write_addr(ds, addr);
1093         if (ret < 0)
1094                 return ret;
1095
1096         ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, 0x0c,
1097                                    (0x10 << port) | state);
1098         if (ret)
1099                 return ret;
1100
1101         ret = _mv88e6xxx_atu_cmd(ds, fid, ATU_CMD_LOAD_FID);
1102
1103         return ret;
1104 }
1105
1106 int mv88e6xxx_port_fdb_add(struct dsa_switch *ds, int port,
1107                            const unsigned char *addr, u16 vid)
1108 {
1109         int state = is_multicast_ether_addr(addr) ?
1110                                         FDB_STATE_MC_STATIC : FDB_STATE_STATIC;
1111         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1112         int ret;
1113
1114         mutex_lock(&ps->smi_mutex);
1115         ret = __mv88e6xxx_port_fdb_cmd(ds, port, addr, state);
1116         mutex_unlock(&ps->smi_mutex);
1117
1118         return ret;
1119 }
1120
1121 int mv88e6xxx_port_fdb_del(struct dsa_switch *ds, int port,
1122                            const unsigned char *addr, u16 vid)
1123 {
1124         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1125         int ret;
1126
1127         mutex_lock(&ps->smi_mutex);
1128         ret = __mv88e6xxx_port_fdb_cmd(ds, port, addr, FDB_STATE_UNUSED);
1129         mutex_unlock(&ps->smi_mutex);
1130
1131         return ret;
1132 }
1133
1134 static int __mv88e6xxx_port_getnext(struct dsa_switch *ds, int port,
1135                                     unsigned char *addr, bool *is_static)
1136 {
1137         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1138         u8 fid = ps->fid[port];
1139         int ret, state;
1140
1141         ret = _mv88e6xxx_atu_wait(ds);
1142         if (ret < 0)
1143                 return ret;
1144
1145         ret = __mv88e6xxx_write_addr(ds, addr);
1146         if (ret < 0)
1147                 return ret;
1148
1149         do {
1150                 ret = _mv88e6xxx_atu_cmd(ds, fid, ATU_CMD_GETNEXT_FID);
1151                 if (ret < 0)
1152                         return ret;
1153
1154                 ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, 0x0c);
1155                 if (ret < 0)
1156                         return ret;
1157                 state = ret & FDB_STATE_MASK;
1158                 if (state == FDB_STATE_UNUSED)
1159                         return -ENOENT;
1160         } while (!(((ret >> 4) & 0xff) & (1 << port)));
1161
1162         ret = __mv88e6xxx_read_addr(ds, addr);
1163         if (ret < 0)
1164                 return ret;
1165
1166         *is_static = state == (is_multicast_ether_addr(addr) ?
1167                                FDB_STATE_MC_STATIC : FDB_STATE_STATIC);
1168
1169         return 0;
1170 }
1171
1172 /* get next entry for port */
1173 int mv88e6xxx_port_fdb_getnext(struct dsa_switch *ds, int port,
1174                                unsigned char *addr, bool *is_static)
1175 {
1176         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1177         int ret;
1178
1179         mutex_lock(&ps->smi_mutex);
1180         ret = __mv88e6xxx_port_getnext(ds, port, addr, is_static);
1181         mutex_unlock(&ps->smi_mutex);
1182
1183         return ret;
1184 }
1185
1186 static void mv88e6xxx_bridge_work(struct work_struct *work)
1187 {
1188         struct mv88e6xxx_priv_state *ps;
1189         struct dsa_switch *ds;
1190         int port;
1191
1192         ps = container_of(work, struct mv88e6xxx_priv_state, bridge_work);
1193         ds = ((struct dsa_switch *)ps) - 1;
1194
1195         while (ps->port_state_update_mask) {
1196                 port = __ffs(ps->port_state_update_mask);
1197                 clear_bit(port, &ps->port_state_update_mask);
1198                 mv88e6xxx_set_port_state(ds, port, ps->port_state[port]);
1199         }
1200 }
1201
1202 int mv88e6xxx_setup_port_common(struct dsa_switch *ds, int port)
1203 {
1204         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1205         int ret, fid;
1206
1207         mutex_lock(&ps->smi_mutex);
1208
1209         /* Port Control 1: disable trunking, disable sending
1210          * learning messages to this port.
1211          */
1212         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), 0x05, 0x0000);
1213         if (ret)
1214                 goto abort;
1215
1216         /* Port based VLAN map: give each port its own address
1217          * database, allow the CPU port to talk to each of the 'real'
1218          * ports, and allow each of the 'real' ports to only talk to
1219          * the upstream port.
1220          */
1221         fid = __ffs(ps->fid_mask);
1222         ps->fid[port] = fid;
1223         ps->fid_mask &= ~(1 << fid);
1224
1225         if (!dsa_is_cpu_port(ds, port))
1226                 ps->bridge_mask[fid] = 1 << port;
1227
1228         ret = _mv88e6xxx_update_port_config(ds, port);
1229         if (ret)
1230                 goto abort;
1231
1232         /* Default VLAN ID and priority: don't set a default VLAN
1233          * ID, and set the default packet priority to zero.
1234          */
1235         ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), 0x07, 0x0000);
1236 abort:
1237         mutex_unlock(&ps->smi_mutex);
1238         return ret;
1239 }
1240
1241 int mv88e6xxx_setup_common(struct dsa_switch *ds)
1242 {
1243         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1244
1245         mutex_init(&ps->smi_mutex);
1246         mutex_init(&ps->stats_mutex);
1247         mutex_init(&ps->phy_mutex);
1248
1249         ps->id = REG_READ(REG_PORT(0), 0x03) & 0xfff0;
1250
1251         ps->fid_mask = (1 << DSA_MAX_PORTS) - 1;
1252
1253         INIT_WORK(&ps->bridge_work, mv88e6xxx_bridge_work);
1254
1255         return 0;
1256 }
1257
1258 int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active)
1259 {
1260         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1261         u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
1262         unsigned long timeout;
1263         int ret;
1264         int i;
1265
1266         /* Set all ports to the disabled state. */
1267         for (i = 0; i < ps->num_ports; i++) {
1268                 ret = REG_READ(REG_PORT(i), 0x04);
1269                 REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
1270         }
1271
1272         /* Wait for transmit queues to drain. */
1273         usleep_range(2000, 4000);
1274
1275         /* Reset the switch. Keep the PPU active if requested. The PPU
1276          * needs to be active to support indirect phy register access
1277          * through global registers 0x18 and 0x19.
1278          */
1279         if (ppu_active)
1280                 REG_WRITE(REG_GLOBAL, 0x04, 0xc000);
1281         else
1282                 REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
1283
1284         /* Wait up to one second for reset to complete. */
1285         timeout = jiffies + 1 * HZ;
1286         while (time_before(jiffies, timeout)) {
1287                 ret = REG_READ(REG_GLOBAL, 0x00);
1288                 if ((ret & is_reset) == is_reset)
1289                         break;
1290                 usleep_range(1000, 2000);
1291         }
1292         if (time_after(jiffies, timeout))
1293                 return -ETIMEDOUT;
1294
1295         return 0;
1296 }
1297
1298 int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
1299 {
1300         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1301         int ret;
1302
1303         mutex_lock(&ps->phy_mutex);
1304         ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
1305         if (ret < 0)
1306                 goto error;
1307         ret = _mv88e6xxx_phy_read_indirect(ds, port, reg);
1308 error:
1309         _mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
1310         mutex_unlock(&ps->phy_mutex);
1311         return ret;
1312 }
1313
1314 int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
1315                              int reg, int val)
1316 {
1317         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1318         int ret;
1319
1320         mutex_lock(&ps->phy_mutex);
1321         ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
1322         if (ret < 0)
1323                 goto error;
1324
1325         ret = _mv88e6xxx_phy_write_indirect(ds, port, reg, val);
1326 error:
1327         _mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
1328         mutex_unlock(&ps->phy_mutex);
1329         return ret;
1330 }
1331
1332 static int mv88e6xxx_port_to_phy_addr(struct dsa_switch *ds, int port)
1333 {
1334         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1335
1336         if (port >= 0 && port < ps->num_ports)
1337                 return port;
1338         return -EINVAL;
1339 }
1340
1341 int
1342 mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
1343 {
1344         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1345         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
1346         int ret;
1347
1348         if (addr < 0)
1349                 return addr;
1350
1351         mutex_lock(&ps->phy_mutex);
1352         ret = _mv88e6xxx_phy_read(ds, addr, regnum);
1353         mutex_unlock(&ps->phy_mutex);
1354         return ret;
1355 }
1356
1357 int
1358 mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
1359 {
1360         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1361         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
1362         int ret;
1363
1364         if (addr < 0)
1365                 return addr;
1366
1367         mutex_lock(&ps->phy_mutex);
1368         ret = _mv88e6xxx_phy_write(ds, addr, regnum, val);
1369         mutex_unlock(&ps->phy_mutex);
1370         return ret;
1371 }
1372
1373 int
1374 mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum)
1375 {
1376         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1377         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
1378         int ret;
1379
1380         if (addr < 0)
1381                 return addr;
1382
1383         mutex_lock(&ps->phy_mutex);
1384         ret = _mv88e6xxx_phy_read_indirect(ds, addr, regnum);
1385         mutex_unlock(&ps->phy_mutex);
1386         return ret;
1387 }
1388
1389 int
1390 mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
1391                              u16 val)
1392 {
1393         struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
1394         int addr = mv88e6xxx_port_to_phy_addr(ds, port);
1395         int ret;
1396
1397         if (addr < 0)
1398                 return addr;
1399
1400         mutex_lock(&ps->phy_mutex);
1401         ret = _mv88e6xxx_phy_write_indirect(ds, addr, regnum, val);
1402         mutex_unlock(&ps->phy_mutex);
1403         return ret;
1404 }
1405
1406 static int __init mv88e6xxx_init(void)
1407 {
1408 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)
1409         register_switch_driver(&mv88e6131_switch_driver);
1410 #endif
1411 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
1412         register_switch_driver(&mv88e6123_61_65_switch_driver);
1413 #endif
1414 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6352)
1415         register_switch_driver(&mv88e6352_switch_driver);
1416 #endif
1417 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
1418         register_switch_driver(&mv88e6171_switch_driver);
1419 #endif
1420         return 0;
1421 }
1422 module_init(mv88e6xxx_init);
1423
1424 static void __exit mv88e6xxx_cleanup(void)
1425 {
1426 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
1427         unregister_switch_driver(&mv88e6171_switch_driver);
1428 #endif
1429 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
1430         unregister_switch_driver(&mv88e6123_61_65_switch_driver);
1431 #endif
1432 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)
1433         unregister_switch_driver(&mv88e6131_switch_driver);
1434 #endif
1435 }
1436 module_exit(mv88e6xxx_cleanup);
1437
1438 MODULE_AUTHOR("Lennert Buytenhek <buytenh@wantstofly.org>");
1439 MODULE_DESCRIPTION("Driver for Marvell 88E6XXX ethernet switch chips");
1440 MODULE_LICENSE("GPL");