target/linux/ramips/patches-4.4/0512-net-mediatek-add-swconfig-driver-for-esw_rt3050.patch

   1 From 4473f30809eed09037e1932a0c1805172cd997f7 Mon Sep 17 00:00:00 2001
   2 From: John Crispin <blogic@openwrt.org>
   3 Date: Mon, 14 Dec 2015 22:07:31 +0100
   4 Subject: [PATCH 512/513] net: mediatek: add swconfig driver for esw_rt3050
   5
   6 Signed-off-by: John Crispin <blogic@openwrt.org>
   7 ---
   8  drivers/net/ethernet/mediatek/esw_rt3050.c |  805 ++++++++++++++++++++++++++++
   9  1 file changed, 805 insertions(+)
  10
  11 --- a/drivers/net/ethernet/mediatek/esw_rt3050.c
  12 +++ b/drivers/net/ethernet/mediatek/esw_rt3050.c
  13 @@ -17,6 +17,8 @@
  14  #include <linux/platform_device.h>
  15  #include <asm/mach-ralink/ralink_regs.h>
  16
  17 +#include <linux/switch.h>
  18 +
  19  #include "mtk_eth_soc.h"
  20
  21  /* HW limitations for this switch:
  22 @@ -141,6 +143,8 @@
  23  #define RT305X_ESW_PORT5               5
  24  #define RT305X_ESW_PORT6               6
  25
  26 +#define RT305X_ESW_PORTS_NONE          0
  27 +
  28  #define RT305X_ESW_PMAP_LLLLLL         0x3f
  29  #define RT305X_ESW_PMAP_LLLLWL         0x2f
  30  #define RT305X_ESW_PMAP_WLLLLL         0x3e
  31 @@ -158,15 +162,51 @@
  32  #define RT305X_ESW_PORTS_ALL                                           \
  33                 (RT305X_ESW_PORTS_NOCPU | RT305X_ESW_PORTS_CPU)
  34
  35 +#define RT305X_ESW_NUM_VLANS           16
  36 +#define RT305X_ESW_NUM_VIDS            4096
  37  #define RT305X_ESW_NUM_PORTS           7
  38 +#define RT305X_ESW_NUM_LANWAN          6
  39  #define RT305X_ESW_NUM_LEDS            5
  40
  41 +#define RT5350_ESW_REG_PXTPC(_x)       (0x150 + (4 * _x))
  42  #define RT5350_EWS_REG_LED_POLARITY    0x168
  43  #define RT5350_RESET_EPHY              BIT(24)
  44
  45 +enum {
  46 +       /* Global attributes. */
  47 +       RT305X_ESW_ATTR_ENABLE_VLAN,
  48 +       RT305X_ESW_ATTR_ALT_VLAN_DISABLE,
  49 +       RT305X_ESW_ATTR_BC_STATUS,
  50 +       RT305X_ESW_ATTR_LED_FREQ,
  51 +       /* Port attributes. */
  52 +       RT305X_ESW_ATTR_PORT_DISABLE,
  53 +       RT305X_ESW_ATTR_PORT_DOUBLETAG,
  54 +       RT305X_ESW_ATTR_PORT_UNTAG,
  55 +       RT305X_ESW_ATTR_PORT_LED,
  56 +       RT305X_ESW_ATTR_PORT_LAN,
  57 +       RT305X_ESW_ATTR_PORT_RECV_BAD,
  58 +       RT305X_ESW_ATTR_PORT_RECV_GOOD,
  59 +       RT5350_ESW_ATTR_PORT_TR_BAD,
  60 +       RT5350_ESW_ATTR_PORT_TR_GOOD,
  61 +};
  62 +
  63  struct esw_port {
  64         bool    disable;
  65 +       bool    doubletag;
  66 +       bool    untag;
  67         u8      led;
  68 +       u16     pvid;
  69 +};
  70 +
  71 +struct esw_vlan {
  72 +       u8      ports;
  73 +       u16     vid;
  74 +};
  75 +
  76 +enum {
  77 +       RT305X_ESW_VLAN_CONFIG_NONE = 0,
  78 +       RT305X_ESW_VLAN_CONFIG_LLLLW,
  79 +       RT305X_ESW_VLAN_CONFIG_WLLLL,
  80  };
  81
  82  struct rt305x_esw {
  83 @@ -180,6 +220,12 @@ struct rt305x_esw {
  84         unsigned char           port_map;
  85         unsigned int            reg_led_polarity;
  86
  87 +       struct switch_dev       swdev;
  88 +       bool                    global_vlan_enable;
  89 +       bool                    alt_vlan_disable;
  90 +       int                     bc_storm_protect;
  91 +       int                     led_frequency;
  92 +       struct esw_vlan vlans[RT305X_ESW_NUM_VLANS];
  93         struct esw_port ports[RT305X_ESW_NUM_PORTS];
  94
  95  };
  96 @@ -252,6 +298,71 @@ out:
  97         return ret;
  98  }
  99
 100 +static unsigned esw_get_vlan_id(struct rt305x_esw *esw, unsigned vlan)
 101 +{
 102 +       unsigned s;
 103 +       unsigned val;
 104 +
 105 +       s = RT305X_ESW_VLANI_VID_S * (vlan % 2);
 106 +       val = esw_r32(esw, RT305X_ESW_REG_VLANI(vlan / 2));
 107 +       val = (val >> s) & RT305X_ESW_VLANI_VID_M;
 108 +
 109 +       return val;
 110 +}
 111 +
 112 +static void esw_set_vlan_id(struct rt305x_esw *esw, unsigned vlan, unsigned vid)
 113 +{
 114 +       unsigned s;
 115 +
 116 +       s = RT305X_ESW_VLANI_VID_S * (vlan % 2);
 117 +       esw_rmw(esw,
 118 +                      RT305X_ESW_REG_VLANI(vlan / 2),
 119 +                      RT305X_ESW_VLANI_VID_M << s,
 120 +                      (vid & RT305X_ESW_VLANI_VID_M) << s);
 121 +}
 122 +
 123 +static unsigned esw_get_pvid(struct rt305x_esw *esw, unsigned port)
 124 +{
 125 +       unsigned s, val;
 126 +
 127 +       s = RT305X_ESW_PVIDC_PVID_S * (port % 2);
 128 +       val = esw_r32(esw, RT305X_ESW_REG_PVIDC(port / 2));
 129 +       return (val >> s) & RT305X_ESW_PVIDC_PVID_M;
 130 +}
 131 +
 132 +static void esw_set_pvid(struct rt305x_esw *esw, unsigned port, unsigned pvid)
 133 +{
 134 +       unsigned s;
 135 +
 136 +       s = RT305X_ESW_PVIDC_PVID_S * (port % 2);
 137 +       esw_rmw(esw,
 138 +                      RT305X_ESW_REG_PVIDC(port / 2),
 139 +                      RT305X_ESW_PVIDC_PVID_M << s,
 140 +                      (pvid & RT305X_ESW_PVIDC_PVID_M) << s);
 141 +}
 142 +
 143 +static unsigned esw_get_vmsc(struct rt305x_esw *esw, unsigned vlan)
 144 +{
 145 +       unsigned s, val;
 146 +
 147 +       s = RT305X_ESW_VMSC_MSC_S * (vlan % 4);
 148 +       val = esw_r32(esw, RT305X_ESW_REG_VMSC(vlan / 4));
 149 +       val = (val >> s) & RT305X_ESW_VMSC_MSC_M;
 150 +
 151 +       return val;
 152 +}
 153 +
 154 +static void esw_set_vmsc(struct rt305x_esw *esw, unsigned vlan, unsigned msc)
 155 +{
 156 +       unsigned s;
 157 +
 158 +       s = RT305X_ESW_VMSC_MSC_S * (vlan % 4);
 159 +       esw_rmw(esw,
 160 +                      RT305X_ESW_REG_VMSC(vlan / 4),
 161 +                      RT305X_ESW_VMSC_MSC_M << s,
 162 +                      (msc & RT305X_ESW_VMSC_MSC_M) << s);
 163 +}
 164 +
 165  static unsigned esw_get_port_disable(struct rt305x_esw *esw)
 166  {
 167         unsigned reg;
 168 @@ -261,6 +372,59 @@ static unsigned esw_get_port_disable(str
 169                RT305X_ESW_POC0_DIS_PORT_M;
 170  }
 171
 172 +static void esw_set_port_disable(struct rt305x_esw *esw, unsigned disable_mask)
 173 +{
 174 +       unsigned old_mask;
 175 +       unsigned enable_mask;
 176 +       unsigned changed;
 177 +       int i;
 178 +
 179 +       old_mask = esw_get_port_disable(esw);
 180 +       changed = old_mask ^ disable_mask;
 181 +       enable_mask = old_mask & disable_mask;
 182 +
 183 +       /* enable before writing to MII */
 184 +       esw_rmw(esw, RT305X_ESW_REG_POC0,
 185 +                      (RT305X_ESW_POC0_DIS_PORT_M <<
 186 +                       RT305X_ESW_POC0_DIS_PORT_S),
 187 +                      enable_mask << RT305X_ESW_POC0_DIS_PORT_S);
 188 +
 189 +       for (i = 0; i < RT305X_ESW_NUM_LEDS; i++) {
 190 +               if (!(changed & (1 << i)))
 191 +                       continue;
 192 +               if (disable_mask & (1 << i)) {
 193 +                       /* disable */
 194 +                       rt305x_mii_write(esw, i, MII_BMCR,
 195 +                                        BMCR_PDOWN);
 196 +               } else {
 197 +                       /* enable */
 198 +                       rt305x_mii_write(esw, i, MII_BMCR,
 199 +                                        BMCR_FULLDPLX |
 200 +                                        BMCR_ANENABLE |
 201 +                                        BMCR_ANRESTART |
 202 +                                        BMCR_SPEED100);
 203 +               }
 204 +       }
 205 +
 206 +       /* disable after writing to MII */
 207 +       esw_rmw(esw, RT305X_ESW_REG_POC0,
 208 +                      (RT305X_ESW_POC0_DIS_PORT_M <<
 209 +                       RT305X_ESW_POC0_DIS_PORT_S),
 210 +                      disable_mask << RT305X_ESW_POC0_DIS_PORT_S);
 211 +}
 212 +
 213 +static void esw_set_gsc(struct rt305x_esw *esw)
 214 +{
 215 +       esw_rmw(esw, RT305X_ESW_REG_SGC,
 216 +               RT305X_ESW_GSC_BC_STROM_MASK << RT305X_ESW_GSC_BC_STROM_SHIFT,
 217 +               esw->bc_storm_protect << RT305X_ESW_GSC_BC_STROM_SHIFT);
 218 +       esw_rmw(esw, RT305X_ESW_REG_SGC,
 219 +               RT305X_ESW_GSC_LED_FREQ_MASK << RT305X_ESW_GSC_LED_FREQ_SHIFT,
 220 +               esw->led_frequency << RT305X_ESW_GSC_LED_FREQ_SHIFT);
 221 +}
 222 +
 223 +static int esw_apply_config(struct switch_dev *dev);
 224 +
 225  static void esw_hw_init(struct rt305x_esw *esw)
 226  {
 227         int i;
 228 @@ -519,6 +683,9 @@ static void esw_hw_init(struct rt305x_es
 229         for (i = 0; i < RT305X_ESW_NUM_LEDS; i++)
 230                 esw->ports[i].led = 0x05;
 231
 232 +       /* Apply the empty config. */
 233 +       esw_apply_config(&esw->swdev);
 234 +
 235         /* Only unmask the port change interrupt */
 236         esw_w32(esw, ~RT305X_ESW_PORT_ST_CHG, RT305X_ESW_REG_IMR);
 237  }
 238 @@ -541,11 +708,629 @@ static irqreturn_t esw_interrupt(int irq
 239         return IRQ_HANDLED;
 240  }
 241
 242 +static int esw_apply_config(struct switch_dev *dev)
 243 +{
 244 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 245 +       int i;
 246 +       u8 disable = 0;
 247 +       u8 doubletag = 0;
 248 +       u8 en_vlan = 0;
 249 +       u8 untag = 0;
 250 +
 251 +       for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
 252 +               u32 vid, vmsc;
 253 +               if (esw->global_vlan_enable) {
 254 +                       vid = esw->vlans[i].vid;
 255 +                       vmsc = esw->vlans[i].ports;
 256 +               } else {
 257 +                       vid = RT305X_ESW_VLAN_NONE;
 258 +                       vmsc = RT305X_ESW_PORTS_NONE;
 259 +               }
 260 +               esw_set_vlan_id(esw, i, vid);
 261 +               esw_set_vmsc(esw, i, vmsc);
 262 +       }
 263 +
 264 +       for (i = 0; i < RT305X_ESW_NUM_PORTS; i++) {
 265 +               u32 pvid;
 266 +               disable |= esw->ports[i].disable << i;
 267 +               if (esw->global_vlan_enable) {
 268 +                       doubletag |= esw->ports[i].doubletag << i;
 269 +                       en_vlan   |= 1                       << i;
 270 +                       untag     |= esw->ports[i].untag     << i;
 271 +                       pvid       = esw->ports[i].pvid;
 272 +               } else {
 273 +                       int x = esw->alt_vlan_disable ? 0 : 1;
 274 +                       doubletag |= x << i;
 275 +                       en_vlan   |= x << i;
 276 +                       untag     |= x << i;
 277 +                       pvid       = 0;
 278 +               }
 279 +               esw_set_pvid(esw, i, pvid);
 280 +               if (i < RT305X_ESW_NUM_LEDS)
 281 +                       esw_w32(esw, esw->ports[i].led,
 282 +                                     RT305X_ESW_REG_P0LED + 4*i);
 283 +       }
 284 +
 285 +       esw_set_gsc(esw);
 286 +       esw_set_port_disable(esw, disable);
 287 +       esw_rmw(esw, RT305X_ESW_REG_SGC2,
 288 +                      (RT305X_ESW_SGC2_DOUBLE_TAG_M <<
 289 +                       RT305X_ESW_SGC2_DOUBLE_TAG_S),
 290 +                      doubletag << RT305X_ESW_SGC2_DOUBLE_TAG_S);
 291 +       esw_rmw(esw, RT305X_ESW_REG_PFC1,
 292 +                      RT305X_ESW_PFC1_EN_VLAN_M << RT305X_ESW_PFC1_EN_VLAN_S,
 293 +                      en_vlan << RT305X_ESW_PFC1_EN_VLAN_S);
 294 +       esw_rmw(esw, RT305X_ESW_REG_POC2,
 295 +                      RT305X_ESW_POC2_UNTAG_EN_M << RT305X_ESW_POC2_UNTAG_EN_S,
 296 +                      untag << RT305X_ESW_POC2_UNTAG_EN_S);
 297 +
 298 +       if (!esw->global_vlan_enable) {
 299 +               /*
 300 +                * Still need to put all ports into vlan 0 or they'll be
 301 +                * isolated.
 302 +                * NOTE: vlan 0 is special, no vlan tag is prepended
 303 +                */
 304 +               esw_set_vlan_id(esw, 0, 0);
 305 +               esw_set_vmsc(esw, 0, RT305X_ESW_PORTS_ALL);
 306 +       }
 307 +
 308 +       return 0;
 309 +}
 310 +
 311 +static int esw_reset_switch(struct switch_dev *dev)
 312 +{
 313 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 314 +
 315 +       esw->global_vlan_enable = 0;
 316 +       memset(esw->ports, 0, sizeof(esw->ports));
 317 +       memset(esw->vlans, 0, sizeof(esw->vlans));
 318 +       esw_hw_init(esw);
 319 +
 320 +       return 0;
 321 +}
 322 +
 323 +static int esw_get_vlan_enable(struct switch_dev *dev,
 324 +                          const struct switch_attr *attr,
 325 +                          struct switch_val *val)
 326 +{
 327 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 328 +
 329 +       val->value.i = esw->global_vlan_enable;
 330 +
 331 +       return 0;
 332 +}
 333 +
 334 +static int esw_set_vlan_enable(struct switch_dev *dev,
 335 +                          const struct switch_attr *attr,
 336 +                          struct switch_val *val)
 337 +{
 338 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 339 +
 340 +       esw->global_vlan_enable = val->value.i != 0;
 341 +
 342 +       return 0;
 343 +}
 344 +
 345 +static int esw_get_alt_vlan_disable(struct switch_dev *dev,
 346 +                               const struct switch_attr *attr,
 347 +                               struct switch_val *val)
 348 +{
 349 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 350 +
 351 +       val->value.i = esw->alt_vlan_disable;
 352 +
 353 +       return 0;
 354 +}
 355 +
 356 +static int esw_set_alt_vlan_disable(struct switch_dev *dev,
 357 +                               const struct switch_attr *attr,
 358 +                               struct switch_val *val)
 359 +{
 360 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 361 +
 362 +       esw->alt_vlan_disable = val->value.i != 0;
 363 +
 364 +       return 0;
 365 +}
 366 +
 367 +static int
 368 +rt305x_esw_set_bc_status(struct switch_dev *dev,
 369 +                       const struct switch_attr *attr,
 370 +                       struct switch_val *val)
 371 +{
 372 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 373 +
 374 +       esw->bc_storm_protect = val->value.i & RT305X_ESW_GSC_BC_STROM_MASK;
 375 +
 376 +       return 0;
 377 +}
 378 +
 379 +static int
 380 +rt305x_esw_get_bc_status(struct switch_dev *dev,
 381 +                       const struct switch_attr *attr,
 382 +                       struct switch_val *val)
 383 +{
 384 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 385 +
 386 +       val->value.i = esw->bc_storm_protect;
 387 +
 388 +       return 0;
 389 +}
 390 +
 391 +static int
 392 +rt305x_esw_set_led_freq(struct switch_dev *dev,
 393 +                       const struct switch_attr *attr,
 394 +                       struct switch_val *val)
 395 +{
 396 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 397 +
 398 +       esw->led_frequency = val->value.i & RT305X_ESW_GSC_LED_FREQ_MASK;
 399 +
 400 +       return 0;
 401 +}
 402 +
 403 +static int
 404 +rt305x_esw_get_led_freq(struct switch_dev *dev,
 405 +                       const struct switch_attr *attr,
 406 +                       struct switch_val *val)
 407 +{
 408 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 409 +
 410 +       val->value.i = esw->led_frequency;
 411 +
 412 +       return 0;
 413 +}
 414 +
 415 +static int esw_get_port_link(struct switch_dev *dev,
 416 +                        int port,
 417 +                        struct switch_port_link *link)
 418 +{
 419 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 420 +       u32 speed, poa;
 421 +
 422 +       if (port < 0 || port >= RT305X_ESW_NUM_PORTS)
 423 +               return -EINVAL;
 424 +
 425 +       poa = esw_r32(esw, RT305X_ESW_REG_POA) >> port;
 426 +
 427 +       link->link = (poa >> RT305X_ESW_LINK_S) & 1;
 428 +       link->duplex = (poa >> RT305X_ESW_DUPLEX_S) & 1;
 429 +       if (port < RT305X_ESW_NUM_LEDS) {
 430 +               speed = (poa >> RT305X_ESW_SPD_S) & 1;
 431 +       } else {
 432 +               if (port == RT305X_ESW_NUM_PORTS - 1)
 433 +                       poa >>= 1;
 434 +               speed = (poa >> RT305X_ESW_SPD_S) & 3;
 435 +       }
 436 +       switch (speed) {
 437 +       case 0:
 438 +               link->speed = SWITCH_PORT_SPEED_10;
 439 +               break;
 440 +       case 1:
 441 +               link->speed = SWITCH_PORT_SPEED_100;
 442 +               break;
 443 +       case 2:
 444 +       case 3: /* forced gige speed can be 2 or 3 */
 445 +               link->speed = SWITCH_PORT_SPEED_1000;
 446 +               break;
 447 +       default:
 448 +               link->speed = SWITCH_PORT_SPEED_UNKNOWN;
 449 +               break;
 450 +       }
 451 +
 452 +       return 0;
 453 +}
 454 +
 455 +static int esw_get_port_bool(struct switch_dev *dev,
 456 +                        const struct switch_attr *attr,
 457 +                        struct switch_val *val)
 458 +{
 459 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 460 +       int idx = val->port_vlan;
 461 +       u32 x, reg, shift;
 462 +
 463 +       if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS)
 464 +               return -EINVAL;
 465 +
 466 +       switch (attr->id) {
 467 +       case RT305X_ESW_ATTR_PORT_DISABLE:
 468 +               reg = RT305X_ESW_REG_POC0;
 469 +               shift = RT305X_ESW_POC0_DIS_PORT_S;
 470 +               break;
 471 +       case RT305X_ESW_ATTR_PORT_DOUBLETAG:
 472 +               reg = RT305X_ESW_REG_SGC2;
 473 +               shift = RT305X_ESW_SGC2_DOUBLE_TAG_S;
 474 +               break;
 475 +       case RT305X_ESW_ATTR_PORT_UNTAG:
 476 +               reg = RT305X_ESW_REG_POC2;
 477 +               shift = RT305X_ESW_POC2_UNTAG_EN_S;
 478 +               break;
 479 +       case RT305X_ESW_ATTR_PORT_LAN:
 480 +               reg = RT305X_ESW_REG_SGC2;
 481 +               shift = RT305X_ESW_SGC2_LAN_PMAP_S;
 482 +               if (idx >= RT305X_ESW_NUM_LANWAN)
 483 +                       return -EINVAL;
 484 +               break;
 485 +       default:
 486 +               return -EINVAL;
 487 +       }
 488 +
 489 +       x = esw_r32(esw, reg);
 490 +       val->value.i = (x >> (idx + shift)) & 1;
 491 +
 492 +       return 0;
 493 +}
 494 +
 495 +static int esw_set_port_bool(struct switch_dev *dev,
 496 +                        const struct switch_attr *attr,
 497 +                        struct switch_val *val)
 498 +{
 499 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 500 +       int idx = val->port_vlan;
 501 +
 502 +       if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS ||
 503 +           val->value.i < 0 || val->value.i > 1)
 504 +               return -EINVAL;
 505 +
 506 +       switch (attr->id) {
 507 +       case RT305X_ESW_ATTR_PORT_DISABLE:
 508 +               esw->ports[idx].disable = val->value.i;
 509 +               break;
 510 +       case RT305X_ESW_ATTR_PORT_DOUBLETAG:
 511 +               esw->ports[idx].doubletag = val->value.i;
 512 +               break;
 513 +       case RT305X_ESW_ATTR_PORT_UNTAG:
 514 +               esw->ports[idx].untag = val->value.i;
 515 +               break;
 516 +       default:
 517 +               return -EINVAL;
 518 +       }
 519 +
 520 +       return 0;
 521 +}
 522 +
 523 +static int esw_get_port_recv_badgood(struct switch_dev *dev,
 524 +                                const struct switch_attr *attr,
 525 +                                struct switch_val *val)
 526 +{
 527 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 528 +       int idx = val->port_vlan;
 529 +       int shift = attr->id == RT305X_ESW_ATTR_PORT_RECV_GOOD ? 0 : 16;
 530 +       u32 reg;
 531 +
 532 +       if (idx < 0 || idx >= RT305X_ESW_NUM_LANWAN)
 533 +               return -EINVAL;
 534 +       reg = esw_r32(esw, RT305X_ESW_REG_PXPC(idx));
 535 +       val->value.i = (reg >> shift) & 0xffff;
 536 +
 537 +       return 0;
 538 +}
 539 +
 540 +static int
 541 +esw_get_port_tr_badgood(struct switch_dev *dev,
 542 +                                const struct switch_attr *attr,
 543 +                                struct switch_val *val)
 544 +{
 545 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 546 +
 547 +       int idx = val->port_vlan;
 548 +       int shift = attr->id == RT5350_ESW_ATTR_PORT_TR_GOOD ? 0 : 16;
 549 +       u32 reg;
 550 +
 551 +       if ((ralink_soc != RT305X_SOC_RT5350) && (ralink_soc != MT762X_SOC_MT7628AN) && (ralink_soc != MT762X_SOC_MT7688))
 552 +               return -EINVAL;
 553 +
 554 +       if (idx < 0 || idx >= RT305X_ESW_NUM_LANWAN)
 555 +               return -EINVAL;
 556 +
 557 +       reg = esw_r32(esw, RT5350_ESW_REG_PXTPC(idx));
 558 +       val->value.i = (reg >> shift) & 0xffff;
 559 +
 560 +       return 0;
 561 +}
 562 +
 563 +static int esw_get_port_led(struct switch_dev *dev,
 564 +                       const struct switch_attr *attr,
 565 +                       struct switch_val *val)
 566 +{
 567 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 568 +       int idx = val->port_vlan;
 569 +
 570 +       if (idx < 0 || idx >= RT305X_ESW_NUM_PORTS ||
 571 +           idx >= RT305X_ESW_NUM_LEDS)
 572 +               return -EINVAL;
 573 +
 574 +       val->value.i = esw_r32(esw, RT305X_ESW_REG_P0LED + 4*idx);
 575 +
 576 +       return 0;
 577 +}
 578 +
 579 +static int esw_set_port_led(struct switch_dev *dev,
 580 +                       const struct switch_attr *attr,
 581 +                       struct switch_val *val)
 582 +{
 583 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 584 +       int idx = val->port_vlan;
 585 +
 586 +       if (idx < 0 || idx >= RT305X_ESW_NUM_LEDS)
 587 +               return -EINVAL;
 588 +
 589 +       esw->ports[idx].led = val->value.i;
 590 +
 591 +       return 0;
 592 +}
 593 +
 594 +static int esw_get_port_pvid(struct switch_dev *dev, int port, int *val)
 595 +{
 596 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 597 +
 598 +       if (port >= RT305X_ESW_NUM_PORTS)
 599 +               return -EINVAL;
 600 +
 601 +       *val = esw_get_pvid(esw, port);
 602 +
 603 +       return 0;
 604 +}
 605 +
 606 +static int esw_set_port_pvid(struct switch_dev *dev, int port, int val)
 607 +{
 608 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 609 +
 610 +       if (port >= RT305X_ESW_NUM_PORTS)
 611 +               return -EINVAL;
 612 +
 613 +       esw->ports[port].pvid = val;
 614 +
 615 +       return 0;
 616 +}
 617 +
 618 +static int esw_get_vlan_ports(struct switch_dev *dev, struct switch_val *val)
 619 +{
 620 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 621 +       u32 vmsc, poc2;
 622 +       int vlan_idx = -1;
 623 +       int i;
 624 +
 625 +       val->len = 0;
 626 +
 627 +       if (val->port_vlan < 0 || val->port_vlan >= RT305X_ESW_NUM_VIDS)
 628 +               return -EINVAL;
 629 +
 630 +       /* valid vlan? */
 631 +       for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
 632 +               if (esw_get_vlan_id(esw, i) == val->port_vlan &&
 633 +                   esw_get_vmsc(esw, i) != RT305X_ESW_PORTS_NONE) {
 634 +                       vlan_idx = i;
 635 +                       break;
 636 +               }
 637 +       }
 638 +
 639 +       if (vlan_idx == -1)
 640 +               return -EINVAL;
 641 +
 642 +       vmsc = esw_get_vmsc(esw, vlan_idx);
 643 +       poc2 = esw_r32(esw, RT305X_ESW_REG_POC2);
 644 +
 645 +       for (i = 0; i < RT305X_ESW_NUM_PORTS; i++) {
 646 +               struct switch_port *p;
 647 +               int port_mask = 1 << i;
 648 +
 649 +               if (!(vmsc & port_mask))
 650 +                       continue;
 651 +
 652 +               p = &val->value.ports[val->len++];
 653 +               p->id = i;
 654 +               if (poc2 & (port_mask << RT305X_ESW_POC2_UNTAG_EN_S))
 655 +                       p->flags = 0;
 656 +               else
 657 +                       p->flags = 1 << SWITCH_PORT_FLAG_TAGGED;
 658 +       }
 659 +
 660 +       return 0;
 661 +}
 662 +
 663 +static int esw_set_vlan_ports(struct switch_dev *dev, struct switch_val *val)
 664 +{
 665 +       struct rt305x_esw *esw = container_of(dev, struct rt305x_esw, swdev);
 666 +       int ports;
 667 +       int vlan_idx = -1;
 668 +       int i;
 669 +
 670 +       if (val->port_vlan < 0 || val->port_vlan >= RT305X_ESW_NUM_VIDS ||
 671 +           val->len > RT305X_ESW_NUM_PORTS)
 672 +               return -EINVAL;
 673 +
 674 +       /* one of the already defined vlans? */
 675 +       for (i = 0; i < RT305X_ESW_NUM_VLANS; i++) {
 676 +               if (esw->vlans[i].vid == val->port_vlan &&
 677 +                   esw->vlans[i].ports != RT305X_ESW_PORTS_NONE) {
 678 +                       vlan_idx = i;
 679 +                       break;
 680 +               }
 681 +       }
 682 +
 683 +       /* select a free slot */
 684 +       for (i = 0; vlan_idx == -1 && i < RT305X_ESW_NUM_VLANS; i++) {
 685 +               if (esw->vlans[i].ports == RT305X_ESW_PORTS_NONE)
 686 +                       vlan_idx = i;
 687 +       }
 688 +
 689 +       /* bail if all slots are in use */
 690 +       if (vlan_idx == -1)
 691 +               return -EINVAL;
 692 +
 693 +       ports = RT305X_ESW_PORTS_NONE;
 694 +       for (i = 0; i < val->len; i++) {
 695 +               struct switch_port *p = &val->value.ports[i];
 696 +               int port_mask = 1 << p->id;
 697 +               bool untagged = !(p->flags & (1 << SWITCH_PORT_FLAG_TAGGED));
 698 +
 699 +               if (p->id >= RT305X_ESW_NUM_PORTS)
 700 +                       return -EINVAL;
 701 +
 702 +               ports |= port_mask;
 703 +               esw->ports[p->id].untag = untagged;
 704 +       }
 705 +       esw->vlans[vlan_idx].ports = ports;
 706 +       if (ports == RT305X_ESW_PORTS_NONE)
 707 +               esw->vlans[vlan_idx].vid = RT305X_ESW_VLAN_NONE;
 708 +       else
 709 +               esw->vlans[vlan_idx].vid = val->port_vlan;
 710 +
 711 +       return 0;
 712 +}
 713 +
 714 +static const struct switch_attr esw_global[] = {
 715 +       {
 716 +               .type = SWITCH_TYPE_INT,
 717 +               .name = "enable_vlan",
 718 +               .description = "VLAN mode (1:enabled)",
 719 +               .max = 1,
 720 +               .id = RT305X_ESW_ATTR_ENABLE_VLAN,
 721 +               .get = esw_get_vlan_enable,
 722 +               .set = esw_set_vlan_enable,
 723 +       },
 724 +       {
 725 +               .type = SWITCH_TYPE_INT,
 726 +               .name = "alternate_vlan_disable",
 727 +               .description = "Use en_vlan instead of doubletag to disable"
 728 +                               " VLAN mode",
 729 +               .max = 1,
 730 +               .id = RT305X_ESW_ATTR_ALT_VLAN_DISABLE,
 731 +               .get = esw_get_alt_vlan_disable,
 732 +               .set = esw_set_alt_vlan_disable,
 733 +       },
 734 +       {
 735 +               .type = SWITCH_TYPE_INT,
 736 +               .name = "bc_storm_protect",
 737 +               .description = "Global broadcast storm protection (0:Disable, 1:64 blocks, 2:96 blocks, 3:128 blocks)",
 738 +               .max = 3,
 739 +               .id = RT305X_ESW_ATTR_BC_STATUS,
 740 +               .get = rt305x_esw_get_bc_status,
 741 +               .set = rt305x_esw_set_bc_status,
 742 +       },
 743 +       {
 744 +               .type = SWITCH_TYPE_INT,
 745 +               .name = "led_frequency",
 746 +               .description = "LED Flash frequency (0:30mS, 1:60mS, 2:240mS, 3:480mS)",
 747 +               .max = 3,
 748 +               .id = RT305X_ESW_ATTR_LED_FREQ,
 749 +               .get = rt305x_esw_get_led_freq,
 750 +               .set = rt305x_esw_set_led_freq,
 751 +       }
 752 +};
 753 +
 754 +static const struct switch_attr esw_port[] = {
 755 +       {
 756 +               .type = SWITCH_TYPE_INT,
 757 +               .name = "disable",
 758 +               .description = "Port state (1:disabled)",
 759 +               .max = 1,
 760 +               .id = RT305X_ESW_ATTR_PORT_DISABLE,
 761 +               .get = esw_get_port_bool,
 762 +               .set = esw_set_port_bool,
 763 +       },
 764 +       {
 765 +               .type = SWITCH_TYPE_INT,
 766 +               .name = "doubletag",
 767 +               .description = "Double tagging for incoming vlan packets "
 768 +                               "(1:enabled)",
 769 +               .max = 1,
 770 +               .id = RT305X_ESW_ATTR_PORT_DOUBLETAG,
 771 +               .get = esw_get_port_bool,
 772 +               .set = esw_set_port_bool,
 773 +       },
 774 +       {
 775 +               .type = SWITCH_TYPE_INT,
 776 +               .name = "untag",
 777 +               .description = "Untag (1:strip outgoing vlan tag)",
 778 +               .max = 1,
 779 +               .id = RT305X_ESW_ATTR_PORT_UNTAG,
 780 +               .get = esw_get_port_bool,
 781 +               .set = esw_set_port_bool,
 782 +       },
 783 +       {
 784 +               .type = SWITCH_TYPE_INT,
 785 +               .name = "led",
 786 +               .description = "LED mode (0:link, 1:100m, 2:duplex, 3:activity,"
 787 +                               " 4:collision, 5:linkact, 6:duplcoll, 7:10mact,"
 788 +                               " 8:100mact, 10:blink, 11:off, 12:on)",
 789 +               .max = 15,
 790 +               .id = RT305X_ESW_ATTR_PORT_LED,
 791 +               .get = esw_get_port_led,
 792 +               .set = esw_set_port_led,
 793 +       },
 794 +       {
 795 +               .type = SWITCH_TYPE_INT,
 796 +               .name = "lan",
 797 +               .description = "HW port group (0:wan, 1:lan)",
 798 +               .max = 1,
 799 +               .id = RT305X_ESW_ATTR_PORT_LAN,
 800 +               .get = esw_get_port_bool,
 801 +       },
 802 +       {
 803 +               .type = SWITCH_TYPE_INT,
 804 +               .name = "recv_bad",
 805 +               .description = "Receive bad packet counter",
 806 +               .id = RT305X_ESW_ATTR_PORT_RECV_BAD,
 807 +               .get = esw_get_port_recv_badgood,
 808 +       },
 809 +       {
 810 +               .type = SWITCH_TYPE_INT,
 811 +               .name = "recv_good",
 812 +               .description = "Receive good packet counter",
 813 +               .id = RT305X_ESW_ATTR_PORT_RECV_GOOD,
 814 +               .get = esw_get_port_recv_badgood,
 815 +       },
 816 +       {
 817 +               .type = SWITCH_TYPE_INT,
 818 +               .name = "tr_bad",
 819 +
 820 +               .description = "Transmit bad packet counter. rt5350 only",
 821 +               .id = RT5350_ESW_ATTR_PORT_TR_BAD,
 822 +               .get = esw_get_port_tr_badgood,
 823 +       },
 824 +       {
 825 +               .type = SWITCH_TYPE_INT,
 826 +               .name = "tr_good",
 827 +
 828 +               .description = "Transmit good packet counter. rt5350 only",
 829 +               .id = RT5350_ESW_ATTR_PORT_TR_GOOD,
 830 +               .get = esw_get_port_tr_badgood,
 831 +       },
 832 +};
 833 +
 834 +static const struct switch_attr esw_vlan[] = {
 835 +};
 836 +
 837 +static const struct switch_dev_ops esw_ops = {
 838 +       .attr_global = {
 839 +               .attr = esw_global,
 840 +               .n_attr = ARRAY_SIZE(esw_global),
 841 +       },
 842 +       .attr_port = {
 843 +               .attr = esw_port,
 844 +               .n_attr = ARRAY_SIZE(esw_port),
 845 +       },
 846 +       .attr_vlan = {
 847 +               .attr = esw_vlan,
 848 +               .n_attr = ARRAY_SIZE(esw_vlan),
 849 +       },
 850 +       .get_vlan_ports = esw_get_vlan_ports,
 851 +       .set_vlan_ports = esw_set_vlan_ports,
 852 +       .get_port_pvid = esw_get_port_pvid,
 853 +       .set_port_pvid = esw_set_port_pvid,
 854 +       .get_port_link = esw_get_port_link,
 855 +       .apply_config = esw_apply_config,
 856 +       .reset_switch = esw_reset_switch,
 857 +};
 858 +
 859  static int esw_probe(struct platform_device *pdev)
 860  {
 861         struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 862         struct device_node *np = pdev->dev.of_node;
 863         const __be32 *port_map, *reg_init;
 864 +       struct switch_dev *swdev;
 865         struct rt305x_esw *esw;
 866         struct resource *irq;
 867         int ret;
 868 @@ -568,6 +1353,21 @@ static int esw_probe(struct platform_dev
 869         if (reg_init)
 870                 esw->reg_led_polarity = be32_to_cpu(*reg_init);
 871
 872 +       swdev = &esw->swdev;
 873 +       swdev->of_node = pdev->dev.of_node;
 874 +       swdev->name = "rt305x-esw";
 875 +       swdev->alias = "rt305x";
 876 +       swdev->cpu_port = RT305X_ESW_PORT6;
 877 +       swdev->ports = RT305X_ESW_NUM_PORTS;
 878 +       swdev->vlans = RT305X_ESW_NUM_VIDS;
 879 +       swdev->ops = &esw_ops;
 880 +
 881 +       ret = register_switch(swdev, NULL);
 882 +       if (ret < 0) {
 883 +               dev_err(&pdev->dev, "register_switch failed\n");
 884 +               goto unmap_base;
 885 +       }
 886 +
 887         platform_set_drvdata(pdev, esw);
 888
 889         spin_lock_init(&esw->reg_rw_lock);
 890 @@ -583,6 +1383,11 @@ static int esw_probe(struct platform_dev
 891         }
 892
 893         return ret;
 894 +
 895 +unmap_base:
 896 +       iounmap(esw->base);
 897 +       kfree(esw);
 898 +       return ret;
 899  }
 900
 901  static int esw_remove(struct platform_device *pdev)