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