drivers/staging/fsl-dpaa2/ethsw/ethsw.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * DPAA2 Ethernet Switch driver
   4  *
   5  * Copyright 2014-2016 Freescale Semiconductor Inc.
   6  * Copyright 2017-2018 NXP
   7  *
   8  */
   9
  10 #include <linux/module.h>
  11
  12 #include <linux/interrupt.h>
  13 #include <linux/msi.h>
  14 #include <linux/kthread.h>
  15 #include <linux/workqueue.h>
  16
  17 #include <linux/fsl/mc.h>
  18
  19 #include "ethsw.h"
  20
  21 /* Minimal supported DPSW version */
  22 #define DPSW_MIN_VER_MAJOR              8
  23 #define DPSW_MIN_VER_MINOR              1
  24
  25 #define DEFAULT_VLAN_ID                 1
  26
  27 static int ethsw_add_vlan(struct ethsw_core *ethsw, u16 vid)
  28 {
  29         int err;
  30
  31         struct dpsw_vlan_cfg    vcfg = {
  32                 .fdb_id = 0,
  33         };
  34
  35         err = dpsw_vlan_add(ethsw->mc_io, 0,
  36                             ethsw->dpsw_handle, vid, &vcfg);
  37         if (err) {
  38                 dev_err(ethsw->dev, "dpsw_vlan_add err %d\n", err);
  39                 return err;
  40         }
  41         ethsw->vlans[vid] = ETHSW_VLAN_MEMBER;
  42
  43         return 0;
  44 }
  45
  46 static bool ethsw_port_is_up(struct ethsw_port_priv *port_priv)
  47 {
  48         struct net_device *netdev = port_priv->netdev;
  49         struct dpsw_link_state state;
  50         int err;
  51
  52         err = dpsw_if_get_link_state(port_priv->ethsw_data->mc_io, 0,
  53                                      port_priv->ethsw_data->dpsw_handle,
  54                                      port_priv->idx, &state);
  55         if (err) {
  56                 netdev_err(netdev, "dpsw_if_get_link_state() err %d\n", err);
  57                 return true;
  58         }
  59
  60         WARN_ONCE(state.up > 1, "Garbage read into link_state");
  61
  62         return state.up ? true : false;
  63 }
  64
  65 static int ethsw_port_set_pvid(struct ethsw_port_priv *port_priv, u16 pvid)
  66 {
  67         struct ethsw_core *ethsw = port_priv->ethsw_data;
  68         struct net_device *netdev = port_priv->netdev;
  69         struct dpsw_tci_cfg tci_cfg = { 0 };
  70         bool up;
  71         int err, ret;
  72
  73         err = dpsw_if_get_tci(ethsw->mc_io, 0, ethsw->dpsw_handle,
  74                               port_priv->idx, &tci_cfg);
  75         if (err) {
  76                 netdev_err(netdev, "dpsw_if_get_tci err %d\n", err);
  77                 return err;
  78         }
  79
  80         tci_cfg.vlan_id = pvid;
  81
  82         /* Interface needs to be down to change PVID */
  83         up = ethsw_port_is_up(port_priv);
  84         if (up) {
  85                 err = dpsw_if_disable(ethsw->mc_io, 0,
  86                                       ethsw->dpsw_handle,
  87                                       port_priv->idx);
  88                 if (err) {
  89                         netdev_err(netdev, "dpsw_if_disable err %d\n", err);
  90                         return err;
  91                 }
  92         }
  93
  94         err = dpsw_if_set_tci(ethsw->mc_io, 0, ethsw->dpsw_handle,
  95                               port_priv->idx, &tci_cfg);
  96         if (err) {
  97                 netdev_err(netdev, "dpsw_if_set_tci err %d\n", err);
  98                 goto set_tci_error;
  99         }
 100
 101         /* Delete previous PVID info and mark the new one */
 102         port_priv->vlans[port_priv->pvid] &= ~ETHSW_VLAN_PVID;
 103         port_priv->vlans[pvid] |= ETHSW_VLAN_PVID;
 104         port_priv->pvid = pvid;
 105
 106 set_tci_error:
 107         if (up) {
 108                 ret = dpsw_if_enable(ethsw->mc_io, 0,
 109                                      ethsw->dpsw_handle,
 110                                      port_priv->idx);
 111                 if (ret) {
 112                         netdev_err(netdev, "dpsw_if_enable err %d\n", ret);
 113                         return ret;
 114                 }
 115         }
 116
 117         return err;
 118 }
 119
 120 static int ethsw_port_add_vlan(struct ethsw_port_priv *port_priv,
 121                                u16 vid, u16 flags)
 122 {
 123         struct ethsw_core *ethsw = port_priv->ethsw_data;
 124         struct net_device *netdev = port_priv->netdev;
 125         struct dpsw_vlan_if_cfg vcfg;
 126         int err;
 127
 128         if (port_priv->vlans[vid]) {
 129                 netdev_warn(netdev, "VLAN %d already configured\n", vid);
 130                 return -EEXIST;
 131         }
 132
 133         vcfg.num_ifs = 1;
 134         vcfg.if_id[0] = port_priv->idx;
 135         err = dpsw_vlan_add_if(ethsw->mc_io, 0, ethsw->dpsw_handle, vid, &vcfg);
 136         if (err) {
 137                 netdev_err(netdev, "dpsw_vlan_add_if err %d\n", err);
 138                 return err;
 139         }
 140
 141         port_priv->vlans[vid] = ETHSW_VLAN_MEMBER;
 142
 143         if (flags & BRIDGE_VLAN_INFO_UNTAGGED) {
 144                 err = dpsw_vlan_add_if_untagged(ethsw->mc_io, 0,
 145                                                 ethsw->dpsw_handle,
 146                                                 vid, &vcfg);
 147                 if (err) {
 148                         netdev_err(netdev,
 149                                    "dpsw_vlan_add_if_untagged err %d\n", err);
 150                         return err;
 151                 }
 152                 port_priv->vlans[vid] |= ETHSW_VLAN_UNTAGGED;
 153         }
 154
 155         if (flags & BRIDGE_VLAN_INFO_PVID) {
 156                 err = ethsw_port_set_pvid(port_priv, vid);
 157                 if (err)
 158                         return err;
 159         }
 160
 161         return 0;
 162 }
 163
 164 static int ethsw_set_learning(struct ethsw_core *ethsw, bool enable)
 165 {
 166         enum dpsw_fdb_learning_mode learn_mode;
 167         int err;
 168
 169         if (enable)
 170                 learn_mode = DPSW_FDB_LEARNING_MODE_HW;
 171         else
 172                 learn_mode = DPSW_FDB_LEARNING_MODE_DIS;
 173
 174         err = dpsw_fdb_set_learning_mode(ethsw->mc_io, 0, ethsw->dpsw_handle, 0,
 175                                          learn_mode);
 176         if (err) {
 177                 dev_err(ethsw->dev, "dpsw_fdb_set_learning_mode err %d\n", err);
 178                 return err;
 179         }
 180         ethsw->learning = enable;
 181
 182         return 0;
 183 }
 184
 185 static int ethsw_port_set_flood(struct ethsw_port_priv *port_priv, bool enable)
 186 {
 187         int err;
 188
 189         err = dpsw_if_set_flooding(port_priv->ethsw_data->mc_io, 0,
 190                                    port_priv->ethsw_data->dpsw_handle,
 191                                    port_priv->idx, enable);
 192         if (err) {
 193                 netdev_err(port_priv->netdev,
 194                            "dpsw_if_set_flooding err %d\n", err);
 195                 return err;
 196         }
 197         port_priv->flood = enable;
 198
 199         return 0;
 200 }
 201
 202 static int ethsw_port_set_stp_state(struct ethsw_port_priv *port_priv, u8 state)
 203 {
 204         struct dpsw_stp_cfg stp_cfg = {
 205                 .state = state,
 206         };
 207         int err;
 208         u16 vid;
 209
 210         if (!netif_running(port_priv->netdev) || state == port_priv->stp_state)
 211                 return 0;       /* Nothing to do */
 212
 213         for (vid = 0; vid <= VLAN_VID_MASK; vid++) {
 214                 if (port_priv->vlans[vid] & ETHSW_VLAN_MEMBER) {
 215                         stp_cfg.vlan_id = vid;
 216                         err = dpsw_if_set_stp(port_priv->ethsw_data->mc_io, 0,
 217                                               port_priv->ethsw_data->dpsw_handle,
 218                                               port_priv->idx, &stp_cfg);
 219                         if (err) {
 220                                 netdev_err(port_priv->netdev,
 221                                            "dpsw_if_set_stp err %d\n", err);
 222                                 return err;
 223                         }
 224                 }
 225         }
 226
 227         port_priv->stp_state = state;
 228
 229         return 0;
 230 }
 231
 232 static int ethsw_dellink_switch(struct ethsw_core *ethsw, u16 vid)
 233 {
 234         struct ethsw_port_priv *ppriv_local = NULL;
 235         int i, err;
 236
 237         if (!ethsw->vlans[vid])
 238                 return -ENOENT;
 239
 240         err = dpsw_vlan_remove(ethsw->mc_io, 0, ethsw->dpsw_handle, vid);
 241         if (err) {
 242                 dev_err(ethsw->dev, "dpsw_vlan_remove err %d\n", err);
 243                 return err;
 244         }
 245         ethsw->vlans[vid] = 0;
 246
 247         for (i = 0; i < ethsw->sw_attr.num_ifs; i++) {
 248                 ppriv_local = ethsw->ports[i];
 249                 ppriv_local->vlans[vid] = 0;
 250         }
 251
 252         return 0;
 253 }
 254
 255 static int ethsw_port_fdb_add_uc(struct ethsw_port_priv *port_priv,
 256                                  const unsigned char *addr)
 257 {
 258         struct dpsw_fdb_unicast_cfg entry = {0};
 259         int err;
 260
 261         entry.if_egress = port_priv->idx;
 262         entry.type = DPSW_FDB_ENTRY_STATIC;
 263         ether_addr_copy(entry.mac_addr, addr);
 264
 265         err = dpsw_fdb_add_unicast(port_priv->ethsw_data->mc_io, 0,
 266                                    port_priv->ethsw_data->dpsw_handle,
 267                                    0, &entry);
 268         if (err)
 269                 netdev_err(port_priv->netdev,
 270                            "dpsw_fdb_add_unicast err %d\n", err);
 271         return err;
 272 }
 273
 274 static int ethsw_port_fdb_del_uc(struct ethsw_port_priv *port_priv,
 275                                  const unsigned char *addr)
 276 {
 277         struct dpsw_fdb_unicast_cfg entry = {0};
 278         int err;
 279
 280         entry.if_egress = port_priv->idx;
 281         entry.type = DPSW_FDB_ENTRY_STATIC;
 282         ether_addr_copy(entry.mac_addr, addr);
 283
 284         err = dpsw_fdb_remove_unicast(port_priv->ethsw_data->mc_io, 0,
 285                                       port_priv->ethsw_data->dpsw_handle,
 286                                       0, &entry);
 287         /* Silently discard error for calling multiple times the del command */
 288         if (err && err != -ENXIO)
 289                 netdev_err(port_priv->netdev,
 290                            "dpsw_fdb_remove_unicast err %d\n", err);
 291         return err;
 292 }
 293
 294 static int ethsw_port_fdb_add_mc(struct ethsw_port_priv *port_priv,
 295                                  const unsigned char *addr)
 296 {
 297         struct dpsw_fdb_multicast_cfg entry = {0};
 298         int err;
 299
 300         ether_addr_copy(entry.mac_addr, addr);
 301         entry.type = DPSW_FDB_ENTRY_STATIC;
 302         entry.num_ifs = 1;
 303         entry.if_id[0] = port_priv->idx;
 304
 305         err = dpsw_fdb_add_multicast(port_priv->ethsw_data->mc_io, 0,
 306                                      port_priv->ethsw_data->dpsw_handle,
 307                                      0, &entry);
 308         /* Silently discard error for calling multiple times the add command */
 309         if (err && err != -ENXIO)
 310                 netdev_err(port_priv->netdev, "dpsw_fdb_add_multicast err %d\n",
 311                            err);
 312         return err;
 313 }
 314
 315 static int ethsw_port_fdb_del_mc(struct ethsw_port_priv *port_priv,
 316                                  const unsigned char *addr)
 317 {
 318         struct dpsw_fdb_multicast_cfg entry = {0};
 319         int err;
 320
 321         ether_addr_copy(entry.mac_addr, addr);
 322         entry.type = DPSW_FDB_ENTRY_STATIC;
 323         entry.num_ifs = 1;
 324         entry.if_id[0] = port_priv->idx;
 325
 326         err = dpsw_fdb_remove_multicast(port_priv->ethsw_data->mc_io, 0,
 327                                         port_priv->ethsw_data->dpsw_handle,
 328                                         0, &entry);
 329         /* Silently discard error for calling multiple times the del command */
 330         if (err && err != -ENAVAIL)
 331                 netdev_err(port_priv->netdev,
 332                            "dpsw_fdb_remove_multicast err %d\n", err);
 333         return err;
 334 }
 335
 336 static int port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 337                         struct net_device *dev, const unsigned char *addr,
 338                         u16 vid, u16 flags,
 339                         struct netlink_ext_ack *extack)
 340 {
 341         if (is_unicast_ether_addr(addr))
 342                 return ethsw_port_fdb_add_uc(netdev_priv(dev),
 343                                              addr);
 344         else
 345                 return ethsw_port_fdb_add_mc(netdev_priv(dev),
 346                                              addr);
 347 }
 348
 349 static int port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 350                         struct net_device *dev,
 351                         const unsigned char *addr, u16 vid)
 352 {
 353         if (is_unicast_ether_addr(addr))
 354                 return ethsw_port_fdb_del_uc(netdev_priv(dev),
 355                                              addr);
 356         else
 357                 return ethsw_port_fdb_del_mc(netdev_priv(dev),
 358                                              addr);
 359 }
 360
 361 static void port_get_stats(struct net_device *netdev,
 362                            struct rtnl_link_stats64 *stats)
 363 {
 364         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 365         u64 tmp;
 366         int err;
 367
 368         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 369                                   port_priv->ethsw_data->dpsw_handle,
 370                                   port_priv->idx,
 371                                   DPSW_CNT_ING_FRAME, &stats->rx_packets);
 372         if (err)
 373                 goto error;
 374
 375         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 376                                   port_priv->ethsw_data->dpsw_handle,
 377                                   port_priv->idx,
 378                                   DPSW_CNT_EGR_FRAME, &stats->tx_packets);
 379         if (err)
 380                 goto error;
 381
 382         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 383                                   port_priv->ethsw_data->dpsw_handle,
 384                                   port_priv->idx,
 385                                   DPSW_CNT_ING_BYTE, &stats->rx_bytes);
 386         if (err)
 387                 goto error;
 388
 389         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 390                                   port_priv->ethsw_data->dpsw_handle,
 391                                   port_priv->idx,
 392                                   DPSW_CNT_EGR_BYTE, &stats->tx_bytes);
 393         if (err)
 394                 goto error;
 395
 396         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 397                                   port_priv->ethsw_data->dpsw_handle,
 398                                   port_priv->idx,
 399                                   DPSW_CNT_ING_FRAME_DISCARD,
 400                                   &stats->rx_dropped);
 401         if (err)
 402                 goto error;
 403
 404         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 405                                   port_priv->ethsw_data->dpsw_handle,
 406                                   port_priv->idx,
 407                                   DPSW_CNT_ING_FLTR_FRAME,
 408                                   &tmp);
 409         if (err)
 410                 goto error;
 411         stats->rx_dropped += tmp;
 412
 413         err = dpsw_if_get_counter(port_priv->ethsw_data->mc_io, 0,
 414                                   port_priv->ethsw_data->dpsw_handle,
 415                                   port_priv->idx,
 416                                   DPSW_CNT_EGR_FRAME_DISCARD,
 417                                   &stats->tx_dropped);
 418         if (err)
 419                 goto error;
 420
 421         return;
 422
 423 error:
 424         netdev_err(netdev, "dpsw_if_get_counter err %d\n", err);
 425 }
 426
 427 static bool port_has_offload_stats(const struct net_device *netdev,
 428                                    int attr_id)
 429 {
 430         return (attr_id == IFLA_OFFLOAD_XSTATS_CPU_HIT);
 431 }
 432
 433 static int port_get_offload_stats(int attr_id,
 434                                   const struct net_device *netdev,
 435                                   void *sp)
 436 {
 437         switch (attr_id) {
 438         case IFLA_OFFLOAD_XSTATS_CPU_HIT:
 439                 port_get_stats((struct net_device *)netdev, sp);
 440                 return 0;
 441         }
 442
 443         return -EINVAL;
 444 }
 445
 446 static int port_change_mtu(struct net_device *netdev, int mtu)
 447 {
 448         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 449         int err;
 450
 451         err = dpsw_if_set_max_frame_length(port_priv->ethsw_data->mc_io,
 452                                            0,
 453                                            port_priv->ethsw_data->dpsw_handle,
 454                                            port_priv->idx,
 455                                            (u16)ETHSW_L2_MAX_FRM(mtu));
 456         if (err) {
 457                 netdev_err(netdev,
 458                            "dpsw_if_set_max_frame_length() err %d\n", err);
 459                 return err;
 460         }
 461
 462         netdev->mtu = mtu;
 463         return 0;
 464 }
 465
 466 static int port_carrier_state_sync(struct net_device *netdev)
 467 {
 468         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 469         struct dpsw_link_state state;
 470         int err;
 471
 472         /* Interrupts are received even though no one issued an 'ifconfig up'
 473          * on the switch interface. Ignore these link state update interrupts
 474          */
 475         if (!netif_running(netdev))
 476                 return 0;
 477
 478         err = dpsw_if_get_link_state(port_priv->ethsw_data->mc_io, 0,
 479                                      port_priv->ethsw_data->dpsw_handle,
 480                                      port_priv->idx, &state);
 481         if (err) {
 482                 netdev_err(netdev, "dpsw_if_get_link_state() err %d\n", err);
 483                 return err;
 484         }
 485
 486         WARN_ONCE(state.up > 1, "Garbage read into link_state");
 487
 488         if (state.up != port_priv->link_state) {
 489                 if (state.up)
 490                         netif_carrier_on(netdev);
 491                 else
 492                         netif_carrier_off(netdev);
 493                 port_priv->link_state = state.up;
 494         }
 495
 496         return 0;
 497 }
 498
 499 static int port_open(struct net_device *netdev)
 500 {
 501         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 502         int err;
 503
 504         /* No need to allow Tx as control interface is disabled */
 505         netif_tx_stop_all_queues(netdev);
 506
 507         /* Explicitly set carrier off, otherwise
 508          * netif_carrier_ok() will return true and cause 'ip link show'
 509          * to report the LOWER_UP flag, even though the link
 510          * notification wasn't even received.
 511          */
 512         netif_carrier_off(netdev);
 513
 514         err = dpsw_if_enable(port_priv->ethsw_data->mc_io, 0,
 515                              port_priv->ethsw_data->dpsw_handle,
 516                              port_priv->idx);
 517         if (err) {
 518                 netdev_err(netdev, "dpsw_if_enable err %d\n", err);
 519                 return err;
 520         }
 521
 522         /* sync carrier state */
 523         err = port_carrier_state_sync(netdev);
 524         if (err) {
 525                 netdev_err(netdev,
 526                            "port_carrier_state_sync err %d\n", err);
 527                 goto err_carrier_sync;
 528         }
 529
 530         return 0;
 531
 532 err_carrier_sync:
 533         dpsw_if_disable(port_priv->ethsw_data->mc_io, 0,
 534                         port_priv->ethsw_data->dpsw_handle,
 535                         port_priv->idx);
 536         return err;
 537 }
 538
 539 static int port_stop(struct net_device *netdev)
 540 {
 541         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 542         int err;
 543
 544         err = dpsw_if_disable(port_priv->ethsw_data->mc_io, 0,
 545                               port_priv->ethsw_data->dpsw_handle,
 546                               port_priv->idx);
 547         if (err) {
 548                 netdev_err(netdev, "dpsw_if_disable err %d\n", err);
 549                 return err;
 550         }
 551
 552         return 0;
 553 }
 554
 555 static netdev_tx_t port_dropframe(struct sk_buff *skb,
 556                                   struct net_device *netdev)
 557 {
 558         /* we don't support I/O for now, drop the frame */
 559         dev_kfree_skb_any(skb);
 560
 561         return NETDEV_TX_OK;
 562 }
 563
 564 static int swdev_get_port_parent_id(struct net_device *dev,
 565                                     struct netdev_phys_item_id *ppid)
 566 {
 567         struct ethsw_port_priv *port_priv = netdev_priv(dev);
 568
 569         ppid->id_len = 1;
 570         ppid->id[0] = port_priv->ethsw_data->dev_id;
 571
 572         return 0;
 573 }
 574
 575 static int port_get_phys_name(struct net_device *netdev, char *name,
 576                               size_t len)
 577 {
 578         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 579         int err;
 580
 581         err = snprintf(name, len, "p%d", port_priv->idx);
 582         if (err >= len)
 583                 return -EINVAL;
 584
 585         return 0;
 586 }
 587
 588 struct ethsw_dump_ctx {
 589         struct net_device *dev;
 590         struct sk_buff *skb;
 591         struct netlink_callback *cb;
 592         int idx;
 593 };
 594
 595 static int ethsw_fdb_do_dump(struct fdb_dump_entry *entry,
 596                              struct ethsw_dump_ctx *dump)
 597 {
 598         int is_dynamic = entry->type & DPSW_FDB_ENTRY_DINAMIC;
 599         u32 portid = NETLINK_CB(dump->cb->skb).portid;
 600         u32 seq = dump->cb->nlh->nlmsg_seq;
 601         struct nlmsghdr *nlh;
 602         struct ndmsg *ndm;
 603
 604         if (dump->idx < dump->cb->args[2])
 605                 goto skip;
 606
 607         nlh = nlmsg_put(dump->skb, portid, seq, RTM_NEWNEIGH,
 608                         sizeof(*ndm), NLM_F_MULTI);
 609         if (!nlh)
 610                 return -EMSGSIZE;
 611
 612         ndm = nlmsg_data(nlh);
 613         ndm->ndm_family  = AF_BRIDGE;
 614         ndm->ndm_pad1    = 0;
 615         ndm->ndm_pad2    = 0;
 616         ndm->ndm_flags   = NTF_SELF;
 617         ndm->ndm_type    = 0;
 618         ndm->ndm_ifindex = dump->dev->ifindex;
 619         ndm->ndm_state   = is_dynamic ? NUD_REACHABLE : NUD_NOARP;
 620
 621         if (nla_put(dump->skb, NDA_LLADDR, ETH_ALEN, entry->mac_addr))
 622                 goto nla_put_failure;
 623
 624         nlmsg_end(dump->skb, nlh);
 625
 626 skip:
 627         dump->idx++;
 628         return 0;
 629
 630 nla_put_failure:
 631         nlmsg_cancel(dump->skb, nlh);
 632         return -EMSGSIZE;
 633 }
 634
 635 static int port_fdb_valid_entry(struct fdb_dump_entry *entry,
 636                                 struct ethsw_port_priv *port_priv)
 637 {
 638         int idx = port_priv->idx;
 639         int valid;
 640
 641         if (entry->type & DPSW_FDB_ENTRY_TYPE_UNICAST)
 642                 valid = entry->if_info == port_priv->idx;
 643         else
 644                 valid = entry->if_mask[idx / 8] & BIT(idx % 8);
 645
 646         return valid;
 647 }
 648
 649 static int port_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
 650                          struct net_device *net_dev,
 651                          struct net_device *filter_dev, int *idx)
 652 {
 653         struct ethsw_port_priv *port_priv = netdev_priv(net_dev);
 654         struct ethsw_core *ethsw = port_priv->ethsw_data;
 655         struct device *dev = net_dev->dev.parent;
 656         struct fdb_dump_entry *fdb_entries;
 657         struct fdb_dump_entry fdb_entry;
 658         struct ethsw_dump_ctx dump = {
 659                 .dev = net_dev,
 660                 .skb = skb,
 661                 .cb = cb,
 662                 .idx = *idx,
 663         };
 664         dma_addr_t fdb_dump_iova;
 665         u16 num_fdb_entries;
 666         u32 fdb_dump_size;
 667         int err = 0, i;
 668         u8 *dma_mem;
 669
 670         fdb_dump_size = ethsw->sw_attr.max_fdb_entries * sizeof(fdb_entry);
 671         dma_mem = kzalloc(fdb_dump_size, GFP_KERNEL);
 672         if (!dma_mem)
 673                 return -ENOMEM;
 674
 675         fdb_dump_iova = dma_map_single(dev, dma_mem, fdb_dump_size,
 676                                        DMA_FROM_DEVICE);
 677         if (dma_mapping_error(dev, fdb_dump_iova)) {
 678                 netdev_err(net_dev, "dma_map_single() failed\n");
 679                 err = -ENOMEM;
 680                 goto err_map;
 681         }
 682
 683         err = dpsw_fdb_dump(ethsw->mc_io, 0, ethsw->dpsw_handle, 0,
 684                             fdb_dump_iova, fdb_dump_size, &num_fdb_entries);
 685         if (err) {
 686                 netdev_err(net_dev, "dpsw_fdb_dump() = %d\n", err);
 687                 goto err_dump;
 688         }
 689
 690         dma_unmap_single(dev, fdb_dump_iova, fdb_dump_size, DMA_FROM_DEVICE);
 691
 692         fdb_entries = (struct fdb_dump_entry *)dma_mem;
 693         for (i = 0; i < num_fdb_entries; i++) {
 694                 fdb_entry = fdb_entries[i];
 695
 696                 if (!port_fdb_valid_entry(&fdb_entry, port_priv))
 697                         continue;
 698
 699                 err = ethsw_fdb_do_dump(&fdb_entry, &dump);
 700                 if (err)
 701                         goto end;
 702         }
 703
 704 end:
 705         *idx = dump.idx;
 706
 707         kfree(dma_mem);
 708
 709         return 0;
 710
 711 err_dump:
 712         dma_unmap_single(dev, fdb_dump_iova, fdb_dump_size, DMA_TO_DEVICE);
 713 err_map:
 714         kfree(dma_mem);
 715         return err;
 716 }
 717
 718 static int ethsw_port_set_mac_addr(struct ethsw_port_priv *port_priv)
 719 {
 720         struct ethsw_core *ethsw = port_priv->ethsw_data;
 721         struct net_device *net_dev = port_priv->netdev;
 722         struct device *dev = net_dev->dev.parent;
 723         u8 mac_addr[ETH_ALEN];
 724         int err;
 725
 726         if (!(ethsw->features & ETHSW_FEATURE_MAC_ADDR))
 727                 return 0;
 728
 729         /* Get firmware address, if any */
 730         err = dpsw_if_get_port_mac_addr(ethsw->mc_io, 0, ethsw->dpsw_handle,
 731                                         port_priv->idx, mac_addr);
 732         if (err) {
 733                 dev_err(dev, "dpsw_if_get_port_mac_addr() failed\n");
 734                 return err;
 735         }
 736
 737         /* First check if firmware has any address configured by bootloader */
 738         if (!is_zero_ether_addr(mac_addr)) {
 739                 memcpy(net_dev->dev_addr, mac_addr, net_dev->addr_len);
 740         } else {
 741                 /* No MAC address configured, fill in net_dev->dev_addr
 742                  * with a random one
 743                  */
 744                 eth_hw_addr_random(net_dev);
 745                 dev_dbg_once(dev, "device(s) have all-zero hwaddr, replaced with random\n");
 746
 747                 /* Override NET_ADDR_RANDOM set by eth_hw_addr_random(); for all
 748                  * practical purposes, this will be our "permanent" mac address,
 749                  * at least until the next reboot. This move will also permit
 750                  * register_netdevice() to properly fill up net_dev->perm_addr.
 751                  */
 752                 net_dev->addr_assign_type = NET_ADDR_PERM;
 753         }
 754
 755         return 0;
 756 }
 757
 758 static const struct net_device_ops ethsw_port_ops = {
 759         .ndo_open               = port_open,
 760         .ndo_stop               = port_stop,
 761
 762         .ndo_set_mac_address    = eth_mac_addr,
 763         .ndo_get_stats64        = port_get_stats,
 764         .ndo_change_mtu         = port_change_mtu,
 765         .ndo_has_offload_stats  = port_has_offload_stats,
 766         .ndo_get_offload_stats  = port_get_offload_stats,
 767         .ndo_fdb_add            = port_fdb_add,
 768         .ndo_fdb_del            = port_fdb_del,
 769         .ndo_fdb_dump           = port_fdb_dump,
 770
 771         .ndo_start_xmit         = port_dropframe,
 772         .ndo_get_port_parent_id = swdev_get_port_parent_id,
 773         .ndo_get_phys_port_name = port_get_phys_name,
 774 };
 775
 776 static bool ethsw_port_dev_check(const struct net_device *netdev,
 777                                  struct notifier_block *nb)
 778 {
 779         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 780
 781         if (netdev->netdev_ops == &ethsw_port_ops &&
 782             (!nb || &port_priv->ethsw_data->port_nb == nb ||
 783              &port_priv->ethsw_data->port_switchdev_nb == nb ||
 784              &port_priv->ethsw_data->port_switchdevb_nb == nb))
 785                 return true;
 786
 787         return false;
 788 }
 789
 790 static void ethsw_links_state_update(struct ethsw_core *ethsw)
 791 {
 792         int i;
 793
 794         for (i = 0; i < ethsw->sw_attr.num_ifs; i++) {
 795                 port_carrier_state_sync(ethsw->ports[i]->netdev);
 796                 ethsw_port_set_mac_addr(ethsw->ports[i]);
 797         }
 798 }
 799
 800 static irqreturn_t ethsw_irq0_handler_thread(int irq_num, void *arg)
 801 {
 802         struct device *dev = (struct device *)arg;
 803         struct ethsw_core *ethsw = dev_get_drvdata(dev);
 804
 805         /* Mask the events and the if_id reserved bits to be cleared on read */
 806         u32 status = DPSW_IRQ_EVENT_LINK_CHANGED | 0xFFFF0000;
 807         int err;
 808
 809         err = dpsw_get_irq_status(ethsw->mc_io, 0, ethsw->dpsw_handle,
 810                                   DPSW_IRQ_INDEX_IF, &status);
 811         if (err) {
 812                 dev_err(dev, "Can't get irq status (err %d)\n", err);
 813
 814                 err = dpsw_clear_irq_status(ethsw->mc_io, 0, ethsw->dpsw_handle,
 815                                             DPSW_IRQ_INDEX_IF, 0xFFFFFFFF);
 816                 if (err)
 817                         dev_err(dev, "Can't clear irq status (err %d)\n", err);
 818                 goto out;
 819         }
 820
 821         if (status & DPSW_IRQ_EVENT_LINK_CHANGED)
 822                 ethsw_links_state_update(ethsw);
 823
 824 out:
 825         return IRQ_HANDLED;
 826 }
 827
 828 static int ethsw_setup_irqs(struct fsl_mc_device *sw_dev)
 829 {
 830         struct device *dev = &sw_dev->dev;
 831         struct ethsw_core *ethsw = dev_get_drvdata(dev);
 832         u32 mask = DPSW_IRQ_EVENT_LINK_CHANGED;
 833         struct fsl_mc_device_irq *irq;
 834         int err;
 835
 836         err = fsl_mc_allocate_irqs(sw_dev);
 837         if (err) {
 838                 dev_err(dev, "MC irqs allocation failed\n");
 839                 return err;
 840         }
 841
 842         if (WARN_ON(sw_dev->obj_desc.irq_count != DPSW_IRQ_NUM)) {
 843                 err = -EINVAL;
 844                 goto free_irq;
 845         }
 846
 847         err = dpsw_set_irq_enable(ethsw->mc_io, 0, ethsw->dpsw_handle,
 848                                   DPSW_IRQ_INDEX_IF, 0);
 849         if (err) {
 850                 dev_err(dev, "dpsw_set_irq_enable err %d\n", err);
 851                 goto free_irq;
 852         }
 853
 854         irq = sw_dev->irqs[DPSW_IRQ_INDEX_IF];
 855
 856         err = devm_request_threaded_irq(dev, irq->msi_desc->irq,
 857                                         NULL,
 858                                         ethsw_irq0_handler_thread,
 859                                         IRQF_NO_SUSPEND | IRQF_ONESHOT,
 860                                         dev_name(dev), dev);
 861         if (err) {
 862                 dev_err(dev, "devm_request_threaded_irq(): %d\n", err);
 863                 goto free_irq;
 864         }
 865
 866         err = dpsw_set_irq_mask(ethsw->mc_io, 0, ethsw->dpsw_handle,
 867                                 DPSW_IRQ_INDEX_IF, mask);
 868         if (err) {
 869                 dev_err(dev, "dpsw_set_irq_mask(): %d\n", err);
 870                 goto free_devm_irq;
 871         }
 872
 873         err = dpsw_set_irq_enable(ethsw->mc_io, 0, ethsw->dpsw_handle,
 874                                   DPSW_IRQ_INDEX_IF, 1);
 875         if (err) {
 876                 dev_err(dev, "dpsw_set_irq_enable(): %d\n", err);
 877                 goto free_devm_irq;
 878         }
 879
 880         return 0;
 881
 882 free_devm_irq:
 883         devm_free_irq(dev, irq->msi_desc->irq, dev);
 884 free_irq:
 885         fsl_mc_free_irqs(sw_dev);
 886         return err;
 887 }
 888
 889 static void ethsw_teardown_irqs(struct fsl_mc_device *sw_dev)
 890 {
 891         struct device *dev = &sw_dev->dev;
 892         struct ethsw_core *ethsw = dev_get_drvdata(dev);
 893         int err;
 894
 895         err = dpsw_set_irq_enable(ethsw->mc_io, 0, ethsw->dpsw_handle,
 896                                   DPSW_IRQ_INDEX_IF, 0);
 897         if (err)
 898                 dev_err(dev, "dpsw_set_irq_enable err %d\n", err);
 899
 900         fsl_mc_free_irqs(sw_dev);
 901 }
 902
 903 static int port_attr_stp_state_set(struct net_device *netdev,
 904                                    struct switchdev_trans *trans,
 905                                    u8 state)
 906 {
 907         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 908
 909         if (switchdev_trans_ph_prepare(trans))
 910                 return 0;
 911
 912         return ethsw_port_set_stp_state(port_priv, state);
 913 }
 914
 915 static int port_attr_br_flags_pre_set(struct net_device *netdev,
 916                                       struct switchdev_trans *trans,
 917                                       unsigned long flags)
 918 {
 919         if (flags & ~(BR_LEARNING | BR_FLOOD))
 920                 return -EINVAL;
 921
 922         return 0;
 923 }
 924
 925 static int port_attr_br_flags_set(struct net_device *netdev,
 926                                   struct switchdev_trans *trans,
 927                                   unsigned long flags)
 928 {
 929         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 930         int err = 0;
 931
 932         if (switchdev_trans_ph_prepare(trans))
 933                 return 0;
 934
 935         /* Learning is enabled per switch */
 936         err = ethsw_set_learning(port_priv->ethsw_data,
 937                                  !!(flags & BR_LEARNING));
 938         if (err)
 939                 goto exit;
 940
 941         err = ethsw_port_set_flood(port_priv, !!(flags & BR_FLOOD));
 942
 943 exit:
 944         return err;
 945 }
 946
 947 static int swdev_port_attr_set(struct net_device *netdev,
 948                                const struct switchdev_attr *attr,
 949                                struct switchdev_trans *trans)
 950 {
 951         int err = 0;
 952
 953         switch (attr->id) {
 954         case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 955                 err = port_attr_stp_state_set(netdev, trans,
 956                                               attr->u.stp_state);
 957                 break;
 958         case SWITCHDEV_ATTR_ID_PORT_PRE_BRIDGE_FLAGS:
 959                 err = port_attr_br_flags_pre_set(netdev, trans,
 960                                                  attr->u.brport_flags);
 961                 break;
 962         case SWITCHDEV_ATTR_ID_PORT_BRIDGE_FLAGS:
 963                 err = port_attr_br_flags_set(netdev, trans,
 964                                              attr->u.brport_flags);
 965                 break;
 966         case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
 967                 /* VLANs are supported by default  */
 968                 break;
 969         default:
 970                 err = -EOPNOTSUPP;
 971                 break;
 972         }
 973
 974         return err;
 975 }
 976
 977 static int port_vlans_add(struct net_device *netdev,
 978                           const struct switchdev_obj_port_vlan *vlan,
 979                           struct switchdev_trans *trans)
 980 {
 981         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
 982         struct ethsw_core *ethsw = port_priv->ethsw_data;
 983         struct dpsw_attr *attr = &ethsw->sw_attr;
 984         int vid, err = 0, new_vlans = 0;
 985
 986         if (switchdev_trans_ph_prepare(trans)) {
 987                 for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++)
 988                         if (!port_priv->ethsw_data->vlans[vid])
 989                                 new_vlans++;
 990
 991                 /* Check if there is space for a new VLAN */
 992                 err = dpsw_get_attributes(ethsw->mc_io, 0, ethsw->dpsw_handle,
 993                                           &ethsw->sw_attr);
 994                 if (err) {
 995                         netdev_err(netdev, "dpsw_get_attributes err %d\n", err);
 996                         return err;
 997                 }
 998                 if (attr->max_vlans - attr->num_vlans < new_vlans)
 999                         return -ENOSPC;
1000
1001                 return 0;
1002         }
1003
1004         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1005                 if (!port_priv->ethsw_data->vlans[vid]) {
1006                         /* this is a new VLAN */
1007                         err = ethsw_add_vlan(port_priv->ethsw_data, vid);
1008                         if (err)
1009                                 return err;
1010
1011                         port_priv->ethsw_data->vlans[vid] |= ETHSW_VLAN_GLOBAL;
1012                 }
1013                 err = ethsw_port_add_vlan(port_priv, vid, vlan->flags);
1014                 if (err)
1015                         break;
1016         }
1017
1018         return err;
1019 }
1020
1021 static int port_lookup_address(struct net_device *netdev, int is_uc,
1022                                const unsigned char *addr)
1023 {
1024         struct netdev_hw_addr_list *list = (is_uc) ? &netdev->uc : &netdev->mc;
1025         struct netdev_hw_addr *ha;
1026
1027         netif_addr_lock_bh(netdev);
1028         list_for_each_entry(ha, &list->list, list) {
1029                 if (ether_addr_equal(ha->addr, addr)) {
1030                         netif_addr_unlock_bh(netdev);
1031                         return 1;
1032                 }
1033         }
1034         netif_addr_unlock_bh(netdev);
1035         return 0;
1036 }
1037
1038 static int port_mdb_add(struct net_device *netdev,
1039                         const struct switchdev_obj_port_mdb *mdb,
1040                         struct switchdev_trans *trans)
1041 {
1042         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
1043         int err;
1044
1045         if (switchdev_trans_ph_prepare(trans))
1046                 return 0;
1047
1048         /* Check if address is already set on this port */
1049         if (port_lookup_address(netdev, 0, mdb->addr))
1050                 return -EEXIST;
1051
1052         err = ethsw_port_fdb_add_mc(port_priv, mdb->addr);
1053         if (err)
1054                 return err;
1055
1056         err = dev_mc_add(netdev, mdb->addr);
1057         if (err) {
1058                 netdev_err(netdev, "dev_mc_add err %d\n", err);
1059                 ethsw_port_fdb_del_mc(port_priv, mdb->addr);
1060         }
1061
1062         return err;
1063 }
1064
1065 static int swdev_port_obj_add(struct net_device *netdev,
1066                               const struct switchdev_obj *obj,
1067                               struct switchdev_trans *trans)
1068 {
1069         int err;
1070
1071         switch (obj->id) {
1072         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1073                 err = port_vlans_add(netdev,
1074                                      SWITCHDEV_OBJ_PORT_VLAN(obj),
1075                                      trans);
1076                 break;
1077         case SWITCHDEV_OBJ_ID_PORT_MDB:
1078                 err = port_mdb_add(netdev,
1079                                    SWITCHDEV_OBJ_PORT_MDB(obj),
1080                                    trans);
1081                 break;
1082         default:
1083                 err = -EOPNOTSUPP;
1084                 break;
1085         }
1086
1087         return err;
1088 }
1089
1090 static int ethsw_port_del_vlan(struct ethsw_port_priv *port_priv, u16 vid)
1091 {
1092         struct ethsw_core *ethsw = port_priv->ethsw_data;
1093         struct net_device *netdev = port_priv->netdev;
1094         struct dpsw_vlan_if_cfg vcfg;
1095         int i, err;
1096
1097         if (!port_priv->vlans[vid])
1098                 return -ENOENT;
1099
1100         if (port_priv->vlans[vid] & ETHSW_VLAN_PVID) {
1101                 err = ethsw_port_set_pvid(port_priv, 0);
1102                 if (err)
1103                         return err;
1104         }
1105
1106         vcfg.num_ifs = 1;
1107         vcfg.if_id[0] = port_priv->idx;
1108         if (port_priv->vlans[vid] & ETHSW_VLAN_UNTAGGED) {
1109                 err = dpsw_vlan_remove_if_untagged(ethsw->mc_io, 0,
1110                                                    ethsw->dpsw_handle,
1111                                                    vid, &vcfg);
1112                 if (err) {
1113                         netdev_err(netdev,
1114                                    "dpsw_vlan_remove_if_untagged err %d\n",
1115                                    err);
1116                 }
1117                 port_priv->vlans[vid] &= ~ETHSW_VLAN_UNTAGGED;
1118         }
1119
1120         if (port_priv->vlans[vid] & ETHSW_VLAN_MEMBER) {
1121                 err = dpsw_vlan_remove_if(ethsw->mc_io, 0, ethsw->dpsw_handle,
1122                                           vid, &vcfg);
1123                 if (err) {
1124                         netdev_err(netdev,
1125                                    "dpsw_vlan_remove_if err %d\n", err);
1126                         return err;
1127                 }
1128                 port_priv->vlans[vid] &= ~ETHSW_VLAN_MEMBER;
1129
1130                 /* Delete VLAN from switch if it is no longer configured on
1131                  * any port
1132                  */
1133                 for (i = 0; i < ethsw->sw_attr.num_ifs; i++)
1134                         if (ethsw->ports[i]->vlans[vid] & ETHSW_VLAN_MEMBER)
1135                                 return 0; /* Found a port member in VID */
1136
1137                 ethsw->vlans[vid] &= ~ETHSW_VLAN_GLOBAL;
1138
1139                 err = ethsw_dellink_switch(ethsw, vid);
1140                 if (err)
1141                         return err;
1142         }
1143
1144         return 0;
1145 }
1146
1147 static int port_vlans_del(struct net_device *netdev,
1148                           const struct switchdev_obj_port_vlan *vlan)
1149 {
1150         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
1151         int vid, err = 0;
1152
1153         if (netif_is_bridge_master(vlan->obj.orig_dev))
1154                 return -EOPNOTSUPP;
1155
1156         for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
1157                 err = ethsw_port_del_vlan(port_priv, vid);
1158                 if (err)
1159                         break;
1160         }
1161
1162         return err;
1163 }
1164
1165 static int port_mdb_del(struct net_device *netdev,
1166                         const struct switchdev_obj_port_mdb *mdb)
1167 {
1168         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
1169         int err;
1170
1171         if (!port_lookup_address(netdev, 0, mdb->addr))
1172                 return -ENOENT;
1173
1174         err = ethsw_port_fdb_del_mc(port_priv, mdb->addr);
1175         if (err)
1176                 return err;
1177
1178         err = dev_mc_del(netdev, mdb->addr);
1179         if (err) {
1180                 netdev_err(netdev, "dev_mc_del err %d\n", err);
1181                 return err;
1182         }
1183
1184         return err;
1185 }
1186
1187 static int swdev_port_obj_del(struct net_device *netdev,
1188                               const struct switchdev_obj *obj)
1189 {
1190         int err;
1191
1192         switch (obj->id) {
1193         case SWITCHDEV_OBJ_ID_PORT_VLAN:
1194                 err = port_vlans_del(netdev, SWITCHDEV_OBJ_PORT_VLAN(obj));
1195                 break;
1196         case SWITCHDEV_OBJ_ID_PORT_MDB:
1197                 err = port_mdb_del(netdev, SWITCHDEV_OBJ_PORT_MDB(obj));
1198                 break;
1199         default:
1200                 err = -EOPNOTSUPP;
1201                 break;
1202         }
1203         return err;
1204 }
1205
1206 static int
1207 ethsw_switchdev_port_attr_set_event(struct net_device *netdev,
1208                                     struct switchdev_notifier_port_attr_info
1209                                     *port_attr_info)
1210 {
1211         int err;
1212
1213         err = swdev_port_attr_set(netdev, port_attr_info->attr,
1214                                   port_attr_info->trans);
1215
1216         port_attr_info->handled = true;
1217         return notifier_from_errno(err);
1218 }
1219
1220 /* For the moment, only flood setting needs to be updated */
1221 static int port_bridge_join(struct net_device *netdev,
1222                             struct net_device *upper_dev)
1223 {
1224         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
1225         struct ethsw_core *ethsw = port_priv->ethsw_data;
1226         struct ethsw_port_priv *other_port_priv;
1227         struct net_device *other_dev;
1228         struct list_head *iter;
1229         int i, err;
1230
1231         for (i = 0; i < ethsw->sw_attr.num_ifs; i++)
1232                 if (ethsw->ports[i]->bridge_dev &&
1233                     (ethsw->ports[i]->bridge_dev != upper_dev)) {
1234                         netdev_err(netdev,
1235                                    "Only one bridge supported per DPSW object!\n");
1236                         return -EINVAL;
1237                 }
1238
1239         netdev_for_each_lower_dev(upper_dev, other_dev, iter) {
1240                 if (!ethsw_port_dev_check(other_dev, NULL))
1241                         continue;
1242
1243                 other_port_priv = netdev_priv(other_dev);
1244                 if (other_port_priv->ethsw_data != port_priv->ethsw_data) {
1245                         netdev_err(netdev,
1246                                    "Interface from a different DPSW is in the bridge already!\n");
1247                         return -EINVAL;
1248                 }
1249         }
1250
1251         /* Enable flooding */
1252         err = ethsw_port_set_flood(port_priv, 1);
1253         if (!err)
1254                 port_priv->bridge_dev = upper_dev;
1255
1256         return err;
1257 }
1258
1259 static int port_bridge_leave(struct net_device *netdev)
1260 {
1261         struct ethsw_port_priv *port_priv = netdev_priv(netdev);
1262         int err;
1263
1264         /* Disable flooding */
1265         err = ethsw_port_set_flood(port_priv, 0);
1266         if (!err)
1267                 port_priv->bridge_dev = NULL;
1268
1269         return err;
1270 }
1271
1272 static int port_netdevice_event(struct notifier_block *nb,
1273                                 unsigned long event, void *ptr)
1274 {
1275         struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
1276         struct netdev_notifier_changeupper_info *info = ptr;
1277         struct net_device *upper_dev;
1278         int err = 0;
1279
1280         if (!ethsw_port_dev_check(netdev, nb))
1281                 return NOTIFY_DONE;
1282
1283         /* Handle just upper dev link/unlink for the moment */
1284         if (event == NETDEV_CHANGEUPPER) {
1285                 upper_dev = info->upper_dev;
1286                 if (netif_is_bridge_master(upper_dev)) {
1287                         if (info->linking)
1288                                 err = port_bridge_join(netdev, upper_dev);
1289                         else
1290                                 err = port_bridge_leave(netdev);
1291                 }
1292         }
1293
1294         return notifier_from_errno(err);
1295 }
1296
1297 struct ethsw_switchdev_event_work {
1298         struct work_struct work;
1299         struct switchdev_notifier_fdb_info fdb_info;
1300         struct net_device *dev;
1301         unsigned long event;
1302 };
1303
1304 static void ethsw_switchdev_event_work(struct work_struct *work)
1305 {
1306         struct ethsw_switchdev_event_work *switchdev_work =
1307                 container_of(work, struct ethsw_switchdev_event_work, work);
1308         struct net_device *dev = switchdev_work->dev;
1309         struct switchdev_notifier_fdb_info *fdb_info;
1310         int err;
1311
1312         rtnl_lock();
1313         fdb_info = &switchdev_work->fdb_info;
1314
1315         switch (switchdev_work->event) {
1316         case SWITCHDEV_FDB_ADD_TO_DEVICE:
1317                 if (!fdb_info->added_by_user)
1318                         break;
1319                 if (is_unicast_ether_addr(fdb_info->addr))
1320                         err = ethsw_port_fdb_add_uc(netdev_priv(dev),
1321                                                     fdb_info->addr);
1322                 else
1323                         err = ethsw_port_fdb_add_mc(netdev_priv(dev),
1324                                                     fdb_info->addr);
1325                 if (err)
1326                         break;
1327                 fdb_info->offloaded = true;
1328                 call_switchdev_notifiers(SWITCHDEV_FDB_OFFLOADED, dev,
1329                                          &fdb_info->info, NULL);
1330                 break;
1331         case SWITCHDEV_FDB_DEL_TO_DEVICE:
1332                 if (!fdb_info->added_by_user)
1333                         break;
1334                 if (is_unicast_ether_addr(fdb_info->addr))
1335                         ethsw_port_fdb_del_uc(netdev_priv(dev), fdb_info->addr);
1336                 else
1337                         ethsw_port_fdb_del_mc(netdev_priv(dev), fdb_info->addr);
1338                 break;
1339         }
1340
1341         rtnl_unlock();
1342         kfree(switchdev_work->fdb_info.addr);
1343         kfree(switchdev_work);
1344         dev_put(dev);
1345 }
1346
1347 /* Called under rcu_read_lock() */
1348 static int port_switchdev_event(struct notifier_block *nb,
1349                                 unsigned long event, void *ptr)
1350 {
1351         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1352         struct ethsw_port_priv *port_priv = netdev_priv(dev);
1353         struct ethsw_switchdev_event_work *switchdev_work;
1354         struct switchdev_notifier_fdb_info *fdb_info = ptr;
1355         struct ethsw_core *ethsw = port_priv->ethsw_data;
1356
1357         if (!ethsw_port_dev_check(dev, nb))
1358                 return NOTIFY_DONE;
1359
1360         if (event == SWITCHDEV_PORT_ATTR_SET)
1361                 return ethsw_switchdev_port_attr_set_event(dev, ptr);
1362
1363         switchdev_work = kzalloc(sizeof(*switchdev_work), GFP_ATOMIC);
1364         if (!switchdev_work)
1365                 return NOTIFY_BAD;
1366
1367         INIT_WORK(&switchdev_work->work, ethsw_switchdev_event_work);
1368         switchdev_work->dev = dev;
1369         switchdev_work->event = event;
1370
1371         switch (event) {
1372         case SWITCHDEV_FDB_ADD_TO_DEVICE:
1373         case SWITCHDEV_FDB_DEL_TO_DEVICE:
1374                 memcpy(&switchdev_work->fdb_info, ptr,
1375                        sizeof(switchdev_work->fdb_info));
1376                 switchdev_work->fdb_info.addr = kzalloc(ETH_ALEN, GFP_ATOMIC);
1377                 if (!switchdev_work->fdb_info.addr)
1378                         goto err_addr_alloc;
1379
1380                 ether_addr_copy((u8 *)switchdev_work->fdb_info.addr,
1381                                 fdb_info->addr);
1382
1383                 /* Take a reference on the device to avoid being freed. */
1384                 dev_hold(dev);
1385                 break;
1386         default:
1387                 kfree(switchdev_work);
1388                 return NOTIFY_DONE;
1389         }
1390
1391         queue_work(ethsw->workqueue, &switchdev_work->work);
1392
1393         return NOTIFY_DONE;
1394
1395 err_addr_alloc:
1396         kfree(switchdev_work);
1397         return NOTIFY_BAD;
1398 }
1399
1400 static int
1401 ethsw_switchdev_port_obj_event(unsigned long event, struct net_device *netdev,
1402                                struct switchdev_notifier_port_obj_info
1403                                *port_obj_info)
1404 {
1405         int err = -EOPNOTSUPP;
1406
1407         switch (event) {
1408         case SWITCHDEV_PORT_OBJ_ADD:
1409                 err = swdev_port_obj_add(netdev, port_obj_info->obj,
1410                                          port_obj_info->trans);
1411                 break;
1412         case SWITCHDEV_PORT_OBJ_DEL:
1413                 err = swdev_port_obj_del(netdev, port_obj_info->obj);
1414                 break;
1415         }
1416
1417         port_obj_info->handled = true;
1418         return notifier_from_errno(err);
1419 }
1420
1421 static int port_switchdev_blocking_event(struct notifier_block *nb,
1422                                          unsigned long event, void *ptr)
1423 {
1424         struct net_device *dev = switchdev_notifier_info_to_dev(ptr);
1425
1426         if (!ethsw_port_dev_check(dev, nb))
1427                 return NOTIFY_DONE;
1428
1429         switch (event) {
1430         case SWITCHDEV_PORT_OBJ_ADD:
1431         case SWITCHDEV_PORT_OBJ_DEL:
1432                 return ethsw_switchdev_port_obj_event(event, dev, ptr);
1433         case SWITCHDEV_PORT_ATTR_SET:
1434                 return ethsw_switchdev_port_attr_set_event(dev, ptr);
1435         }
1436
1437         return NOTIFY_DONE;
1438 }
1439
1440 static int ethsw_register_notifier(struct device *dev)
1441 {
1442         struct ethsw_core *ethsw = dev_get_drvdata(dev);
1443         int err;
1444
1445         ethsw->port_nb.notifier_call = port_netdevice_event;
1446         err = register_netdevice_notifier(&ethsw->port_nb);
1447         if (err) {
1448                 dev_err(dev, "Failed to register netdev notifier\n");
1449                 return err;
1450         }
1451
1452         ethsw->port_switchdev_nb.notifier_call = port_switchdev_event;
1453         err = register_switchdev_notifier(&ethsw->port_switchdev_nb);
1454         if (err) {
1455                 dev_err(dev, "Failed to register switchdev notifier\n");
1456                 goto err_switchdev_nb;
1457         }
1458
1459         ethsw->port_switchdevb_nb.notifier_call = port_switchdev_blocking_event;
1460         err = register_switchdev_blocking_notifier(&ethsw->port_switchdevb_nb);
1461         if (err) {
1462                 dev_err(dev, "Failed to register switchdev blocking notifier\n");
1463                 goto err_switchdev_blocking_nb;
1464         }
1465
1466         return 0;
1467
1468 err_switchdev_blocking_nb:
1469         unregister_switchdev_notifier(&ethsw->port_switchdev_nb);
1470 err_switchdev_nb:
1471         unregister_netdevice_notifier(&ethsw->port_nb);
1472         return err;
1473 }
1474
1475 static void ethsw_detect_features(struct ethsw_core *ethsw)
1476 {
1477         ethsw->features = 0;
1478
1479         if (ethsw->major > 8 || (ethsw->major == 8 && ethsw->minor >= 6))
1480                 ethsw->features |= ETHSW_FEATURE_MAC_ADDR;
1481 }
1482
1483 static int ethsw_init(struct fsl_mc_device *sw_dev)
1484 {
1485         struct device *dev = &sw_dev->dev;
1486         struct ethsw_core *ethsw = dev_get_drvdata(dev);
1487         struct dpsw_stp_cfg stp_cfg;
1488         int err;
1489         u16 i;
1490
1491         ethsw->dev_id = sw_dev->obj_desc.id;
1492
1493         err = dpsw_open(ethsw->mc_io, 0, ethsw->dev_id, &ethsw->dpsw_handle);
1494         if (err) {
1495                 dev_err(dev, "dpsw_open err %d\n", err);
1496                 return err;
1497         }
1498
1499         err = dpsw_get_attributes(ethsw->mc_io, 0, ethsw->dpsw_handle,
1500                                   &ethsw->sw_attr);
1501         if (err) {
1502                 dev_err(dev, "dpsw_get_attributes err %d\n", err);
1503                 goto err_close;
1504         }
1505
1506         err = dpsw_get_api_version(ethsw->mc_io, 0,
1507                                    &ethsw->major,
1508                                    &ethsw->minor);
1509         if (err) {
1510                 dev_err(dev, "dpsw_get_api_version err %d\n", err);
1511                 goto err_close;
1512         }
1513
1514         /* Minimum supported DPSW version check */
1515         if (ethsw->major < DPSW_MIN_VER_MAJOR ||
1516             (ethsw->major == DPSW_MIN_VER_MAJOR &&
1517              ethsw->minor < DPSW_MIN_VER_MINOR)) {
1518                 dev_err(dev, "DPSW version %d:%d not supported. Use %d.%d or greater.\n",
1519                         ethsw->major,
1520                         ethsw->minor,
1521                         DPSW_MIN_VER_MAJOR, DPSW_MIN_VER_MINOR);
1522                 err = -ENOTSUPP;
1523                 goto err_close;
1524         }
1525
1526         ethsw_detect_features(ethsw);
1527
1528         err = dpsw_reset(ethsw->mc_io, 0, ethsw->dpsw_handle);
1529         if (err) {
1530                 dev_err(dev, "dpsw_reset err %d\n", err);
1531                 goto err_close;
1532         }
1533
1534         err = dpsw_fdb_set_learning_mode(ethsw->mc_io, 0, ethsw->dpsw_handle, 0,
1535                                          DPSW_FDB_LEARNING_MODE_HW);
1536         if (err) {
1537                 dev_err(dev, "dpsw_fdb_set_learning_mode err %d\n", err);
1538                 goto err_close;
1539         }
1540
1541         stp_cfg.vlan_id = DEFAULT_VLAN_ID;
1542         stp_cfg.state = DPSW_STP_STATE_FORWARDING;
1543
1544         for (i = 0; i < ethsw->sw_attr.num_ifs; i++) {
1545                 err = dpsw_if_set_stp(ethsw->mc_io, 0, ethsw->dpsw_handle, i,
1546                                       &stp_cfg);
1547                 if (err) {
1548                         dev_err(dev, "dpsw_if_set_stp err %d for port %d\n",
1549                                 err, i);
1550                         goto err_close;
1551                 }
1552
1553                 err = dpsw_if_set_broadcast(ethsw->mc_io, 0,
1554                                             ethsw->dpsw_handle, i, 1);
1555                 if (err) {
1556                         dev_err(dev,
1557                                 "dpsw_if_set_broadcast err %d for port %d\n",
1558                                 err, i);
1559                         goto err_close;
1560                 }
1561         }
1562
1563         ethsw->workqueue = alloc_ordered_workqueue("%s_%d_ordered",
1564                                                    WQ_MEM_RECLAIM, "ethsw",
1565                                                    ethsw->sw_attr.id);
1566         if (!ethsw->workqueue) {
1567                 err = -ENOMEM;
1568                 goto err_close;
1569         }
1570
1571         err = ethsw_register_notifier(dev);
1572         if (err)
1573                 goto err_destroy_ordered_workqueue;
1574
1575         return 0;
1576
1577 err_destroy_ordered_workqueue:
1578         destroy_workqueue(ethsw->workqueue);
1579
1580 err_close:
1581         dpsw_close(ethsw->mc_io, 0, ethsw->dpsw_handle);
1582         return err;
1583 }
1584
1585 static int ethsw_port_init(struct ethsw_port_priv *port_priv, u16 port)
1586 {
1587         struct net_device *netdev = port_priv->netdev;
1588         struct ethsw_core *ethsw = port_priv->ethsw_data;
1589         struct dpsw_vlan_if_cfg vcfg;
1590         int err;
1591
1592         /* Switch starts with all ports configured to VLAN 1. Need to
1593          * remove this setting to allow configuration at bridge join
1594          */
1595         vcfg.num_ifs = 1;
1596         vcfg.if_id[0] = port_priv->idx;
1597
1598         err = dpsw_vlan_remove_if_untagged(ethsw->mc_io, 0, ethsw->dpsw_handle,
1599                                            DEFAULT_VLAN_ID, &vcfg);
1600         if (err) {
1601                 netdev_err(netdev, "dpsw_vlan_remove_if_untagged err %d\n",
1602                            err);
1603                 return err;
1604         }
1605
1606         err = ethsw_port_set_pvid(port_priv, 0);
1607         if (err)
1608                 return err;
1609
1610         err = dpsw_vlan_remove_if(ethsw->mc_io, 0, ethsw->dpsw_handle,
1611                                   DEFAULT_VLAN_ID, &vcfg);
1612         if (err)
1613                 netdev_err(netdev, "dpsw_vlan_remove_if err %d\n", err);
1614
1615         return err;
1616 }
1617
1618 static void ethsw_unregister_notifier(struct device *dev)
1619 {
1620         struct ethsw_core *ethsw = dev_get_drvdata(dev);
1621         struct notifier_block *nb;
1622         int err;
1623
1624         nb = &ethsw->port_switchdevb_nb;
1625         err = unregister_switchdev_blocking_notifier(nb);
1626         if (err)
1627                 dev_err(dev,
1628                         "Failed to unregister switchdev blocking notifier (%d)\n",
1629                         err);
1630
1631         err = unregister_switchdev_notifier(&ethsw->port_switchdev_nb);
1632         if (err)
1633                 dev_err(dev,
1634                         "Failed to unregister switchdev notifier (%d)\n", err);
1635
1636         err = unregister_netdevice_notifier(&ethsw->port_nb);
1637         if (err)
1638                 dev_err(dev,
1639                         "Failed to unregister netdev notifier (%d)\n", err);
1640 }
1641
1642 static void ethsw_takedown(struct fsl_mc_device *sw_dev)
1643 {
1644         struct device *dev = &sw_dev->dev;
1645         struct ethsw_core *ethsw = dev_get_drvdata(dev);
1646         int err;
1647
1648         ethsw_unregister_notifier(dev);
1649
1650         err = dpsw_close(ethsw->mc_io, 0, ethsw->dpsw_handle);
1651         if (err)
1652                 dev_warn(dev, "dpsw_close err %d\n", err);
1653 }
1654
1655 static int ethsw_remove(struct fsl_mc_device *sw_dev)
1656 {
1657         struct ethsw_port_priv *port_priv;
1658         struct ethsw_core *ethsw;
1659         struct device *dev;
1660         int i;
1661
1662         dev = &sw_dev->dev;
1663         ethsw = dev_get_drvdata(dev);
1664
1665         ethsw_teardown_irqs(sw_dev);
1666
1667         dpsw_disable(ethsw->mc_io, 0, ethsw->dpsw_handle);
1668
1669         for (i = 0; i < ethsw->sw_attr.num_ifs; i++) {
1670                 port_priv = ethsw->ports[i];
1671                 unregister_netdev(port_priv->netdev);
1672                 free_netdev(port_priv->netdev);
1673         }
1674         kfree(ethsw->ports);
1675
1676         ethsw_takedown(sw_dev);
1677
1678         destroy_workqueue(ethsw->workqueue);
1679
1680         fsl_mc_portal_free(ethsw->mc_io);
1681
1682         kfree(ethsw);
1683
1684         dev_set_drvdata(dev, NULL);
1685
1686         return 0;
1687 }
1688
1689 static int ethsw_probe_port(struct ethsw_core *ethsw, u16 port_idx)
1690 {
1691         struct ethsw_port_priv *port_priv;
1692         struct device *dev = ethsw->dev;
1693         struct net_device *port_netdev;
1694         int err;
1695
1696         port_netdev = alloc_etherdev(sizeof(struct ethsw_port_priv));
1697         if (!port_netdev) {
1698                 dev_err(dev, "alloc_etherdev error\n");
1699                 return -ENOMEM;
1700         }
1701
1702         port_priv = netdev_priv(port_netdev);
1703         port_priv->netdev = port_netdev;
1704         port_priv->ethsw_data = ethsw;
1705
1706         port_priv->idx = port_idx;
1707         port_priv->stp_state = BR_STATE_FORWARDING;
1708
1709         /* Flooding is implicitly enabled */
1710         port_priv->flood = true;
1711
1712         SET_NETDEV_DEV(port_netdev, dev);
1713         port_netdev->netdev_ops = &ethsw_port_ops;
1714         port_netdev->ethtool_ops = &dpaa2_switch_port_ethtool_ops;
1715
1716         /* Set MTU limits */
1717         port_netdev->min_mtu = ETH_MIN_MTU;
1718         port_netdev->max_mtu = ETHSW_MAX_FRAME_LENGTH;
1719
1720         err = ethsw_port_init(port_priv, port_idx);
1721         if (err)
1722                 goto err_port_probe;
1723
1724         err = ethsw_port_set_mac_addr(port_priv);
1725         if (err)
1726                 goto err_port_probe;
1727
1728         err = register_netdev(port_netdev);
1729         if (err < 0) {
1730                 dev_err(dev, "register_netdev error %d\n", err);
1731                 goto err_port_probe;
1732         }
1733
1734         ethsw->ports[port_idx] = port_priv;
1735
1736         return 0;
1737
1738 err_port_probe:
1739         free_netdev(port_netdev);
1740
1741         return err;
1742 }
1743
1744 static int ethsw_probe(struct fsl_mc_device *sw_dev)
1745 {
1746         struct device *dev = &sw_dev->dev;
1747         struct ethsw_core *ethsw;
1748         int i, err;
1749
1750         /* Allocate switch core*/
1751         ethsw = kzalloc(sizeof(*ethsw), GFP_KERNEL);
1752
1753         if (!ethsw)
1754                 return -ENOMEM;
1755
1756         ethsw->dev = dev;
1757         dev_set_drvdata(dev, ethsw);
1758
1759         err = fsl_mc_portal_allocate(sw_dev, FSL_MC_IO_ATOMIC_CONTEXT_PORTAL,
1760                                      &ethsw->mc_io);
1761         if (err) {
1762                 if (err == -ENXIO)
1763                         err = -EPROBE_DEFER;
1764                 else
1765                         dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
1766                 goto err_free_drvdata;
1767         }
1768
1769         err = ethsw_init(sw_dev);
1770         if (err)
1771                 goto err_free_cmdport;
1772
1773         /* DEFAULT_VLAN_ID is implicitly configured on the switch */
1774         ethsw->vlans[DEFAULT_VLAN_ID] = ETHSW_VLAN_MEMBER;
1775
1776         /* Learning is implicitly enabled */
1777         ethsw->learning = true;
1778
1779         ethsw->ports = kcalloc(ethsw->sw_attr.num_ifs, sizeof(*ethsw->ports),
1780                                GFP_KERNEL);
1781         if (!(ethsw->ports)) {
1782                 err = -ENOMEM;
1783                 goto err_takedown;
1784         }
1785
1786         for (i = 0; i < ethsw->sw_attr.num_ifs; i++) {
1787                 err = ethsw_probe_port(ethsw, i);
1788                 if (err)
1789                         goto err_free_ports;
1790         }
1791
1792         err = dpsw_enable(ethsw->mc_io, 0, ethsw->dpsw_handle);
1793         if (err) {
1794                 dev_err(ethsw->dev, "dpsw_enable err %d\n", err);
1795                 goto err_free_ports;
1796         }
1797
1798         /* Make sure the switch ports are disabled at probe time */
1799         for (i = 0; i < ethsw->sw_attr.num_ifs; i++)
1800                 dpsw_if_disable(ethsw->mc_io, 0, ethsw->dpsw_handle, i);
1801
1802         /* Setup IRQs */
1803         err = ethsw_setup_irqs(sw_dev);
1804         if (err)
1805                 goto err_stop;
1806
1807         dev_info(dev, "probed %d port switch\n", ethsw->sw_attr.num_ifs);
1808         return 0;
1809
1810 err_stop:
1811         dpsw_disable(ethsw->mc_io, 0, ethsw->dpsw_handle);
1812
1813 err_free_ports:
1814         /* Cleanup registered ports only */
1815         for (i--; i >= 0; i--) {
1816                 unregister_netdev(ethsw->ports[i]->netdev);
1817                 free_netdev(ethsw->ports[i]->netdev);
1818         }
1819         kfree(ethsw->ports);
1820
1821 err_takedown:
1822         ethsw_takedown(sw_dev);
1823
1824 err_free_cmdport:
1825         fsl_mc_portal_free(ethsw->mc_io);
1826
1827 err_free_drvdata:
1828         kfree(ethsw);
1829         dev_set_drvdata(dev, NULL);
1830
1831         return err;
1832 }
1833
1834 static const struct fsl_mc_device_id ethsw_match_id_table[] = {
1835         {
1836                 .vendor = FSL_MC_VENDOR_FREESCALE,
1837                 .obj_type = "dpsw",
1838         },
1839         { .vendor = 0x0 }
1840 };
1841 MODULE_DEVICE_TABLE(fslmc, ethsw_match_id_table);
1842
1843 static struct fsl_mc_driver eth_sw_drv = {
1844         .driver = {
1845                 .name = KBUILD_MODNAME,
1846                 .owner = THIS_MODULE,
1847         },
1848         .probe = ethsw_probe,
1849         .remove = ethsw_remove,
1850         .match_id_table = ethsw_match_id_table
1851 };
1852
1853 module_fsl_mc_driver(eth_sw_drv);
1854
1855 MODULE_LICENSE("GPL v2");
1856 MODULE_DESCRIPTION("DPAA2 Ethernet Switch Driver");