drivers/net/dsa/xrs700x/xrs700x.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2020 NovaTech LLC
   4  * George McCollister <[email protected]>
   5  */
   6
   7 #include <net/dsa.h>
   8 #include <linux/if_bridge.h>
   9 #include <linux/of_device.h>
  10 #include <linux/netdev_features.h>
  11 #include <linux/if_hsr.h>
  12 #include "xrs700x.h"
  13 #include "xrs700x_reg.h"
  14
  15 #define XRS700X_MIB_INTERVAL msecs_to_jiffies(3000)
  16
  17 #define XRS7000X_SUPPORTED_HSR_FEATURES \
  18         (NETIF_F_HW_HSR_TAG_INS | NETIF_F_HW_HSR_TAG_RM | \
  19          NETIF_F_HW_HSR_FWD | NETIF_F_HW_HSR_DUP)
  20
  21 #define XRS7003E_ID     0x100
  22 #define XRS7003F_ID     0x101
  23 #define XRS7004E_ID     0x200
  24 #define XRS7004F_ID     0x201
  25
  26 const struct xrs700x_info xrs7003e_info = {XRS7003E_ID, "XRS7003E", 3};
  27 EXPORT_SYMBOL(xrs7003e_info);
  28
  29 const struct xrs700x_info xrs7003f_info = {XRS7003F_ID, "XRS7003F", 3};
  30 EXPORT_SYMBOL(xrs7003f_info);
  31
  32 const struct xrs700x_info xrs7004e_info = {XRS7004E_ID, "XRS7004E", 4};
  33 EXPORT_SYMBOL(xrs7004e_info);
  34
  35 const struct xrs700x_info xrs7004f_info = {XRS7004F_ID, "XRS7004F", 4};
  36 EXPORT_SYMBOL(xrs7004f_info);
  37
  38 struct xrs700x_regfield {
  39         struct reg_field rf;
  40         struct regmap_field **rmf;
  41 };
  42
  43 struct xrs700x_mib {
  44         unsigned int offset;
  45         const char *name;
  46         int stats64_offset;
  47 };
  48
  49 #define XRS700X_MIB_ETHTOOL_ONLY(o, n) {o, n, -1}
  50 #define XRS700X_MIB(o, n, m) {o, n, offsetof(struct rtnl_link_stats64, m)}
  51
  52 static const struct xrs700x_mib xrs700x_mibs[] = {
  53         XRS700X_MIB(XRS_RX_GOOD_OCTETS_L, "rx_good_octets", rx_bytes),
  54         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_BAD_OCTETS_L, "rx_bad_octets"),
  55         XRS700X_MIB(XRS_RX_UNICAST_L, "rx_unicast", rx_packets),
  56         XRS700X_MIB(XRS_RX_BROADCAST_L, "rx_broadcast", rx_packets),
  57         XRS700X_MIB(XRS_RX_MULTICAST_L, "rx_multicast", multicast),
  58         XRS700X_MIB(XRS_RX_UNDERSIZE_L, "rx_undersize", rx_length_errors),
  59         XRS700X_MIB(XRS_RX_FRAGMENTS_L, "rx_fragments", rx_length_errors),
  60         XRS700X_MIB(XRS_RX_OVERSIZE_L, "rx_oversize", rx_length_errors),
  61         XRS700X_MIB(XRS_RX_JABBER_L, "rx_jabber", rx_length_errors),
  62         XRS700X_MIB(XRS_RX_ERR_L, "rx_err", rx_errors),
  63         XRS700X_MIB(XRS_RX_CRC_L, "rx_crc", rx_crc_errors),
  64         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_64_L, "rx_64"),
  65         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_65_127_L, "rx_65_127"),
  66         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_128_255_L, "rx_128_255"),
  67         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_256_511_L, "rx_256_511"),
  68         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_512_1023_L, "rx_512_1023"),
  69         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_1024_1536_L, "rx_1024_1536"),
  70         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_HSR_PRP_L, "rx_hsr_prp"),
  71         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_WRONGLAN_L, "rx_wronglan"),
  72         XRS700X_MIB_ETHTOOL_ONLY(XRS_RX_DUPLICATE_L, "rx_duplicate"),
  73         XRS700X_MIB(XRS_TX_OCTETS_L, "tx_octets", tx_bytes),
  74         XRS700X_MIB(XRS_TX_UNICAST_L, "tx_unicast", tx_packets),
  75         XRS700X_MIB(XRS_TX_BROADCAST_L, "tx_broadcast", tx_packets),
  76         XRS700X_MIB(XRS_TX_MULTICAST_L, "tx_multicast", tx_packets),
  77         XRS700X_MIB_ETHTOOL_ONLY(XRS_TX_HSR_PRP_L, "tx_hsr_prp"),
  78         XRS700X_MIB(XRS_PRIQ_DROP_L, "priq_drop", tx_dropped),
  79         XRS700X_MIB(XRS_EARLY_DROP_L, "early_drop", tx_dropped),
  80 };
  81
  82 static void xrs700x_get_strings(struct dsa_switch *ds, int port,
  83                                 u32 stringset, u8 *data)
  84 {
  85         int i;
  86
  87         if (stringset != ETH_SS_STATS)
  88                 return;
  89
  90         for (i = 0; i < ARRAY_SIZE(xrs700x_mibs); i++) {
  91                 strscpy(data, xrs700x_mibs[i].name, ETH_GSTRING_LEN);
  92                 data += ETH_GSTRING_LEN;
  93         }
  94 }
  95
  96 static int xrs700x_get_sset_count(struct dsa_switch *ds, int port, int sset)
  97 {
  98         if (sset != ETH_SS_STATS)
  99                 return -EOPNOTSUPP;
 100
 101         return ARRAY_SIZE(xrs700x_mibs);
 102 }
 103
 104 static void xrs700x_read_port_counters(struct xrs700x *priv, int port)
 105 {
 106         struct xrs700x_port *p = &priv->ports[port];
 107         struct rtnl_link_stats64 stats;
 108         int i;
 109
 110         memset(&stats, 0, sizeof(stats));
 111
 112         mutex_lock(&p->mib_mutex);
 113
 114         /* Capture counter values */
 115         regmap_write(priv->regmap, XRS_CNT_CTRL(port), 1);
 116
 117         for (i = 0; i < ARRAY_SIZE(xrs700x_mibs); i++) {
 118                 unsigned int high = 0, low = 0, reg;
 119
 120                 reg = xrs700x_mibs[i].offset + XRS_PORT_OFFSET * port;
 121                 regmap_read(priv->regmap, reg, &low);
 122                 regmap_read(priv->regmap, reg + 2, &high);
 123
 124                 p->mib_data[i] += (high << 16) | low;
 125
 126                 if (xrs700x_mibs[i].stats64_offset >= 0) {
 127                         u8 *s = (u8 *)&stats + xrs700x_mibs[i].stats64_offset;
 128                         *(u64 *)s += p->mib_data[i];
 129                 }
 130         }
 131
 132         /* multicast must be added to rx_packets (which already includes
 133          * unicast and broadcast)
 134          */
 135         stats.rx_packets += stats.multicast;
 136
 137         u64_stats_update_begin(&p->syncp);
 138         p->stats64 = stats;
 139         u64_stats_update_end(&p->syncp);
 140
 141         mutex_unlock(&p->mib_mutex);
 142 }
 143
 144 static void xrs700x_mib_work(struct work_struct *work)
 145 {
 146         struct xrs700x *priv = container_of(work, struct xrs700x,
 147                                             mib_work.work);
 148         int i;
 149
 150         for (i = 0; i < priv->ds->num_ports; i++)
 151                 xrs700x_read_port_counters(priv, i);
 152
 153         schedule_delayed_work(&priv->mib_work, XRS700X_MIB_INTERVAL);
 154 }
 155
 156 static void xrs700x_get_ethtool_stats(struct dsa_switch *ds, int port,
 157                                       u64 *data)
 158 {
 159         struct xrs700x *priv = ds->priv;
 160         struct xrs700x_port *p = &priv->ports[port];
 161
 162         xrs700x_read_port_counters(priv, port);
 163
 164         mutex_lock(&p->mib_mutex);
 165         memcpy(data, p->mib_data, sizeof(*data) * ARRAY_SIZE(xrs700x_mibs));
 166         mutex_unlock(&p->mib_mutex);
 167 }
 168
 169 static void xrs700x_get_stats64(struct dsa_switch *ds, int port,
 170                                 struct rtnl_link_stats64 *s)
 171 {
 172         struct xrs700x *priv = ds->priv;
 173         struct xrs700x_port *p = &priv->ports[port];
 174         unsigned int start;
 175
 176         do {
 177                 start = u64_stats_fetch_begin(&p->syncp);
 178                 *s = p->stats64;
 179         } while (u64_stats_fetch_retry(&p->syncp, start));
 180 }
 181
 182 static int xrs700x_setup_regmap_range(struct xrs700x *priv)
 183 {
 184         struct xrs700x_regfield regfields[] = {
 185                 {
 186                         .rf = REG_FIELD_ID(XRS_PORT_STATE(0), 0, 1,
 187                                            priv->ds->num_ports,
 188                                            XRS_PORT_OFFSET),
 189                         .rmf = &priv->ps_forward
 190                 },
 191                 {
 192                         .rf = REG_FIELD_ID(XRS_PORT_STATE(0), 2, 3,
 193                                            priv->ds->num_ports,
 194                                            XRS_PORT_OFFSET),
 195                         .rmf = &priv->ps_management
 196                 },
 197                 {
 198                         .rf = REG_FIELD_ID(XRS_PORT_STATE(0), 4, 9,
 199                                            priv->ds->num_ports,
 200                                            XRS_PORT_OFFSET),
 201                         .rmf = &priv->ps_sel_speed
 202                 },
 203                 {
 204                         .rf = REG_FIELD_ID(XRS_PORT_STATE(0), 10, 11,
 205                                            priv->ds->num_ports,
 206                                            XRS_PORT_OFFSET),
 207                         .rmf = &priv->ps_cur_speed
 208                 }
 209         };
 210         int i = 0;
 211
 212         for (; i < ARRAY_SIZE(regfields); i++) {
 213                 *regfields[i].rmf = devm_regmap_field_alloc(priv->dev,
 214                                                             priv->regmap,
 215                                                             regfields[i].rf);
 216                 if (IS_ERR(*regfields[i].rmf))
 217                         return PTR_ERR(*regfields[i].rmf);
 218         }
 219
 220         return 0;
 221 }
 222
 223 static enum dsa_tag_protocol xrs700x_get_tag_protocol(struct dsa_switch *ds,
 224                                                       int port,
 225                                                       enum dsa_tag_protocol m)
 226 {
 227         return DSA_TAG_PROTO_XRS700X;
 228 }
 229
 230 static int xrs700x_reset(struct dsa_switch *ds)
 231 {
 232         struct xrs700x *priv = ds->priv;
 233         unsigned int val;
 234         int ret;
 235
 236         ret = regmap_write(priv->regmap, XRS_GENERAL, XRS_GENERAL_RESET);
 237         if (ret)
 238                 goto error;
 239
 240         ret = regmap_read_poll_timeout(priv->regmap, XRS_GENERAL,
 241                                        val, !(val & XRS_GENERAL_RESET),
 242                                        10, 1000);
 243 error:
 244         if (ret) {
 245                 dev_err_ratelimited(priv->dev, "error resetting switch: %d\n",
 246                                     ret);
 247         }
 248
 249         return ret;
 250 }
 251
 252 static void xrs700x_port_stp_state_set(struct dsa_switch *ds, int port,
 253                                        u8 state)
 254 {
 255         struct xrs700x *priv = ds->priv;
 256         unsigned int bpdus = 1;
 257         unsigned int val;
 258
 259         switch (state) {
 260         case BR_STATE_DISABLED:
 261                 bpdus = 0;
 262                 fallthrough;
 263         case BR_STATE_BLOCKING:
 264         case BR_STATE_LISTENING:
 265                 val = XRS_PORT_DISABLED;
 266                 break;
 267         case BR_STATE_LEARNING:
 268                 val = XRS_PORT_LEARNING;
 269                 break;
 270         case BR_STATE_FORWARDING:
 271                 val = XRS_PORT_FORWARDING;
 272                 break;
 273         default:
 274                 dev_err(ds->dev, "invalid STP state: %d\n", state);
 275                 return;
 276         }
 277
 278         regmap_fields_write(priv->ps_forward, port, val);
 279
 280         /* Enable/disable inbound policy added by xrs700x_port_add_bpdu_ipf()
 281          * which allows BPDU forwarding to the CPU port when the front facing
 282          * port is in disabled/learning state.
 283          */
 284         regmap_update_bits(priv->regmap, XRS_ETH_ADDR_CFG(port, 0), 1, bpdus);
 285
 286         dev_dbg_ratelimited(priv->dev, "%s - port: %d, state: %u, val: 0x%x\n",
 287                             __func__, port, state, val);
 288 }
 289
 290 /* Add an inbound policy filter which matches the BPDU destination MAC
 291  * and forwards to the CPU port. Leave the policy disabled, it will be
 292  * enabled as needed.
 293  */
 294 static int xrs700x_port_add_bpdu_ipf(struct dsa_switch *ds, int port)
 295 {
 296         struct xrs700x *priv = ds->priv;
 297         unsigned int val = 0;
 298         int i = 0;
 299         int ret;
 300
 301         /* Compare all 48 bits of the destination MAC address. */
 302         ret = regmap_write(priv->regmap, XRS_ETH_ADDR_CFG(port, 0), 48 << 2);
 303         if (ret)
 304                 return ret;
 305
 306         /* match BPDU destination 01:80:c2:00:00:00 */
 307         for (i = 0; i < sizeof(eth_stp_addr); i += 2) {
 308                 ret = regmap_write(priv->regmap, XRS_ETH_ADDR_0(port, 0) + i,
 309                                    eth_stp_addr[i] |
 310                                    (eth_stp_addr[i + 1] << 8));
 311                 if (ret)
 312                         return ret;
 313         }
 314
 315         /* Mirror BPDU to CPU port */
 316         for (i = 0; i < ds->num_ports; i++) {
 317                 if (dsa_is_cpu_port(ds, i))
 318                         val |= BIT(i);
 319         }
 320
 321         ret = regmap_write(priv->regmap, XRS_ETH_ADDR_FWD_MIRROR(port, 0), val);
 322         if (ret)
 323                 return ret;
 324
 325         ret = regmap_write(priv->regmap, XRS_ETH_ADDR_FWD_ALLOW(port, 0), 0);
 326         if (ret)
 327                 return ret;
 328
 329         return 0;
 330 }
 331
 332 static int xrs700x_port_setup(struct dsa_switch *ds, int port)
 333 {
 334         bool cpu_port = dsa_is_cpu_port(ds, port);
 335         struct xrs700x *priv = ds->priv;
 336         unsigned int val = 0;
 337         int ret, i;
 338
 339         xrs700x_port_stp_state_set(ds, port, BR_STATE_DISABLED);
 340
 341         /* Disable forwarding to non-CPU ports */
 342         for (i = 0; i < ds->num_ports; i++) {
 343                 if (!dsa_is_cpu_port(ds, i))
 344                         val |= BIT(i);
 345         }
 346
 347         /* 1 = Disable forwarding to the port */
 348         ret = regmap_write(priv->regmap, XRS_PORT_FWD_MASK(port), val);
 349         if (ret)
 350                 return ret;
 351
 352         val = cpu_port ? XRS_PORT_MODE_MANAGEMENT : XRS_PORT_MODE_NORMAL;
 353         ret = regmap_fields_write(priv->ps_management, port, val);
 354         if (ret)
 355                 return ret;
 356
 357         if (!cpu_port) {
 358                 ret = xrs700x_port_add_bpdu_ipf(ds, port);
 359                 if (ret)
 360                         return ret;
 361         }
 362
 363         return 0;
 364 }
 365
 366 static int xrs700x_setup(struct dsa_switch *ds)
 367 {
 368         struct xrs700x *priv = ds->priv;
 369         int ret, i;
 370
 371         ret = xrs700x_reset(ds);
 372         if (ret)
 373                 return ret;
 374
 375         for (i = 0; i < ds->num_ports; i++) {
 376                 ret = xrs700x_port_setup(ds, i);
 377                 if (ret)
 378                         return ret;
 379         }
 380
 381         schedule_delayed_work(&priv->mib_work, XRS700X_MIB_INTERVAL);
 382
 383         return 0;
 384 }
 385
 386 static void xrs700x_teardown(struct dsa_switch *ds)
 387 {
 388         struct xrs700x *priv = ds->priv;
 389
 390         cancel_delayed_work_sync(&priv->mib_work);
 391 }
 392
 393 static void xrs700x_phylink_validate(struct dsa_switch *ds, int port,
 394                                      unsigned long *supported,
 395                                      struct phylink_link_state *state)
 396 {
 397         __ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
 398
 399         switch (port) {
 400         case 0:
 401                 break;
 402         case 1:
 403         case 2:
 404         case 3:
 405                 phylink_set(mask, 1000baseT_Full);
 406                 break;
 407         default:
 408                 bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
 409                 dev_err(ds->dev, "Unsupported port: %i\n", port);
 410                 return;
 411         }
 412
 413         phylink_set_port_modes(mask);
 414
 415         /* The switch only supports full duplex. */
 416         phylink_set(mask, 10baseT_Full);
 417         phylink_set(mask, 100baseT_Full);
 418
 419         bitmap_and(supported, supported, mask,
 420                    __ETHTOOL_LINK_MODE_MASK_NBITS);
 421         bitmap_and(state->advertising, state->advertising, mask,
 422                    __ETHTOOL_LINK_MODE_MASK_NBITS);
 423 }
 424
 425 static void xrs700x_mac_link_up(struct dsa_switch *ds, int port,
 426                                 unsigned int mode, phy_interface_t interface,
 427                                 struct phy_device *phydev,
 428                                 int speed, int duplex,
 429                                 bool tx_pause, bool rx_pause)
 430 {
 431         struct xrs700x *priv = ds->priv;
 432         unsigned int val;
 433
 434         switch (speed) {
 435         case SPEED_1000:
 436                 val = XRS_PORT_SPEED_1000;
 437                 break;
 438         case SPEED_100:
 439                 val = XRS_PORT_SPEED_100;
 440                 break;
 441         case SPEED_10:
 442                 val = XRS_PORT_SPEED_10;
 443                 break;
 444         default:
 445                 return;
 446         }
 447
 448         regmap_fields_write(priv->ps_sel_speed, port, val);
 449
 450         dev_dbg_ratelimited(priv->dev, "%s: port: %d mode: %u speed: %u\n",
 451                             __func__, port, mode, speed);
 452 }
 453
 454 static int xrs700x_bridge_common(struct dsa_switch *ds, int port,
 455                                  struct net_device *bridge, bool join)
 456 {
 457         unsigned int i, cpu_mask = 0, mask = 0;
 458         struct xrs700x *priv = ds->priv;
 459         int ret;
 460
 461         for (i = 0; i < ds->num_ports; i++) {
 462                 if (dsa_is_cpu_port(ds, i))
 463                         continue;
 464
 465                 cpu_mask |= BIT(i);
 466
 467                 if (dsa_to_port(ds, i)->bridge_dev == bridge)
 468                         continue;
 469
 470                 mask |= BIT(i);
 471         }
 472
 473         for (i = 0; i < ds->num_ports; i++) {
 474                 if (dsa_to_port(ds, i)->bridge_dev != bridge)
 475                         continue;
 476
 477                 /* 1 = Disable forwarding to the port */
 478                 ret = regmap_write(priv->regmap, XRS_PORT_FWD_MASK(i), mask);
 479                 if (ret)
 480                         return ret;
 481         }
 482
 483         if (!join) {
 484                 ret = regmap_write(priv->regmap, XRS_PORT_FWD_MASK(port),
 485                                    cpu_mask);
 486                 if (ret)
 487                         return ret;
 488         }
 489
 490         return 0;
 491 }
 492
 493 static int xrs700x_bridge_join(struct dsa_switch *ds, int port,
 494                                struct net_device *bridge)
 495 {
 496         return xrs700x_bridge_common(ds, port, bridge, true);
 497 }
 498
 499 static void xrs700x_bridge_leave(struct dsa_switch *ds, int port,
 500                                  struct net_device *bridge)
 501 {
 502         xrs700x_bridge_common(ds, port, bridge, false);
 503 }
 504
 505 static int xrs700x_hsr_join(struct dsa_switch *ds, int port,
 506                             struct net_device *hsr)
 507 {
 508         unsigned int val = XRS_HSR_CFG_HSR_PRP;
 509         struct dsa_port *partner = NULL, *dp;
 510         struct xrs700x *priv = ds->priv;
 511         struct net_device *slave;
 512         int ret, i, hsr_pair[2];
 513         enum hsr_version ver;
 514
 515         ret = hsr_get_version(hsr, &ver);
 516         if (ret)
 517                 return ret;
 518
 519         /* Only ports 1 and 2 can be HSR/PRP redundant ports. */
 520         if (port != 1 && port != 2)
 521                 return -EOPNOTSUPP;
 522
 523         if (ver == HSR_V1)
 524                 val |= XRS_HSR_CFG_HSR;
 525         else if (ver == PRP_V1)
 526                 val |= XRS_HSR_CFG_PRP;
 527         else
 528                 return -EOPNOTSUPP;
 529
 530         dsa_hsr_foreach_port(dp, ds, hsr) {
 531                 if (dp->index != port) {
 532                         partner = dp;
 533                         break;
 534                 }
 535         }
 536
 537         /* We can't enable redundancy on the switch until both
 538          * redundant ports have signed up.
 539          */
 540         if (!partner)
 541                 return 0;
 542
 543         regmap_fields_write(priv->ps_forward, partner->index,
 544                             XRS_PORT_DISABLED);
 545         regmap_fields_write(priv->ps_forward, port, XRS_PORT_DISABLED);
 546
 547         regmap_write(priv->regmap, XRS_HSR_CFG(partner->index),
 548                      val | XRS_HSR_CFG_LANID_A);
 549         regmap_write(priv->regmap, XRS_HSR_CFG(port),
 550                      val | XRS_HSR_CFG_LANID_B);
 551
 552         /* Clear bits for both redundant ports (HSR only) and the CPU port to
 553          * enable forwarding.
 554          */
 555         val = GENMASK(ds->num_ports - 1, 0);
 556         if (ver == HSR_V1) {
 557                 val &= ~BIT(partner->index);
 558                 val &= ~BIT(port);
 559         }
 560         val &= ~BIT(dsa_upstream_port(ds, port));
 561         regmap_write(priv->regmap, XRS_PORT_FWD_MASK(partner->index), val);
 562         regmap_write(priv->regmap, XRS_PORT_FWD_MASK(port), val);
 563
 564         regmap_fields_write(priv->ps_forward, partner->index,
 565                             XRS_PORT_FORWARDING);
 566         regmap_fields_write(priv->ps_forward, port, XRS_PORT_FORWARDING);
 567
 568         hsr_pair[0] = port;
 569         hsr_pair[1] = partner->index;
 570         for (i = 0; i < ARRAY_SIZE(hsr_pair); i++) {
 571                 slave = dsa_to_port(ds, hsr_pair[i])->slave;
 572                 slave->features |= XRS7000X_SUPPORTED_HSR_FEATURES;
 573         }
 574
 575         return 0;
 576 }
 577
 578 static int xrs700x_hsr_leave(struct dsa_switch *ds, int port,
 579                              struct net_device *hsr)
 580 {
 581         struct dsa_port *partner = NULL, *dp;
 582         struct xrs700x *priv = ds->priv;
 583         struct net_device *slave;
 584         int i, hsr_pair[2];
 585         unsigned int val;
 586
 587         dsa_hsr_foreach_port(dp, ds, hsr) {
 588                 if (dp->index != port) {
 589                         partner = dp;
 590                         break;
 591                 }
 592         }
 593
 594         if (!partner)
 595                 return 0;
 596
 597         regmap_fields_write(priv->ps_forward, partner->index,
 598                             XRS_PORT_DISABLED);
 599         regmap_fields_write(priv->ps_forward, port, XRS_PORT_DISABLED);
 600
 601         regmap_write(priv->regmap, XRS_HSR_CFG(partner->index), 0);
 602         regmap_write(priv->regmap, XRS_HSR_CFG(port), 0);
 603
 604         /* Clear bit for the CPU port to enable forwarding. */
 605         val = GENMASK(ds->num_ports - 1, 0);
 606         val &= ~BIT(dsa_upstream_port(ds, port));
 607         regmap_write(priv->regmap, XRS_PORT_FWD_MASK(partner->index), val);
 608         regmap_write(priv->regmap, XRS_PORT_FWD_MASK(port), val);
 609
 610         regmap_fields_write(priv->ps_forward, partner->index,
 611                             XRS_PORT_FORWARDING);
 612         regmap_fields_write(priv->ps_forward, port, XRS_PORT_FORWARDING);
 613
 614         hsr_pair[0] = port;
 615         hsr_pair[1] = partner->index;
 616         for (i = 0; i < ARRAY_SIZE(hsr_pair); i++) {
 617                 slave = dsa_to_port(ds, hsr_pair[i])->slave;
 618                 slave->features &= ~XRS7000X_SUPPORTED_HSR_FEATURES;
 619         }
 620
 621         return 0;
 622 }
 623
 624 static const struct dsa_switch_ops xrs700x_ops = {
 625         .get_tag_protocol       = xrs700x_get_tag_protocol,
 626         .setup                  = xrs700x_setup,
 627         .teardown               = xrs700x_teardown,
 628         .port_stp_state_set     = xrs700x_port_stp_state_set,
 629         .phylink_validate       = xrs700x_phylink_validate,
 630         .phylink_mac_link_up    = xrs700x_mac_link_up,
 631         .get_strings            = xrs700x_get_strings,
 632         .get_sset_count         = xrs700x_get_sset_count,
 633         .get_ethtool_stats      = xrs700x_get_ethtool_stats,
 634         .get_stats64            = xrs700x_get_stats64,
 635         .port_bridge_join       = xrs700x_bridge_join,
 636         .port_bridge_leave      = xrs700x_bridge_leave,
 637         .port_hsr_join          = xrs700x_hsr_join,
 638         .port_hsr_leave         = xrs700x_hsr_leave,
 639 };
 640
 641 static int xrs700x_detect(struct xrs700x *priv)
 642 {
 643         const struct xrs700x_info *info;
 644         unsigned int id;
 645         int ret;
 646
 647         ret = regmap_read(priv->regmap, XRS_DEV_ID0, &id);
 648         if (ret) {
 649                 dev_err(priv->dev, "error %d while reading switch id.\n",
 650                         ret);
 651                 return ret;
 652         }
 653
 654         info = of_device_get_match_data(priv->dev);
 655         if (!info)
 656                 return -EINVAL;
 657
 658         if (info->id == id) {
 659                 priv->ds->num_ports = info->num_ports;
 660                 dev_info(priv->dev, "%s detected.\n", info->name);
 661                 return 0;
 662         }
 663
 664         dev_err(priv->dev, "expected switch id 0x%x but found 0x%x.\n",
 665                 info->id, id);
 666
 667         return -ENODEV;
 668 }
 669
 670 struct xrs700x *xrs700x_switch_alloc(struct device *base, void *devpriv)
 671 {
 672         struct dsa_switch *ds;
 673         struct xrs700x *priv;
 674
 675         ds = devm_kzalloc(base, sizeof(*ds), GFP_KERNEL);
 676         if (!ds)
 677                 return NULL;
 678
 679         ds->dev = base;
 680
 681         priv = devm_kzalloc(base, sizeof(*priv), GFP_KERNEL);
 682         if (!priv)
 683                 return NULL;
 684
 685         INIT_DELAYED_WORK(&priv->mib_work, xrs700x_mib_work);
 686
 687         ds->ops = &xrs700x_ops;
 688         ds->priv = priv;
 689         priv->dev = base;
 690
 691         priv->ds = ds;
 692         priv->priv = devpriv;
 693
 694         return priv;
 695 }
 696 EXPORT_SYMBOL(xrs700x_switch_alloc);
 697
 698 static int xrs700x_alloc_port_mib(struct xrs700x *priv, int port)
 699 {
 700         struct xrs700x_port *p = &priv->ports[port];
 701
 702         p->mib_data = devm_kcalloc(priv->dev, ARRAY_SIZE(xrs700x_mibs),
 703                                    sizeof(*p->mib_data), GFP_KERNEL);
 704         if (!p->mib_data)
 705                 return -ENOMEM;
 706
 707         mutex_init(&p->mib_mutex);
 708         u64_stats_init(&p->syncp);
 709
 710         return 0;
 711 }
 712
 713 int xrs700x_switch_register(struct xrs700x *priv)
 714 {
 715         int ret;
 716         int i;
 717
 718         ret = xrs700x_detect(priv);
 719         if (ret)
 720                 return ret;
 721
 722         ret = xrs700x_setup_regmap_range(priv);
 723         if (ret)
 724                 return ret;
 725
 726         priv->ports = devm_kcalloc(priv->dev, priv->ds->num_ports,
 727                                    sizeof(*priv->ports), GFP_KERNEL);
 728         if (!priv->ports)
 729                 return -ENOMEM;
 730
 731         for (i = 0; i < priv->ds->num_ports; i++) {
 732                 ret = xrs700x_alloc_port_mib(priv, i);
 733                 if (ret)
 734                         return ret;
 735         }
 736
 737         return dsa_register_switch(priv->ds);
 738 }
 739 EXPORT_SYMBOL(xrs700x_switch_register);
 740
 741 void xrs700x_switch_remove(struct xrs700x *priv)
 742 {
 743         dsa_unregister_switch(priv->ds);
 744 }
 745 EXPORT_SYMBOL(xrs700x_switch_remove);
 746
 747 MODULE_AUTHOR("George McCollister <[email protected]>");
 748 MODULE_DESCRIPTION("Arrow SpeedChips XRS700x DSA driver");
 749 MODULE_LICENSE("GPL v2");